diff --git a/.aws/mirror-buildspec.yml b/.aws/mirror-buildspec.yml deleted file mode 100644 index a537f5bfd994..000000000000 --- a/.aws/mirror-buildspec.yml +++ /dev/null @@ -1,15 +0,0 @@ -version: 0.2 - -phases: - install: - runtime-versions: - nodejs: 14 - pre_build: - commands: - - git config --global user.name "Isaac LAB CI Bot" - - git config --global user.email "isaac-lab-ci-bot@nvidia.com" - build: - commands: - - git remote set-url origin https://github.com/${TARGET_REPO}.git - - git checkout $SOURCE_BRANCH - - git push --force https://$GITHUB_TOKEN@github.com/${TARGET_REPO}.git $SOURCE_BRANCH:$TARGET_BRANCH diff --git a/.aws/postmerge-ci-buildspec.yml b/.aws/postmerge-ci-buildspec.yml deleted file mode 100644 index d58a4d043c5a..000000000000 --- a/.aws/postmerge-ci-buildspec.yml +++ /dev/null @@ -1,49 +0,0 @@ -version: 0.2 - -phases: - build: - commands: - - echo "Building and pushing Docker image" - - | - # Determine branch name or use fallback - if [ -n "$CODEBUILD_WEBHOOK_HEAD_REF" ]; then - BRANCH_NAME=$(echo $CODEBUILD_WEBHOOK_HEAD_REF | sed 's/refs\/heads\///') - elif [ -n "$CODEBUILD_SOURCE_VERSION" ]; then - BRANCH_NAME=$CODEBUILD_SOURCE_VERSION - else - BRANCH_NAME="unknown" - fi - - # Replace '/' with '-' and remove any invalid characters for Docker tag - SAFE_BRANCH_NAME=$(echo $BRANCH_NAME | sed 's/[^a-zA-Z0-9._-]/-/g') - - # Use "latest" if branch name is empty or only contains invalid characters - if [ -z "$SAFE_BRANCH_NAME" ]; then - SAFE_BRANCH_NAME="latest" - fi - - # Get the git repository short name - REPO_SHORT_NAME=$(basename -s .git `git config --get remote.origin.url`) - if [ -z "$REPO_SHORT_NAME" ]; then - REPO_SHORT_NAME="verification" - fi - - # Parse the env variable string into an array - mapfile -d ' ' -t IMAGE_BASE_VERSIONS <<< "$ISAACSIM_BASE_VERSIONS_STRING" - for IMAGE_BASE_VERSION in "${IMAGE_BASE_VERSIONS[@]}"; do - IMAGE_BASE_VERSION=$(echo "$IMAGE_BASE_VERSION" | tr -d '[:space:]') - # Combine repo short name and branch name for the tag - COMBINED_TAG="${REPO_SHORT_NAME}-${SAFE_BRANCH_NAME}-${IMAGE_BASE_VERSION}" - - docker login -u \$oauthtoken -p $NGC_TOKEN nvcr.io - docker build -t $IMAGE_NAME:$COMBINED_TAG \ - --build-arg ISAACSIM_BASE_IMAGE_ARG=$ISAACSIM_BASE_IMAGE \ - --build-arg ISAACSIM_VERSION_ARG=$IMAGE_BASE_VERSION \ - --build-arg ISAACSIM_ROOT_PATH_ARG=/isaac-sim \ - --build-arg ISAACLAB_PATH_ARG=/workspace/isaaclab \ - --build-arg DOCKER_USER_HOME_ARG=/root \ - -f docker/Dockerfile.base . - docker push $IMAGE_NAME:$COMBINED_TAG - docker tag $IMAGE_NAME:$COMBINED_TAG $IMAGE_NAME:$COMBINED_TAG-b$CODEBUILD_BUILD_NUMBER - docker push $IMAGE_NAME:$COMBINED_TAG-b$CODEBUILD_BUILD_NUMBER - done diff --git a/.aws/premerge-ci-buildspec.yml b/.aws/premerge-ci-buildspec.yml deleted file mode 100644 index 6bbfe081cd4b..000000000000 --- a/.aws/premerge-ci-buildspec.yml +++ /dev/null @@ -1,83 +0,0 @@ -version: 0.2 - -phases: - pre_build: - commands: - - echo "Launching EC2 instance to run tests" - - | - INSTANCE_ID=$(aws ec2 run-instances \ - --image-id ami-0e6cc441f9f4caab3 \ - --count 1 \ - --instance-type g5.2xlarge \ - --key-name production/ssh/isaaclab \ - --security-group-ids sg-02617e4b8916794c4 \ - --subnet-id subnet-0907ceaeb40fd9eac \ - --block-device-mappings '[{"DeviceName":"/dev/sda1","Ebs":{"VolumeSize":500}}]' \ - --output text \ - --query 'Instances[0].InstanceId') - - aws ec2 wait instance-running --instance-ids $INSTANCE_ID - - | - EC2_INSTANCE_IP=$(aws ec2 describe-instances \ - --filters "Name=instance-state-name,Values=running" "Name=instance-id,Values=$INSTANCE_ID" \ - --query 'Reservations[*].Instances[*].[PrivateIpAddress]' \ - --output text) - - mkdir -p ~/.ssh - - | - aws ec2 describe-key-pairs --include-public-key --key-name production/ssh/isaaclab \ - --query 'KeyPairs[0].PublicKey' --output text > ~/.ssh/id_rsa.pub - - | - aws secretsmanager get-secret-value --secret-id production/ssh/isaaclab \ - --query SecretString --output text > ~/.ssh/id_rsa - - chmod 400 ~/.ssh/id_* - - echo "Host $EC2_INSTANCE_IP\n\tStrictHostKeyChecking no\n" >> ~/.ssh/config - - | - aws ec2-instance-connect send-ssh-public-key \ - --instance-id $INSTANCE_ID \ - --availability-zone us-west-2a \ - --ssh-public-key file://~/.ssh/id_rsa.pub \ - --instance-os-user ubuntu - build: - commands: - - echo "Running tests on EC2 instance" - - SRC_DIR=$(basename $CODEBUILD_SRC_DIR) - - cd .. - - | - bash -c ' - function retry_scp() { - local retries=5 - local wait_time=30 - local count=0 - while [ $count -lt $retries ]; do - sleep $wait_time - scp -r $SRC_DIR ubuntu@$EC2_INSTANCE_IP:~ - if [ $? -eq 0 ]; then - echo "SCP command succeeded" - return 0 - fi - count=$((count + 1)) - echo "SCP command failed. Retrying in $wait_time seconds..." - done - echo "SCP command failed after $retries attempts." - return 1 - } - retry_scp - ' - - ssh ubuntu@$EC2_INSTANCE_IP "docker login -u \\\$oauthtoken -p $NGC_TOKEN nvcr.io" - - | - ssh ubuntu@$EC2_INSTANCE_IP " - cd $SRC_DIR - DOCKER_BUILDKIT=1 docker build -t isaac-lab-dev \ - --build-arg ISAACSIM_BASE_IMAGE_ARG=$ISAACSIM_BASE_IMAGE \ - --build-arg ISAACSIM_VERSION_ARG=$ISAACSIM_BASE_VERSION \ - --build-arg ISAACSIM_ROOT_PATH_ARG=/isaac-sim \ - --build-arg ISAACLAB_PATH_ARG=/workspace/isaaclab \ - --build-arg DOCKER_USER_HOME_ARG=/root \ - -f docker/Dockerfile.base . - docker run --rm --entrypoint bash --gpus all --network=host \ - --name isaac-lab-test isaac-lab-dev ./isaaclab.sh -t && exit \$? - " - - post_build: - commands: - - echo "Terminating EC2 instance" - - aws ec2 terminate-instances --instance-ids $INSTANCE_ID diff --git a/.dockerignore b/.dockerignore index 6f6867127cff..b6687c95fa7a 100644 --- a/.dockerignore +++ b/.dockerignore @@ -4,12 +4,15 @@ .gitignore # ignore docs docs/ +# copy in licenses folder to the container +!docs/licenses/ # ignore logs **/logs/ **/runs/ **/output/* **/outputs/* **/videos/* +**/wandb/* *.tmp # ignore docker docker/cluster/exports/ @@ -21,3 +24,7 @@ recordings/ **/*.egg-info/ # ignore isaac sim symlink _isaac_sim? +# Docker history +docker/.isaac-lab-docker-history +# ignore uv environment +env_isaaclab diff --git a/.flake8 b/.flake8 deleted file mode 100644 index 9b4a023d685b..000000000000 --- a/.flake8 +++ /dev/null @@ -1,23 +0,0 @@ -[flake8] -show-source=True -statistics=True -per-file-ignores=*/__init__.py:F401 -# E402: Module level import not at top of file -# E501: Line too long -# W503: Line break before binary operator -# E203: Whitespace before ':' -> conflicts with black -# D401: First line should be in imperative mood -# R504: Unnecessary variable assignment before return statement. -# R505: Unnecessary elif after return statement -# SIM102: Use a single if-statement instead of nested if-statements -# SIM117: Merge with statements for context managers that have same scope. -# SIM118: Checks for key-existence checks against dict.keys() calls. -ignore=E402,E501,W503,E203,D401,R504,R505,SIM102,SIM117,SIM118 -max-line-length = 120 -max-complexity = 30 -exclude=_*,.vscode,.git,docs/** -# docstrings -docstring-convention=google -# annotations -suppress-none-returning=True -allow-star-arg-any=True diff --git a/.gitattributes b/.gitattributes index 65a6b946ab24..e3c0ead689d1 100644 --- a/.gitattributes +++ b/.gitattributes @@ -10,3 +10,5 @@ *.pt filter=lfs diff=lfs merge=lfs -text *.jit filter=lfs diff=lfs merge=lfs -text *.hdf5 filter=lfs diff=lfs merge=lfs -text + +*.bat text eol=crlf diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f21bd729c898..cdee41345d25 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -16,62 +16,86 @@ # App experience files # These are the files that are used to launch the app with the correct settings and configurations -/apps/ @kellyguo11 @hhansen-bdai @Mayankm96 @Dhoeller19 +/apps/ @kellyguo11 @hhansen-bdai @Mayankm96 # Core Framework -/source/isaaclab/ @Dhoeller19 @Mayankm96 @jsmith-bdai @kellyguo11 -/source/isaaclab/isaaclab/actuators @Dhoeller19 @Mayankm96 @nikitardn @jtigue-bdai +/source/isaaclab/isaaclab/actuators @Mayankm96 @jtigue-bdai /source/isaaclab/isaaclab/app @hhansen-bdai @kellyguo11 -/source/isaaclab/isaaclab/assets @jsmith-bdai @Dhoeller19 @kellyguo11 @Mayankm96 @jtigue-bdai -/source/isaaclab/isaaclab/assets/deformable_object @kellyguo11 @Mayankm96 @masoudmoghani +/source/isaaclab/isaaclab/assets @kellyguo11 @Mayankm96 @jtigue-bdai +/source/isaaclab/isaaclab/assets/deformable_object @masoudmoghani @ooctipus /source/isaaclab/isaaclab/controllers @Mayankm96 -/source/isaaclab/isaaclab/envs @jsmith-bdai @Dhoeller19 @kellyguo11 @Mayankm96 -/source/isaaclab/isaaclab/envs/manager_based_* @jsmith-bdai @Dhoeller19 @Mayankm96 +/source/isaaclab/isaaclab/envs/manager_based_* @Mayankm96 @jtigue-bdai @ooctipus /source/isaaclab/isaaclab/envs/direct_* @kellyguo11 -/source/isaaclab/isaaclab/managers @jsmith-bdai @Dhoeller19 @Mayankm96 -/source/isaaclab/isaaclab/sensors @jsmith-bdai @Dhoeller19 @pascal-roth @Mayankm96 @jtigue-bdai +/source/isaaclab/isaaclab/envs/mdp @ooctipus +/source/isaaclab/isaaclab/envs/mimic_* @peterd-NV +/source/isaaclab/isaaclab/envs/ui @ooctipus @ossamaAhmed +/source/isaaclab/isaaclab/envs/utils @Toni-SM +/source/isaaclab/isaaclab/managers @jtigue-bdai @Mayankm96 @ooctipus +/source/isaaclab/isaaclab/sensors/sensor_base* @pascal-roth /source/isaaclab/isaaclab/sensors/camera @kellyguo11 @pascal-roth -/source/isaaclab/isaaclab/sensors/contact_sensor @jtigue-bdai -/source/isaaclab/isaaclab/sensors/frame_transformer @jsmith-bdai -/source/isaaclab/isaaclab/sensors/ray_caster @pascal-roth @Dhoeller19 -/source/isaaclab/isaaclab/sim @Mayankm96 @jsmith-bdai -/source/isaaclab/isaaclab/sim/simulation_context.py @Dhoeller19 @kellyguo11 -/source/isaaclab/isaaclab/terrains @Dhoeller19 @Mayankm96 @nikitardn -/source/isaaclab/isaaclab/utils @Mayankm96 @jsmith-bdai -/source/isaaclab/isaaclab/utils/modifiers @jtigue-bdai +/source/isaaclab/isaaclab/sensors/contact_sensor @jtigue-bdai @ooctipus +/source/isaaclab/isaaclab/sensors/imu @jtigue-bdai @pascal-roth +/source/isaaclab/isaaclab/sensors/ray_caster @pascal-roth +/source/isaaclab/isaaclab/sensors/frame_transformer @jtigue-bdai +/source/isaaclab/isaaclab/sim/converters @Mayankm96 @jtigue-bdai @kellyguo11 +/source/isaaclab/isaaclab/sim/schemas @Mayankm96 @jtigue-bdai @kellyguo11 +/source/isaaclab/isaaclab/sim/spawners @Mayankm96 @jtigue-bdai @ooctipus +/source/isaaclab/isaaclab/sim/simulation_* @matthewtrepte @ossamaAhmed @kellyguo11 +/source/isaaclab/isaaclab/terrains @Mayankm96 +/source/isaaclab/isaaclab/ui @pascal-roth @jtigue-bdai +/source/isaaclab/isaaclab/utils/buffers @ooctipus @jtigue-bdai +/source/isaaclab/isaaclab/utils/datasets @Peter-NV /source/isaaclab/isaaclab/utils/interpolation @jtigue-bdai +/source/isaaclab/isaaclab/utils/io @ooctipus +/source/isaaclab/isaaclab/utils/modifiers @jtigue-bdai /source/isaaclab/isaaclab/utils/noise @jtigue-bdai @kellyguo11 -/source/isaaclab/isaaclab/utils/warp @Dhoeller19 @pascal-roth -/source/isaaclab/isaaclab/utils/assets.py @Dhoeller19 @kellyguo11 @Mayankm96 -/source/isaaclab/isaaclab/utils/math.py @jsmith-bdai @Dhoeller19 @Mayankm96 -/source/isaaclab/isaaclab/utils/configclass.py @Mayankm96 @Dhoeller19 +/source/isaaclab/isaaclab/utils/warp @pascal-roth +/source/isaaclab/isaaclab/utils/assets.py @kellyguo11 @Mayankm96 +/source/isaaclab/isaaclab/utils/math.py @jtigue-bdai @Mayankm96 +/source/isaaclab/isaaclab/utils/configclass.py @Mayankm96 +/source/isaaclab/isaaclab/utils/sensors.py @kellyguo11 @pascal-roth # RL Environment -/source/isaaclab_tasks/ @Dhoeller19 @Mayankm96 @jsmith-bdai @kellyguo11 -/source/isaaclab_tasks/isaaclab_tasks/direct @Dhoeller19 @kellyguo11 -/source/isaaclab_tasks/isaaclab_tasks/manager_based @Dhoeller19 @Mayankm96 @jsmith-bdai @jtigue-bdai +/source/isaaclab_tasks/isaaclab_tasks/direct @kellyguo11 +/source/isaaclab_tasks/isaaclab_tasks/manager_based @Mayankm96 +/source/isaaclab_tasks/isaaclab_tasks/utils @Mayankm96 # Assets -/source/isaaclab_assets/isaaclab_assets/ @Dhoeller19 @pascal-roth @jsmith-bdai +/source/isaaclab_assets/isaaclab_assets/ @pascal-roth + +# Mimic +/source/isaaclab_mimic/isaaclab_mimic @peterd-NV +/source/isaaclab_mimic/isaaclab_mimic @njawale42 +/source/isaaclab_mimic/isaaclab_mimic @michaellin6 +/source/isaaclab_mimic/isaaclab_mimic @jaybdub +/source/isaaclab_mimic/isaaclab_mimic @huihuaNvidia2023 +/source/isaaclab_mimic/isaaclab_mimic @xyao-nv + +# RL +/source/isaaclab_rl/isaaclab_rl/rsl_rl @Mayankm96 @ClemensSchwarke +/source/isaaclab_rl/isaaclab_rl/rl_games @Toni-SM +/source/isaaclab_rl/isaaclab_rl/sb3 @Toni-SM +/source/isaaclab_rl/isaaclab_rl/skrl @Toni-SM # Standalone Scripts -/scripts/demos/ @jsmith-bdai @jtigue-bdai @Dhoeller19 @kellyguo11 @Mayankm96 -/scripts/environments/ @Mayankm96 -/scripts/tools/ @jsmith-bdai @Mayankm96 -/scripts/tutorials/ @jsmith-bdai @pascal-roth @kellyguo11 @Dhoeller19 @Mayankm96 -/scripts/reinforcement_learning/ @jsmith-bdai @Dhoeller19 @kellyguo11 @Mayankm96 -/scripts/imitation_learning/ @jsmith-bdai @Dhoeller19 @kellyguo11 @Mayankm96 +/scripts/benchmarks/ @ooctipus @kellyguo11 +/scripts/demos/ @ooctipus +/scripts/environments/ @ooctipus +/scripts/imitation_learning/ @Peter-NV +/scripts/reinforcement_learning/ @ooctipus @Toni-NV +/scripts/tools/ @jtigue-bdai @Mayankm96 +/scripts/tutorials/ @jtigue-bdai @pascal-roth # Github Actions # This list is for people wanting to be notified every time there's a change # related to Github Actions -/.github/ @kellyguo11 @jsmith-bdai +/.github/ @kellyguo11 @hhansen-bdai # Visual Studio Code /.vscode/ @hhansen-bdai @Mayankm96 # Infrastructure (Docker, Docs, Tools) /docker/ @hhansen-bdai @pascal-roth -/docs/ @jsmith-bdai @Dhoeller19 @kellyguo11 @Mayankm96 -/tools/ @hhansen-bdai @jsmith-bdai @Dhoeller19 -/isaaclab.* @hhansen-bdai @Dhoeller19 @Mayankm96 @kellyguo11 +/docs/ @jtigue-bdai @kellyguo11 @Mayankm96 +/tools/ @hhansen-bdai +/isaaclab.* @hhansen-bdai @Mayankm96 @kellyguo11 diff --git a/.github/ISSUE_TEMPLATE/bug.md b/.github/ISSUE_TEMPLATE/bug.md index 1ea755acc6c5..54d6f21a8086 100644 --- a/.github/ISSUE_TEMPLATE/bug.md +++ b/.github/ISSUE_TEMPLATE/bug.md @@ -31,11 +31,11 @@ Describe the characteristic of your environment: - Commit: [e.g. 8f3b9ca] -- Isaac Sim Version: [e.g. 2022.2.0, this can be obtained by `cat ${ISAACSIM_PATH}/VERSION`] -- OS: [e.g. Ubuntu 20.04] -- GPU: [e.g. RTX 2060 Super] -- CUDA: [e.g. 11.4] -- GPU Driver: [e.g. 470.82.01, this can be seen by using `nvidia-smi` command.] +- Isaac Sim Version: [e.g. 5.0, this can be obtained by `cat ${ISAACSIM_PATH}/VERSION`] +- OS: [e.g. Ubuntu 22.04] +- GPU: [e.g. RTX 5090] +- CUDA: [e.g. 12.8] +- GPU Driver: [e.g. 553.05, this can be seen by using `nvidia-smi` command.] ### Additional context diff --git a/.github/ISSUE_TEMPLATE/proposal.md b/.github/ISSUE_TEMPLATE/proposal.md index 9946d02165da..c07d7f56dc85 100644 --- a/.github/ISSUE_TEMPLATE/proposal.md +++ b/.github/ISSUE_TEMPLATE/proposal.md @@ -21,6 +21,14 @@ If this is related to another GitHub issue, please link here too. A clear and concise description of any alternative solutions or features you've considered, if any. +### Build Info + +Describe the versions where you are observing the missing feature in: + + +- Isaac Lab Version: [e.g. 2.3.2] +- Isaac Sim Version: [e.g. 5.1, this can be obtained by `cat ${ISAACSIM_PATH}/VERSION`] + ### Additional context Add any other context or screenshots about the feature request here. diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md index b6e18b7fd54f..6e0582f37379 100644 --- a/.github/ISSUE_TEMPLATE/question.md +++ b/.github/ISSUE_TEMPLATE/question.md @@ -10,4 +10,12 @@ Basic questions, related to robot learning, that are not bugs or feature request Advanced/nontrivial questions, especially in areas where documentation is lacking, are very much welcome. -For questions that are related to running and understanding Isaac Sim, please post them at the official [Isaac Sim forums](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/isaac_sim_forums.html). +For questions that are related to running and understanding Isaac Sim, please post them at the official [Isaac Sim forums](https://forums.developer.nvidia.com/c/omniverse/simulation/69). + +### Build Info + +Describe the versions that you are currently using: + + +- Isaac Lab Version: [e.g. 2.3.2] +- Isaac Sim Version: [e.g. 5.1, this can be obtained by `cat ${ISAACSIM_PATH}/VERSION`] diff --git a/.github/LICENSE_HEADER.txt b/.github/LICENSE_HEADER.txt index e9b115861a5f..f078a3a4e8a7 100644 --- a/.github/LICENSE_HEADER.txt +++ b/.github/LICENSE_HEADER.txt @@ -1,4 +1,4 @@ -Copyright (c) 2022-2025, The Isaac Lab Project Developers. +Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). All rights reserved. SPDX-License-Identifier: BSD-3-Clause diff --git a/.github/LICENSE_HEADER_MIMIC.txt b/.github/LICENSE_HEADER_MIMIC.txt index 6196e791dc7c..d5779ee27290 100644 --- a/.github/LICENSE_HEADER_MIMIC.txt +++ b/.github/LICENSE_HEADER_MIMIC.txt @@ -1,4 +1,4 @@ -Copyright (c) 2024-2025, The Isaac Lab Project Developers. +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 diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index e9176cc47f5b..ee9fa4ebdc5e 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -4,6 +4,8 @@ Thank you for your interest in sending a pull request. Please make sure to check the contribution guidelines. Link: https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html + +๐Ÿ’ก Please try to keep PRs small and focused. Large PRs are harder to review and merge. --> Please include a summary of the change and which issue is fixed. Please also include relevant motivation and context. @@ -21,8 +23,8 @@ is demanded by more than one party. --> - Bug fix (non-breaking change which fixes an issue) - New feature (non-breaking change which adds functionality) -- Breaking change (fix or feature that would cause existing functionality to not work as expected) -- This change requires a documentation update +- Breaking change (existing functionality will not work without user modification) +- Documentation update ## Screenshots @@ -40,6 +42,7 @@ To upload images to a PR -- simply drag and drop an image while in edit mode and ## Checklist +- [ ] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [ ] I have made corresponding changes to the documentation - [ ] My changes generate no new warnings diff --git a/.github/actions/combine-results/action.yml b/.github/actions/combine-results/action.yml new file mode 100644 index 000000000000..8ed66e3b4603 --- /dev/null +++ b/.github/actions/combine-results/action.yml @@ -0,0 +1,103 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: 'Combine XML Test Results' +description: 'Combines multiple XML test result files into a single file' + +inputs: + tests-dir: + description: 'Directory containing test result files' + default: 'tests' + required: false + output-file: + description: 'Output combined XML file path' + required: true + reports-dir: + description: 'Directory to store the combined results' + default: 'reports' + required: false + +runs: + using: composite + steps: + - name: Combine XML Test Results + shell: sh + run: | + # Function to combine multiple XML test results + combine_xml_results() { + local tests_dir="$1" + local output_file="$2" + local reports_dir="$3" + + echo "Combining test results from: $tests_dir" + echo "Output file: $output_file" + echo "Reports directory: $reports_dir" + + # Check if reports directory exists + if [ ! -d "$reports_dir" ]; then + echo "โš ๏ธ Reports directory does not exist: $reports_dir" + mkdir -p "$reports_dir" + fi + + # Check if tests directory exists + if [ ! -d "$tests_dir" ]; then + echo "โš ๏ธ Tests directory does not exist: $tests_dir" + echo "Creating fallback XML..." + echo 'Tests directory was not found' > "$output_file" + return + fi + + # Find all XML files in the tests directory + echo "Searching for XML files in: $tests_dir" + xml_files=$(find "$tests_dir" -name "*.xml" -type f 2>/dev/null | sort) + + if [ -z "$xml_files" ]; then + echo "โš ๏ธ No XML files found in: $tests_dir" + echo "Creating fallback XML..." + echo 'No XML test result files were found' > "$output_file" + return + fi + + # Count XML files found + file_count=$(echo "$xml_files" | wc -l) + echo "โœ… Found $file_count XML file(s):" + echo "$xml_files" | while read -r file; do + echo " - $file ($(wc -c < "$file") bytes)" + done + + # Create combined XML + echo "๐Ÿ”„ Combining $file_count XML files..." + echo '' > "$output_file" + echo '' >> "$output_file" + + # Process each XML file + combined_count=0 + echo "$xml_files" | while read -r file; do + if [ -f "$file" ]; then + echo " Processing: $file" + # Remove XML declaration and outer testsuites wrapper from each file + # Remove first line (XML declaration) and strip outer / tags + sed '1d; s/^//; s/<\/testsuites>$//' "$file" >> "$output_file" 2>/dev/null || { + echo " โš ๏ธ Warning: Could not process $file, skipping..." + } + combined_count=$((combined_count + 1)) + fi + done + + echo '' >> "$output_file" + echo "โœ… Successfully combined $combined_count files into: $output_file" + + # Verify output file was created + if [ -f "$output_file" ]; then + echo "โœ… Final output file created: $output_file" + echo "๐Ÿ“Š Output file size: $(wc -c < "$output_file") bytes" + else + echo "โŒ Failed to create output file: $output_file" + exit 1 + fi + } + + # Call the function with provided parameters + combine_xml_results "${{ inputs.tests-dir }}" "${{ inputs.output-file }}" "${{ inputs.reports-dir }}" diff --git a/.github/actions/docker-build/action.yml b/.github/actions/docker-build/action.yml new file mode 100644 index 000000000000..2db402d42042 --- /dev/null +++ b/.github/actions/docker-build/action.yml @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: 'Build Docker Image' +description: 'Builds a Docker image with IsaacSim and IsaacLab dependencies' + +inputs: + image-tag: + description: 'Docker image tag to use' + required: true + isaacsim-base-image: + description: 'IsaacSim base image' + required: true + isaacsim-version: + description: 'IsaacSim version' + required: true + dockerfile-path: + description: 'Path to Dockerfile' + default: 'docker/Dockerfile.curobo' + required: false + context-path: + description: 'Build context path' + default: '.' + required: false + +runs: + using: composite + steps: + - name: NGC Login + shell: sh + run: | + # Only attempt NGC login if API key is available + if [ -n "${{ env.NGC_API_KEY }}" ]; then + echo "Logging into NGC registry..." + docker login -u \$oauthtoken -p ${{ env.NGC_API_KEY }} nvcr.io + echo "โœ… Successfully logged into NGC registry" + else + echo "โš ๏ธ NGC_API_KEY not available - skipping NGC login" + echo "This is normal for PRs from forks or when secrets are not configured" + fi + + - name: Build Docker Image + shell: sh + run: | + # Function to build Docker image + build_docker_image() { + local image_tag="$1" + local isaacsim_base_image="$2" + local isaacsim_version="$3" + local dockerfile_path="$4" + local context_path="$5" + + echo "Building Docker image: $image_tag" + echo "Using Dockerfile: $dockerfile_path" + echo "Build context: $context_path" + + # Build Docker image + docker buildx build --progress=plain --platform linux/amd64 \ + -t isaac-lab-dev \ + -t $image_tag \ + --build-arg ISAACSIM_BASE_IMAGE_ARG="$isaacsim_base_image" \ + --build-arg ISAACSIM_VERSION_ARG="$isaacsim_version" \ + --build-arg ISAACSIM_ROOT_PATH_ARG=/isaac-sim \ + --build-arg ISAACLAB_PATH_ARG=/workspace/isaaclab \ + --build-arg DOCKER_USER_HOME_ARG=/root \ + --cache-from type=gha \ + --cache-to type=gha,mode=max \ + -f $dockerfile_path \ + --load $context_path + + echo "โœ… Docker image built successfully: $image_tag" + docker images | grep isaac-lab-dev + } + + # Call the function with provided parameters + build_docker_image "${{ inputs.image-tag }}" "${{ inputs.isaacsim-base-image }}" "${{ inputs.isaacsim-version }}" "${{ inputs.dockerfile-path }}" "${{ inputs.context-path }}" diff --git a/.github/actions/run-tests/action.yml b/.github/actions/run-tests/action.yml new file mode 100644 index 000000000000..467122860141 --- /dev/null +++ b/.github/actions/run-tests/action.yml @@ -0,0 +1,157 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: 'Run Tests in Docker Container' +description: 'Runs pytest tests in a Docker container with GPU support and result collection' + +inputs: + test-path: + description: 'Path to test directory or pytest arguments' + required: true + result-file: + description: 'Name of the result XML file' + required: true + container-name: + description: 'Name for the Docker container' + required: true + image-tag: + description: 'Docker image tag to use' + required: true + reports-dir: + description: 'Directory to store test results' + default: 'reports' + required: false + pytest-options: + description: 'Additional pytest options (e.g., -k filter)' + default: '' + required: false + filter-pattern: + description: 'Pattern to filter test files (e.g., isaaclab_tasks)' + default: '' + required: false + +runs: + using: composite + steps: + - name: Run Tests in Docker Container + shell: bash + run: | + # Function to run tests in Docker container + run_tests() { + local test_path="$1" + local result_file="$2" + local container_name="$3" + local image_tag="$4" + local reports_dir="$5" + local pytest_options="$6" + local filter_pattern="$7" + + echo "Running tests in: $test_path" + if [ -n "$pytest_options" ]; then + echo "With pytest options: $pytest_options" + fi + if [ -n "$filter_pattern" ]; then + echo "With filter pattern: $filter_pattern" + fi + + # Create reports directory + mkdir -p "$reports_dir" + + # Clean up any existing container + docker rm -f $container_name 2>/dev/null || true + + # Build Docker environment variables + docker_env_vars="\ + -e OMNI_KIT_ACCEPT_EULA=yes \ + -e ACCEPT_EULA=Y \ + -e OMNI_KIT_DISABLE_CUP=1 \ + -e ISAAC_SIM_HEADLESS=1 \ + -e ISAAC_SIM_LOW_MEMORY=1 \ + -e PYTHONUNBUFFERED=1 \ + -e PYTHONIOENCODING=utf-8 \ + -e TEST_RESULT_FILE=$result_file" + + if [ -n "$filter_pattern" ]; then + if [[ "$filter_pattern" == not* ]]; then + # Handle "not pattern" case + exclude_pattern="${filter_pattern#not }" + docker_env_vars="$docker_env_vars -e TEST_EXCLUDE_PATTERN=$exclude_pattern" + echo "Setting exclude pattern: $exclude_pattern" + else + # Handle positive pattern case + docker_env_vars="$docker_env_vars -e TEST_FILTER_PATTERN=$filter_pattern" + echo "Setting include pattern: $filter_pattern" + fi + else + echo "No filter pattern provided" + fi + + echo "Docker environment variables: '$docker_env_vars'" + + # Run tests in container with error handling + echo "๐Ÿš€ Starting Docker container for tests..." + if docker run --name $container_name \ + --entrypoint bash --gpus all --network=host \ + --security-opt=no-new-privileges:true \ + --memory=$(echo "$(free -m | awk '/^Mem:/{print $2}') * 0.9 / 1" | bc)m \ + --cpus=$(echo "$(nproc) * 0.9" | bc) \ + --oom-kill-disable=false \ + --ulimit nofile=65536:65536 \ + --ulimit nproc=4096:4096 \ + $docker_env_vars \ + $image_tag \ + -c " + set -e + cd /workspace/isaaclab + mkdir -p tests + echo 'Starting pytest with path: $test_path' + /isaac-sim/python.sh -m pytest --ignore=tools/conftest.py $test_path $pytest_options -v --junitxml=tests/$result_file || echo 'Pytest completed with exit code: $?' + "; then + echo "โœ… Docker container completed successfully" + else + echo "โš ๏ธ Docker container failed, but continuing to copy results..." + fi + + # Copy test results with error handling + echo "๐Ÿ“‹ Attempting to copy test results..." + if docker cp $container_name:/workspace/isaaclab/tests/$result_file "$reports_dir/$result_file" 2>/dev/null; then + echo "โœ… Test results copied successfully" + else + echo "โŒ Failed to copy specific result file, trying to copy all test results..." + if docker cp $container_name:/workspace/isaaclab/tests/ "$reports_dir/" 2>/dev/null; then + echo "โœ… All test results copied successfully" + # Look for any XML files and use the first one found + if [ -f "$reports_dir/full_report.xml" ]; then + mv "$reports_dir/full_report.xml" "$reports_dir/$result_file" + echo "โœ… Found and renamed full_report.xml to $result_file" + elif [ -f "$reports_dir/test-reports-"*".xml" ]; then + # Combine individual test reports if no full report exists + echo "๐Ÿ“Š Combining individual test reports..." + echo '' > "$reports_dir/$result_file" + for xml_file in "$reports_dir"/test-reports-*.xml; do + if [ -f "$xml_file" ]; then + echo " Processing: $xml_file" + sed '1d; /^> "$reports_dir/$result_file" 2>/dev/null || true + fi + done + echo '' >> "$reports_dir/$result_file" + echo "โœ… Combined individual test reports into $result_file" + else + echo "โŒ No test result files found, creating fallback" + echo "Container may have failed to generate any results" > "$reports_dir/$result_file" + fi + else + echo "โŒ Failed to copy any test results, creating fallback" + echo "Container may have failed to generate results" > "$reports_dir/$result_file" + fi + fi + + # Clean up container + echo "๐Ÿงน Cleaning up Docker container..." + docker rm $container_name 2>/dev/null || echo "โš ๏ธ Container cleanup failed, but continuing..." + } + + # Call the function with provided parameters + run_tests "${{ inputs.test-path }}" "${{ inputs.result-file }}" "${{ inputs.container-name }}" "${{ inputs.image-tag }}" "${{ inputs.reports-dir }}" "${{ inputs.pytest-options }}" "${{ inputs.filter-pattern }}" diff --git a/.github/labeler.yml b/.github/labeler.yml new file mode 100644 index 000000000000..2e6837fdb71a --- /dev/null +++ b/.github/labeler.yml @@ -0,0 +1,76 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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 + - 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/stale.yml b/.github/stale.yml index 9df5640208f0..6205170b38bd 100644 --- a/.github/stale.yml +++ b/.github/stale.yml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Configuration for probot-stale - https://github.com/probot/stale # Number of days of inactivity before an Issue or Pull Request becomes stale diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 000000000000..cbaa8f7b8e99 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,222 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: Build and Test + +on: + pull_request: + branches: + - devel + - main + - 'release/**' + +# Concurrency control to prevent parallel runs on the same PR +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + pull-requests: write + checks: write + issues: read + +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.1.0' }} + 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: + test-isaaclab-tasks: + runs-on: [self-hosted, gpu] + timeout-minutes: 180 + continue-on-error: true + + steps: + - name: Checkout Code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + lfs: true + + - name: Build Docker Image + uses: ./.github/actions/docker-build + with: + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + isaacsim-base-image: ${{ env.ISAACSIM_BASE_IMAGE }} + isaacsim-version: ${{ env.ISAACSIM_BASE_VERSION }} + + - name: Run IsaacLab Tasks Tests + uses: ./.github/actions/run-tests + with: + test-path: "tools" + result-file: "isaaclab-tasks-report.xml" + container-name: "isaac-lab-tasks-test-$$" + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + pytest-options: "" + filter-pattern: "isaaclab_tasks" + + - name: Copy Test Results from IsaacLab Tasks Container + run: | + CONTAINER_NAME="isaac-lab-tasks-test-$$" + if docker ps -a | grep -q $CONTAINER_NAME; then + echo "Copying test results from IsaacLab Tasks container..." + docker cp $CONTAINER_NAME:/workspace/isaaclab/tests/isaaclab-tasks-report.xml reports/ 2>/dev/null || echo "No test results to copy from IsaacLab Tasks container" + fi + + - name: Upload IsaacLab Tasks Test Results + uses: actions/upload-artifact@v4 + if: always() + with: + name: isaaclab-tasks-test-results + path: reports/isaaclab-tasks-report.xml + 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 + + steps: + - name: Checkout Code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + lfs: true + + - name: Build Docker Image + uses: ./.github/actions/docker-build + with: + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + isaacsim-base-image: ${{ env.ISAACSIM_BASE_IMAGE }} + isaacsim-version: ${{ env.ISAACSIM_BASE_VERSION }} + + - name: Run General Tests + id: run-general-tests + uses: ./.github/actions/run-tests + with: + test-path: "tools" + result-file: "general-tests-report.xml" + container-name: "isaac-lab-general-test-$$" + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + pytest-options: "" + filter-pattern: "not isaaclab_tasks" + + - name: Copy Test Results from General Tests Container + run: | + CONTAINER_NAME="isaac-lab-general-test-$$" + if docker ps -a | grep -q $CONTAINER_NAME; then + echo "Copying test results from General Tests container..." + docker cp $CONTAINER_NAME:/workspace/isaaclab/tests/general-tests-report.xml reports/ 2>/dev/null || echo "No test results to copy from General Tests container" + fi + + - name: Upload General Test Results + uses: actions/upload-artifact@v4 + if: always() + with: + name: general-test-results + path: reports/general-tests-report.xml + 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/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] + runs-on: [self-hosted, gpu] + if: always() + + steps: + - name: Checkout Code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + lfs: false + + - name: Create Reports Directory + run: | + mkdir -p reports + + - name: Download Test Results + uses: actions/download-artifact@v4 + with: + name: isaaclab-tasks-test-results + path: reports/ + continue-on-error: true + + - name: Download General Test Results + uses: actions/download-artifact@v4 + with: + name: general-test-results + path: reports/ + + - name: Combine All Test Results + uses: ./.github/actions/combine-results + with: + tests-dir: "reports" + output-file: "reports/combined-results.xml" + + - name: Upload Combined Test Results + uses: actions/upload-artifact@v4 + if: always() + with: + name: pr-${{ github.event.pull_request.number }}-combined-test-results + path: reports/combined-results.xml + retention-days: 7 + compression-level: 9 + + - name: Comment on Test Results + id: test-reporter + if: github.event.pull_request.head.repo.full_name == github.repository + uses: EnricoMi/publish-unit-test-result-action@v2 + with: + files: "reports/combined-results.xml" + check_name: "Tests Summary" + comment_mode: changes + comment_title: "Test Results Summary" + report_individual_runs: false + deduplicate_classes_by_file_name: true + compare_to_earlier_commit: true + fail_on: errors + action_fail_on_inconclusive: true + + - name: Report Test Results + if: github.event.pull_request.head.repo.full_name == github.repository + uses: dorny/test-reporter@v1 + with: + name: IsaacLab Build and Test Results + path: reports/combined-results.xml + reporter: java-junit + fail-on-error: true + only-summary: false + max-annotations: '50' + report-title: "IsaacLab Test Results - ${{ github.workflow }}" diff --git a/.github/workflows/check-links.yml b/.github/workflows/check-links.yml new file mode 100644 index 000000000000..a5f91e934032 --- /dev/null +++ b/.github/workflows/check-links.yml @@ -0,0 +1,120 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: Check Documentation Links + +on: + # Run on pull requests that modify documentation + pull_request: + paths: + - 'docs/**' + - '**.md' + - '.github/workflows/check-links.yml' + # Run on pushes to main branches + push: + branches: + - main + - devel + - 'release/**' + paths: + - 'docs/**' + - '**.md' + - '.github/workflows/check-links.yml' + # Allow manual trigger + workflow_dispatch: + # Run weekly to catch external links that break over time + schedule: + - cron: '0 0 * * 0' # Every Sunday at midnight UTC + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + check-links: + name: Check for Broken Links + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Restore lychee cache + uses: actions/cache@v4 + with: + path: .lycheecache + key: cache-lychee-${{ github.sha }} + restore-keys: cache-lychee- + + - name: Run Link Checker + uses: lycheeverse/lychee-action@v2 + with: + # Check all markdown files and documentation + args: >- + --verbose + --no-progress + --cache + --max-cache-age 1d + --exclude-path './docs/_build' + --exclude-path './apps/warp-*' + --exclude-path './logs' + --exclude-path './outputs' + --exclude-loopback + --exclude '^file://' + --exclude '^mailto:' + --exclude 'localhost' + --exclude '127\.0\.0\.1' + --exclude 'example\.com' + --exclude 'your-organization' + --exclude 'YOUR_' + --exclude 'yourdomain' + --exclude 'user@' + --exclude 'helm\.ngc\.nvidia\.com' + --exclude 'slurm\.schedmd\.com' + --max-retries 3 + --retry-wait-time 5 + --timeout 30 + --accept 200,201,202,203,204,206,301,302,303,307,308,429 + --scheme https + --scheme http + '*.md' + '**/*.md' + 'docs/**/*.rst' + 'docs/**/*.html' + # Output results to a file + output: ./lychee-output.md + # Fail action on broken links + fail: true + # Optional: Use GitHub token for authenticated requests (higher rate limit) + token: ${{ secrets.GITHUB_TOKEN }} + + - name: Print results to logs + if: always() + run: | + echo "========================================" + echo "Link Checker Results:" + echo "========================================" + if [ -f ./lychee-output.md ]; then + cat ./lychee-output.md + echo "" + echo "========================================" + + # Also add to GitHub step summary for easy viewing + echo "## Link Checker Results" >> $GITHUB_STEP_SUMMARY + echo "" >> $GITHUB_STEP_SUMMARY + cat ./lychee-output.md >> $GITHUB_STEP_SUMMARY + else + echo "No output file generated" + echo "========================================" + fi + + - name: Fail job if broken links found + if: failure() + run: | + echo "โŒ Broken links were found in the documentation!" + echo "Please review the link checker report above and fix all broken links." + exit 1 diff --git a/.github/workflows/daily-compatibility.yml b/.github/workflows/daily-compatibility.yml new file mode 100644 index 000000000000..bbf59e45160d --- /dev/null +++ b/.github/workflows/daily-compatibility.yml @@ -0,0 +1,257 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: Backwards Compatibility Tests + +on: + schedule: + # Run daily at 8 PM PST (4 AM UTC) + - cron: '0 4 * * *' + + workflow_dispatch: + inputs: + isaacsim_version: + description: 'IsaacSim version image tag to test' + required: true + default: '4.5.0' + type: string + +# Concurrency control to prevent parallel runs +concurrency: + group: compatibility-${{ github.ref }}-${{ github.event_name }} + cancel-in-progress: true + +permissions: + contents: read + pull-requests: write + +env: + NGC_API_KEY: ${{ secrets.NGC_API_KEY }} + ISAACSIM_BASE_IMAGE: ${{ vars.ISAACSIM_BASE_IMAGE || 'nvcr.io/nvidia/isaac-sim' }} + +jobs: + setup-versions: + runs-on: ubuntu-latest + outputs: + versions: ${{ steps.set-versions.outputs.versions }} + steps: + - name: Set Isaac Sim Versions + id: set-versions + run: | + # Define all versions to test in one place + DEFAULT_VERSIONS='["4.5.0", "5.0.0"]' + + if [ -n "${{ github.event.inputs.isaacsim_version }}" ]; then + # If a specific version is provided via workflow_dispatch, use only that + echo "versions=[\"${{ github.event.inputs.isaacsim_version }}\"]" >> $GITHUB_OUTPUT + else + # Otherwise, use all default versions + echo "versions=$DEFAULT_VERSIONS" >> $GITHUB_OUTPUT + fi + + test-isaaclab-tasks-compat: + needs: setup-versions + runs-on: [self-hosted, gpu] + timeout-minutes: 180 + continue-on-error: true + strategy: + matrix: + isaacsim_version: ${{ fromJson(needs.setup-versions.outputs.versions) }} + fail-fast: false + env: + CUDA_VISIBLE_DEVICES: all + NVIDIA_VISIBLE_DEVICES: all + NVIDIA_DRIVER_CAPABILITIES: all + CUDA_HOME: /usr/local/cuda + LD_LIBRARY_PATH: /usr/local/cuda/lib64:/usr/local/cuda/extras/CUPTI/lib64 + DOCKER_IMAGE_TAG: isaac-lab-compat:${{ github.ref_name }}-${{ github.sha }}-${{ matrix.isaacsim_version }} + + steps: + - name: Checkout Code + uses: actions/checkout@v3 + with: + fetch-depth: 0 + lfs: true + + - name: Build Docker Image + uses: ./.github/actions/docker-build + with: + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + isaacsim-base-image: ${{ env.ISAACSIM_BASE_IMAGE }} + isaacsim-version: ${{ matrix.isaacsim_version }} + + - name: Run IsaacLab Tasks Tests + uses: ./.github/actions/run-tests + with: + test-path: "tools" + result-file: "isaaclab-tasks-compat-report.xml" + container-name: "isaac-lab-tasks-compat-test-$$" + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + pytest-options: "" + filter-pattern: "isaaclab_tasks" + + - name: Copy All Test Results from IsaacLab Tasks Container + run: | + CONTAINER_NAME="isaac-lab-tasks-compat-test-$$" + if docker ps -a | grep -q $CONTAINER_NAME; then + echo "Copying all test results from IsaacLab Tasks container..." + docker cp $CONTAINER_NAME:/workspace/isaaclab/tests/isaaclab-tasks-compat-report.xml reports/ 2>/dev/null || echo "No test results to copy from IsaacLab Tasks container" + fi + + - name: Upload IsaacLab Tasks Test Results + uses: actions/upload-artifact@v4 + if: always() + with: + name: isaaclab-tasks-compat-results-${{ matrix.isaacsim_version }} + path: reports/isaaclab-tasks-compat-report.xml + retention-days: 7 + compression-level: 9 + + test-general-compat: + needs: setup-versions + runs-on: [self-hosted, gpu] + timeout-minutes: 180 + strategy: + matrix: + isaacsim_version: ${{ fromJson(needs.setup-versions.outputs.versions) }} + fail-fast: false + env: + CUDA_VISIBLE_DEVICES: all + NVIDIA_VISIBLE_DEVICES: all + NVIDIA_DRIVER_CAPABILITIES: all + CUDA_HOME: /usr/local/cuda + LD_LIBRARY_PATH: /usr/local/cuda/lib64:/usr/local/cuda/extras/CUPTI/lib64 + DOCKER_IMAGE_TAG: isaac-lab-compat:${{ github.ref_name }}-${{ github.sha }}-${{ matrix.isaacsim_version }} + + steps: + - name: Checkout Code + uses: actions/checkout@v3 + with: + fetch-depth: 0 + lfs: true + + - name: Build Docker Image + uses: ./.github/actions/docker-build + with: + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + isaacsim-base-image: ${{ env.ISAACSIM_BASE_IMAGE }} + isaacsim-version: ${{ matrix.isaacsim_version }} + + - name: Run General Tests + uses: ./.github/actions/run-tests + with: + test-path: "tools" + result-file: "general-tests-compat-report.xml" + container-name: "isaac-lab-general-compat-test-$$" + image-tag: ${{ env.DOCKER_IMAGE_TAG }} + pytest-options: "" + filter-pattern: "not isaaclab_tasks" + + - name: Copy All Test Results from General Tests Container + run: | + CONTAINER_NAME="isaac-lab-general-compat-test-$$" + if docker ps -a | grep -q $CONTAINER_NAME; then + echo "Copying all test results from General Tests container..." + docker cp $CONTAINER_NAME:/workspace/isaaclab/tests/general-tests-compat-report.xml reports/ 2>/dev/null || echo "No test results to copy from General Tests container" + fi + + - name: Upload General Test Results + uses: actions/upload-artifact@v4 + if: always() + with: + name: general-tests-compat-results-${{ matrix.isaacsim_version }} + path: reports/general-tests-compat-report.xml + retention-days: 7 + compression-level: 9 + + combine-compat-results: + needs: [test-isaaclab-tasks-compat, test-general-compat] + runs-on: [self-hosted, gpu] + if: always() + + steps: + - name: Checkout Code + uses: actions/checkout@v3 + with: + fetch-depth: 0 + lfs: false + + - name: Create Reports Directory + run: | + mkdir -p reports + + - name: Download All Test Results + uses: actions/download-artifact@v4 + with: + pattern: '*-compat-results-*' + path: reports/ + merge-multiple: true + continue-on-error: true + + - name: Combine All Test Results + uses: ./.github/actions/combine-results + with: + tests-dir: "reports" + output-file: "reports/combined-compat-results.xml" + + - name: Upload Combined Test Results + uses: actions/upload-artifact@v4 + if: always() + with: + name: daily-compat-${{ github.run_id }}-combined-test-results + path: reports/combined-compat-results.xml + retention-days: 30 + compression-level: 9 + + - name: Report Test Results + uses: dorny/test-reporter@v1 + if: always() + with: + name: IsaacLab Compatibility Test Results (${{ github.event_name }}) + path: reports/combined-compat-results.xml + reporter: java-junit + max-annotations: '50' + report-title: "IsaacLab Compatibility Test Results - ${{ github.event_name }} - ${{ github.ref_name }}" + + notify-compatibility-status: + needs: [setup-versions, combine-compat-results] + runs-on: [self-hosted, gpu] + if: always() + + steps: + - name: Checkout Code + uses: actions/checkout@v3 + with: + fetch-depth: 0 + lfs: false + + - name: Create Compatibility Report + run: | + TRIGGER_INFO="**Trigger:** ${{ github.event_name }}" + ISAACSIM_VERSIONS="${{ join(fromJson(needs.setup-versions.outputs.versions), ', ') }}" + echo "## Daily Backwards Compatibility Test Results" > compatibility-report.md + echo "" >> compatibility-report.md + echo "$TRIGGER_INFO" >> compatibility-report.md + echo "**IsaacSim Versions Tested:** $ISAACSIM_VERSIONS" >> compatibility-report.md + echo "**Branch:** ${{ github.ref_name }}" >> compatibility-report.md + echo "**Commit:** ${{ github.sha }}" >> compatibility-report.md + echo "**Run ID:** ${{ github.run_id }}" >> compatibility-report.md + echo "" >> compatibility-report.md + echo "### Test Status:" >> compatibility-report.md + echo "- Results: ${{ needs.combine-compat-results.result }}" >> compatibility-report.md + echo "" >> compatibility-report.md + echo "### Artifacts:" >> compatibility-report.md + echo "- [Combined Test Results](https://github.com/${{ github.repository }}/actions/runs/${{ github.run_id }})" >> compatibility-report.md + echo "" >> compatibility-report.md + echo "---" >> compatibility-report.md + echo "*This report was generated automatically by the daily compatibility workflow.*" >> compatibility-report.md + + - name: Upload Compatibility Report + uses: actions/upload-artifact@v4 + if: always() + with: + name: compatibility-report-${{ github.run_id }} + path: compatibility-report.md + retention-days: 30 diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml index 4680ef667f52..9af2d6e94f0b 100644 --- a/.github/workflows/docs.yaml +++ b/.github/workflows/docs.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + name: Build & deploy docs on: @@ -5,6 +10,7 @@ on: branches: - main - devel + - 'release/**' pull_request: types: [opened, synchronize, reopened] @@ -22,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: @@ -38,7 +43,7 @@ jobs: - name: Setup python uses: actions/setup-python@v2 with: - python-version: "3.10" + python-version: "3.11" architecture: x64 - name: Install dev requirements diff --git a/.github/workflows/labeler.yml b/.github/workflows/labeler.yml new file mode 100644 index 000000000000..593aec9a2cb0 --- /dev/null +++ b/.github/workflows/labeler.yml @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 new file mode 100644 index 000000000000..e3753ffcb6b5 --- /dev/null +++ b/.github/workflows/license-check.yaml @@ -0,0 +1,136 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: Check Python Dependency Licenses + +on: + pull_request: + types: [opened, synchronize, reopened] + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + license-check: + runs-on: ubuntu-24.04 + + steps: + - name: Checkout code + uses: actions/checkout@v3 + + # - name: Install jq + # run: sudo apt-get update && sudo apt-get install -y jq + + - name: Clean up disk space + run: | + rm -rf /opt/hostedtoolcache + rm -rf /usr/share/dotnet + rm -rf /opt/ghc + docker container prune -f + docker image prune -af + docker volume prune -f || true + + + - name: Set up Python + uses: actions/setup-python@v4 + with: + 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]==${{ 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 + + - name: Install pip-licenses + run: | + pip install pip-licenses + pip install -r tools/template/requirements.txt + pip install -r docs/requirements.txt + + # Optional: Print the license report for visibility + - 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 + ALLOWED_LICENSES="MIT Apache BSD ISC zlib" + + # Load the exceptions list from the exceptions.json file + EXCEPTIONS_FILE=".github/workflows/license-exceptions.json" + + # Initialize counter for failed packages + FAILED_PACKAGES=0 + + # Get the list of installed packages and their licenses + pip-licenses --from=mixed --format=json > licenses.json + + # Check the output of pip-licenses to ensure it is valid JSON + if ! jq empty licenses.json; then + echo "ERROR: Failed to parse pip-licenses output. Exiting..." + exit 1 + fi + + # Split ALLOWED_LICENSES into individual words + IFS=' ' read -r -a allowed_licenses <<< "$ALLOWED_LICENSES" + + # Loop through the installed packages and their licenses + for pkg in $(jq -r '.[].Name' licenses.json); do + LICENSE=$(jq -r --arg pkg "$pkg" '.[] | select(.Name == $pkg) | .License' licenses.json) + + # Check if any of the allowed licenses are a substring of the package's license + match_found=false + for allowed_license in "${allowed_licenses[@]}"; do + if [[ "$LICENSE" == *"$allowed_license"* ]]; then + match_found=true + break + fi + done + + if [ "$match_found" = false ]; then + # Check if the package is in the exceptions list + EXCEPTION=$(jq -r --arg pkg "$pkg" --arg license "$LICENSE" \ + '.[] | select(.package == $pkg)' "$EXCEPTIONS_FILE") + + # If the package is in the exceptions list + if [ -n "$EXCEPTION" ]; then + # If the license is provided in the exceptions list, check the license + EXCEPTION_LICENSE=$(echo "$EXCEPTION" | jq -r '.license') + + # echo "Comparing licenses for $pkg:" + # echo " EXCEPTION_LICENSE='${EXCEPTION_LICENSE}' (len=${#EXCEPTION_LICENSE})" + # echo " LICENSE='${LICENSE}' (len=${#LICENSE})" + + # If the exceptions list has a license and doesn't match the current license + if [ "$EXCEPTION_LICENSE" != "null" ] && [ "$EXCEPTION_LICENSE" != "$LICENSE" ]; then + echo "ERROR: $pkg has license: $LICENSE" + FAILED_PACKAGES=$((FAILED_PACKAGES + 1)) # Increment the counter + fi + else + # If the package is not in the exceptions list + echo "ERROR: $pkg has license: $LICENSE" + FAILED_PACKAGES=$((FAILED_PACKAGES + 1)) # Increment the counter + fi + fi + done + + # After all packages are processed, check if there were any errors + if [ "$FAILED_PACKAGES" -gt 0 ]; then + echo "ERROR: $FAILED_PACKAGES packages were flagged." + exit 1 # Fail the build + else + echo "All packages were checked." + fi diff --git a/.github/workflows/license-exceptions.json b/.github/workflows/license-exceptions.json new file mode 100644 index 000000000000..27b3b9c65522 --- /dev/null +++ b/.github/workflows/license-exceptions.json @@ -0,0 +1,448 @@ +[ + { + "package": "isaaclab", + "license": null + }, + { + "package": "isaaclab_assets", + "license": null + }, + { + "package": "isaaclab_contrib", + "license": null + }, + { + "package": "isaaclab_mimic", + "license": null + }, + { + "package": "isaaclab_rl", + "license": null + }, + { + "package": "isaaclab_tasks", + "license": null + }, + { + "package": "isaacsim", + "license": null + }, + { + "package": "isaacsim-app", + "license": null + }, + { + "package": "isaacsim-asset", + "license": null + }, + { + "package": "isaacsim-benchmark", + "license": null + }, + { + "package": "isaacsim-code-editor", + "license": null + }, + { + "package": "isaacsim-core", + "license": null + }, + { + "package": "isaacsim-cortex", + "license": null + }, + { + "package": "isaacsim-example", + "license": null + }, + { + "package": "isaacsim-extscache-kit", + "license": null + }, + { + "package": "isaacsim-extscache-kit-sdk", + "license": null + }, + { + "package": "isaacsim-extscache-physics", + "license": null + }, + { + "package": "isaacsim-gui", + "license": null + }, + { + "package": "isaacsim-kernel", + "license": null + }, + { + "package": "isaacsim-replicator", + "license": null + }, + { + "package": "isaacsim-rl", + "license": null + }, + { + "package": "isaacsim-robot", + "license": null + }, + { + "package": "isaacsim-robot-motion", + "license": null + }, + { + "package": "isaacsim-robot-setup", + "license": null + }, + { + "package": "isaacsim-ros1", + "license": null + }, + { + "package": "isaacsim-ros2", + "license": null + }, + { + "package": "isaacsim-sensor", + "license": null + }, + { + "package": "isaacsim-storage", + "license": null + }, + { + "package": "isaacsim-template", + "license": null + }, + { + "package": "isaacsim-test", + "license": null + }, + { + "package": "isaacsim-utils", + "license": null + }, + { + "package": "nvidia-cublas-cu12", + "license": null + }, + { + "package": "nvidia-cuda-cupti-cu12", + "license": null + }, + { + "package": "nvidia-cuda-nvrtc-cu12", + "license": null + }, + { + "package": "nvidia-cuda-runtime-cu12", + "license": null + }, + { + "package": "nvidia-cudnn-cu12", + "license": null + }, + { + "package": "nvidia-cufft-cu12", + "license": null + }, + { + "package": "nvidia-cufile-cu12", + "license": null + }, + { + "package": "nvidia-curand-cu12", + "license": null + }, + { + "package": "nvidia-cusolver-cu12", + "license": null + }, + { + "package": "nvidia-cusparse-cu12", + "license": null + }, + { + "package": "nvidia-cusparselt-cu12", + "license": null + }, + { + "package": "nvidia-nccl-cu12", + "license": null + }, + { + "package": "nvidia-nvjitlink-cu12", + "license": null + }, + { + "package": "nvidia-nvtx-cu12", + "license": null + }, + { + "package": "omniverse-kit", + "license": null + }, + { + "package": "warp-lang", + "license": null + }, + { + "package": "cmeel", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "cmeel-assimp", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "cmeel-boost", + "license": "BSL-1.0", + "comment": "BSL" + }, + { + "package": "cmeel-console-bridge", + "license": "Zlib", + "comment": "ZLIBL" + }, + { + "package": "cmeel-octomap", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "cmeel-qhull", + "license": "UNKNOWN", + "comment": "custom / OSRB" + }, + { + "package": "cmeel-tinyxml", + "license": "Zlib", + "comment": "ZLIBL" + }, + { + "package": "cmeel-urdfdom", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "cmeel-zlib", + "license": "Zlib", + "comment": "ZLIBL" + }, + { + "package": "matplotlib", + "license": "Python Software Foundation License" + }, + { + "package": "certifi", + "license": "Mozilla Public License 2.0 (MPL 2.0)" + }, + { + "package": "rl_games", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "robomimic", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "hpp-fcl", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "pin", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "eigenpy", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "qpsolvers", + "license": "GNU Lesser General Public License v3 (LGPLv3)", + "comment": "OSRB" + }, + { + "package": "quadprog", + "license": "GNU General Public License v2 or later (GPLv2+)", + "comment": "OSRB" + }, + { + "package": "Markdown", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "anytree", + "license": "UNKNOWN", + "comment": "Apache" + }, + { + "package": "click", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "egl_probe", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "filelock", + "license": "Unlicense", + "comment": "no condition" + }, + { + "package": "proglog", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "termcolor", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "typing_extensions", + "license": "Python Software Foundation License", + "comment": "PSFL / OSRB" + }, + { + "package": "urllib3", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "h5py", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "pillow", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "pygame", + "license": "GNU Library or Lesser General Public License (LGPL)", + "comment": "OSRB" + }, + { + "package": "scikit-learn", + "license": "UNKNOWN", + "comment": "BSD" + }, + { + "package": "tensorboardX", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "attrs", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "jsonschema", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "jsonschema-specifications", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "referencing", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "regex", + "license": "UNKNOWN", + "comment": "Apache 2.0" + }, + { + "package": "anyio", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package" : "hf-xet", + "license" : "UNKNOWN", + "comment": "Apache 2.0" + }, + { + "package": "rpds-py", + "license" : "UNKNOWN", + "comment": "MIT" + }, + { + "package": "typing-inspection", + "license" : "UNKNOWN", + "comment": "MIT" + }, + { + "package": "ml_dtypes", + "license" : "UNKNOWN", + "comment": "Apache 2.0" + }, + { + "package": "zipp", + "license" : "UNKNOWN", + "comment": "MIT" + }, + { + "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 new file mode 100644 index 000000000000..4a1f34d38a16 --- /dev/null +++ b/.github/workflows/postmerge-ci.yml @@ -0,0 +1,170 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: Post-Merge CI + +on: + push: + branches: + - main + - devel + - release/** + +# Concurrency control to prevent parallel runs +concurrency: + group: postmerge-ci-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + +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 || '5.1.0' }} + ISAACLAB_IMAGE_NAME: ${{ vars.ISAACLAB_IMAGE_NAME || 'isaac-lab-base' }} + +jobs: + build-and-push-images: + runs-on: [self-hosted, gpu] + timeout-minutes: 180 + environment: + name: postmerge-production + url: https://github.com/${{ github.repository }} + env: + DOCKER_HOST: unix:///var/run/docker.sock + DOCKER_TLS_CERTDIR: "" + + steps: + - name: Checkout Code + uses: actions/checkout@v4 + with: + 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: | + # Only attempt NGC login if API key is available + if [ -n "${{ env.NGC_API_KEY }}" ]; then + echo "Logging into NGC registry..." + docker login -u \$oauthtoken -p ${{ env.NGC_API_KEY }} nvcr.io + echo "โœ… Successfully logged into NGC registry" + else + echo "โš ๏ธ NGC_API_KEY not available - skipping NGC login" + echo "This is normal when secrets are not configured" + fi + + - name: Build and Push Docker Images + run: | + # Determine branch name + BRANCH_NAME="${{ github.ref_name }}" + + # Replace '/' with '-' and remove any invalid characters for Docker tag + SAFE_BRANCH_NAME=$(echo $BRANCH_NAME | sed 's/[^a-zA-Z0-9._-]/-/g') + + # Use "latest" if branch name is empty or only contains invalid characters + if [ -z "$SAFE_BRANCH_NAME" ]; then + SAFE_BRANCH_NAME="latest" + fi + + # Get the git repository short name + REPO_SHORT_NAME="${{ github.event.repository.name }}" + if [ -z "$REPO_SHORT_NAME" ]; then + REPO_SHORT_NAME="verification" + fi + + echo "Building images for branch: $BRANCH_NAME" + echo "Safe branch name: $SAFE_BRANCH_NAME" + echo "Repository name: $REPO_SHORT_NAME" + echo "IsaacSim versions: ${{ env.ISAACSIM_BASE_VERSIONS_STRING }}" + + # Parse the env variable string into an array + IMAGE_BASE_VERSIONS_STRING="${{ env.ISAACSIM_BASE_VERSIONS_STRING }}" + # Use set to split the string into positional parameters, then convert to array + set -- $IMAGE_BASE_VERSIONS_STRING + IMAGE_BASE_VERSIONS=("$@") + + for IMAGE_BASE_VERSION in "${IMAGE_BASE_VERSIONS[@]}"; do + IMAGE_BASE_VERSION=$(echo "$IMAGE_BASE_VERSION" | tr -d '[:space:]') + + # Skip empty versions + if [ -z "$IMAGE_BASE_VERSION" ]; then + continue + fi + + # Combine repo short name and branch name for the tag + 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 for multiple architectures + docker buildx build \ + --platform $BUILD_PLATFORMS \ + --progress=plain \ + -t ${{ env.ISAACLAB_IMAGE_NAME }}:$COMBINED_TAG \ + -t ${{ env.ISAACLAB_IMAGE_NAME }}:$BUILD_TAG \ + --build-arg ISAACSIM_BASE_IMAGE_ARG=${{ env.ISAACSIM_BASE_IMAGE }} \ + --build-arg ISAACSIM_VERSION_ARG=$IMAGE_BASE_VERSION \ + --build-arg ISAACSIM_ROOT_PATH_ARG=/isaac-sim \ + --build-arg ISAACLAB_PATH_ARG=/workspace/isaaclab \ + --build-arg DOCKER_USER_HOME_ARG=/root \ + --cache-from type=gha \ + --cache-to type=gha,mode=max \ + -f docker/Dockerfile.base \ + --push . + + 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 d34fcde4466d..f71f1f373899 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -1,9 +1,13 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + name: Run linters using pre-commit on: pull_request: - push: - branches: [main] + types: [opened, synchronize, reopened] jobs: pre-commit: @@ -11,4 +15,6 @@ jobs: steps: - uses: actions/checkout@v3 - uses: actions/setup-python@v3 + with: + python-version: "3.12" - uses: pre-commit/action@v3.0.0 diff --git a/.gitignore b/.gitignore index ea62f833b86b..7afb58e9ee00 100644 --- a/.gitignore +++ b/.gitignore @@ -60,3 +60,16 @@ _build # Pre-Trained Checkpoints /.pretrained_checkpoints/ + +# Teleop Recorded Dataset +/datasets/ + +# Tests +tests/ + +# Docker history +.isaac-lab-docker-history + +# TacSL sensor +**/tactile_record/* +**/gelsight_r15_data/* diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index f3a785beb667..426a84b59b79 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,16 +1,19 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + repos: - - repo: https://github.com/python/black - rev: 24.3.0 + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.14.10 hooks: - - id: black - args: ["--line-length", "120", "--unstable"] - - repo: https://github.com/pycqa/flake8 - rev: 7.0.0 - hooks: - - id: flake8 - additional_dependencies: [flake8-simplify, flake8-return] + # Run the linter + - id: ruff + args: ["--fix"] + # Run the formatter + - id: ruff-format - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v6.0.0 hooks: - id: trailing-whitespace - id: check-symlinks @@ -26,36 +29,23 @@ repos: - id: check-shebang-scripts-are-executable - id: detect-private-key - id: debug-statements - - repo: https://github.com/pycqa/isort - rev: 5.13.2 - hooks: - - id: isort - name: isort (python) - args: ["--profile", "black", "--filter-files"] - - repo: https://github.com/asottile/pyupgrade - rev: v3.15.1 - hooks: - - id: pyupgrade - args: ["--py310-plus"] - # FIXME: This is a hack because Pytorch does not like: torch.Tensor | dict aliasing - exclude: "source/isaaclab/isaaclab/envs/common.py|source/isaaclab/isaaclab/ui/widgets/image_plot.py|source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py" - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.4.1 hooks: - id: codespell additional_dependencies: - tomli - exclude: "CONTRIBUTORS.md" + exclude: "CONTRIBUTORS.md|docs/source/setup/walkthrough/concepts_env_design.rst" # FIXME: Figure out why this is getting stuck under VPN. # - repo: https://github.com/RobertCraigie/pyright-python # rev: v1.1.315 # hooks: # - id: pyright - repo: https://github.com/Lucas-C/pre-commit-hooks - rev: v1.5.1 + rev: v1.5.5 hooks: - id: insert-license - files: \.py$ + files: \.(py|ya?ml)$ args: # - --remove-header # Remove existing license headers. Useful when updating license. - --license-filepath @@ -64,7 +54,7 @@ repos: exclude: "source/isaaclab_mimic/|scripts/imitation_learning/isaaclab_mimic/" # Apache 2.0 license for mimic files - repo: https://github.com/Lucas-C/pre-commit-hooks - rev: v1.5.1 + rev: v1.5.5 hooks: - id: insert-license files: ^(source/isaaclab_mimic|scripts/imitation_learning/isaaclab_mimic)/.*\.py$ diff --git a/.vscode/tools/launch.template.json b/.vscode/tools/launch.template.json index c7d6471a1790..a44f114c822b 100644 --- a/.vscode/tools/launch.template.json +++ b/.vscode/tools/launch.template.json @@ -33,6 +33,25 @@ "args" : ["--task", "Isaac-Reach-Franka-v0", "--num_envs", "32"], "program": "${workspaceFolder}/scripts/reinforcement_learning/rsl_rl/play.py", "console": "integratedTerminal" + }, + { + "name": "Python: SinglePytest", + "type": "python", + "request": "launch", + "module": "pytest", + "args": [ + "${file}" + ], + "console": "integratedTerminal" + }, + { + "name": "Python: ALL Pytest", + "type": "python", + "request": "launch", + "module": "pytest", + "args": ["source/isaaclab/test"], + "console": "integratedTerminal", + "justMyCode": false } ] } diff --git a/.vscode/tools/settings.template.json b/.vscode/tools/settings.template.json index fa0f1c823169..4b07a6a8f9ad 100644 --- a/.vscode/tools/settings.template.json +++ b/.vscode/tools/settings.template.json @@ -61,15 +61,8 @@ ], // This enables python language server. Seems to work slightly better than jedi: "python.languageServer": "Pylance", - // We use "black" as a formatter: - "python.formatting.provider": "black", - "python.formatting.blackArgs": ["--line-length", "120"], - // Use flake8 for linting - "python.linting.pylintEnabled": false, - "python.linting.flake8Enabled": true, - "python.linting.flake8Args": [ - "--max-line-length=120" - ], + // Use ruff as a formatter and linter + "ruff.configuration": "${workspaceFolder}/pyproject.toml", // Use docstring generator "autoDocstring.docstringFormat": "google", "autoDocstring.guessTypes": true, diff --git a/.vscode/tools/setup_vscode.py b/.vscode/tools/setup_vscode.py index e905767a75e6..9e2c6375ab75 100644 --- a/.vscode/tools/setup_vscode.py +++ b/.vscode/tools/setup_vscode.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/CITATION.cff b/CITATION.cff new file mode 100644 index 000000000000..70d123b55242 --- /dev/null +++ b/CITATION.cff @@ -0,0 +1,227 @@ +cff-version: 1.2.0 +message: "If you use this software, please cite the technical report of Isaac Lab." +title: Isaac Lab +version: 2.3.2 +repository-code: https://github.com/isaac-sim/IsaacLab +type: software +authors: + - name: Isaac Lab Project Developers +identifiers: + - type: url + value: https://github.com/isaac-sim/IsaacLab + - type: doi + value: 10.48550/arXiv.2511.04831 +url: https://isaac-sim.github.io/IsaacLab +license: BSD-3-Clause +preferred-citation: + type: article + title: Isaac Lab - A GPU-Accelerated Simulation Framework for Multi-Modal Robot Learning + authors: + - family-names: Mittal + given-names: Mayank + - family-names: Roth + given-names: Pascal + - family-names: Tigue + given-names: James + - family-names: Richard + given-names: Antoine + - family-names: Zhang + given-names: Octi + - family-names: Du + given-names: Peter + - family-names: Serrano-Muรฑoz + given-names: Antonio + - family-names: Yao + given-names: Xinjie + - family-names: Zurbrรผgg + given-names: Renรฉ + - family-names: Rudin + given-names: Nikita + - family-names: Wawrzyniak + given-names: Lukasz + - family-names: Rakhsha + given-names: Milad + - family-names: Denzler + given-names: Alain + - family-names: Heiden + given-names: Eric + - family-names: Borovicka + given-names: Ales + - family-names: Ahmed + given-names: Ossama + - family-names: Akinola + given-names: Iretiayo + - family-names: Anwar + given-names: Abrar + - family-names: Carlson + given-names: Mark T. + - family-names: Feng + given-names: Ji Yuan + - family-names: Garg + given-names: Animesh + - family-names: Gasoto + given-names: Renato + - family-names: Gulich + given-names: Lionel + - family-names: Guo + given-names: Yijie + - family-names: Gussert + given-names: M. + - family-names: Hansen + given-names: Alex + - family-names: Kulkarni + given-names: Mihir + - family-names: Li + given-names: Chenran + - family-names: Liu + given-names: Wei + - family-names: Makoviychuk + given-names: Viktor + - family-names: Malczyk + given-names: Grzegorz + - family-names: Mazhar + given-names: Hammad + - family-names: Moghani + given-names: Masoud + - family-names: Murali + given-names: Adithyavairavan + - family-names: Noseworthy + given-names: Michael + - family-names: Poddubny + given-names: Alexander + - family-names: Ratliff + given-names: Nathan + - family-names: Rehberg + given-names: Welf + - family-names: Schwarke + given-names: Clemens + - family-names: Singh + given-names: Ritvik + - family-names: Smith + given-names: James Latham + - family-names: Tang + given-names: Bingjie + - family-names: Thaker + given-names: Ruchik + - family-names: Trepte + given-names: Matthew + - family-names: Van Wyk + given-names: Karl + - family-names: Yu + given-names: Fangzhou + - family-names: Millane + given-names: Alex + - family-names: Ramasamy + given-names: Vikram + - family-names: Steiner + given-names: Remo + - family-names: Subramanian + given-names: Sangeeta + - family-names: Volk + given-names: Clemens + - family-names: Chen + given-names: CY + - family-names: Jawale + given-names: Neel + - family-names: Kuruttukulam + given-names: Ashwin Varghese + - family-names: Lin + given-names: Michael A. + - family-names: Mandlekar + given-names: Ajay + - family-names: Patzwaldt + given-names: Karsten + - family-names: Welsh + given-names: John + - family-names: Lafleche + given-names: Jean-Francois + - family-names: Moรซnne-Loccoz + given-names: Nicolas + - family-names: Park + given-names: Soowan + - family-names: Stepinski + given-names: Rob + - family-names: Van Gelder + given-names: Dirk + - family-names: Amevor + given-names: Chris + - family-names: Carius + given-names: Jan + - family-names: Chang + given-names: Jumyung + - family-names: He Chen + given-names: Anka + - family-names: Ciechomski + given-names: Pablo de Heras + - family-names: Daviet + given-names: Gilles + - family-names: Mohajerani + given-names: Mohammad + - family-names: von Muralt + given-names: Julia + - family-names: Reutskyy + given-names: Viktor + - family-names: Sauter + given-names: Michael + - family-names: Schirm + given-names: Simon + - family-names: Shi + given-names: Eric L. + - family-names: Terdiman + given-names: Pierre + - family-names: Vilella + given-names: Kenny + - family-names: Widmer + given-names: Tobias + - family-names: Yeoman + given-names: Gordon + - family-names: Chen + given-names: Tiffany + - family-names: Grizan + given-names: Sergey + - family-names: Li + given-names: Cathy + - family-names: Li + given-names: Lotus + - family-names: Smith + given-names: Connor + - family-names: Wiltz + given-names: Rafael + - family-names: Alexis + given-names: Kostas + - family-names: Chang + given-names: Yan + - family-names: Fan + given-names: Linxi "Jim" + - family-names: Farshidian + given-names: Farbod + - family-names: Handa + given-names: Ankur + - family-names: Huang + given-names: Spencer + - family-names: Hutter + given-names: Marco + - family-names: Narang + given-names: Yashraj + - family-names: Pouya + given-names: Soha + - family-names: Sheng + given-names: Shiwei + - family-names: Zhu + given-names: Yuke + - family-names: Macklin + given-names: Miles + - family-names: Moravanszky + given-names: Adam + - family-names: Reist + given-names: Philipp + - family-names: Guo + given-names: Yunrong + - family-names: Hoeller + given-names: David + - family-names: State + given-names: Gavriel + journal: arXiv preprint arXiv:2511.04831 + year: 2025 + url: https://arxiv.org/abs/2511.04831 + doi: 10.48550/arXiv.2511.04831 diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index b837c0df9241..d627fa2e27b6 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -6,3 +6,40 @@ design proposals and more. For general information on how to contribute see . + +--- + +Developer Certificate of Origin +Version 1.1 + +Copyright (C) 2004, 2006 The Linux Foundation and its contributors. + +Everyone is permitted to copy and distribute verbatim copies of this +license document, but changing it is not allowed. + + +Developer's Certificate of Origin 1.1 + +By making a contribution to this project, I certify that: + +(a) The contribution was created in whole or in part by me and I + have the right to submit it under the open source license + indicated in the file; or + +(b) The contribution is based upon previous work that, to the best + of my knowledge, is covered under an appropriate open source + license and I have the right under that license to submit that + work with modifications, whether created in whole or in part + by me, under the same open source license (unless I am + permitted to submit under a different license), as indicated + in the file; or + +(c) The contribution was provided directly to me by some other + person who certified (a), (b) or (c) and I have not modified + it. + +(d) I understand and agree that this project and the contribution + are public and that a record of the contribution (including all + personal information I submit with it, including my sign-off) is + maintained indefinitely and may be redistributed consistent with + this project or the open source license(s) involved. diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index d84ad8963c36..9fbfe7f1bf30 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -20,6 +20,9 @@ Guidelines for modifications: --- * Antonio Serrano-Muรฑoz +* Ben Johnston +* Brian McCann +* Clemens Schwarke * David Hoeller * Farbod Farshidian * Hunter Hansen @@ -29,74 +32,149 @@ Guidelines for modifications: * Matthew Trepte * Mayank Mittal * Nikita Rudin +* Octi (Zhengyu) Zhang * Pascal Roth * Sheikh Dawood * Ossama Ahmed +* Greg Attra ## Contributors +* Alessandro Assirelli +* Alex Omar * Alice Zhou * Amr Mousa * Andrej Orsula * Anton Bjรธrndahl Mortensen +* Antonin Raffin * Arjun Bhardwaj * Ashwin Varghese Kuruttukulam +* Bikram Pandit +* Bingjie Tang * Brayden Zhang +* Brian Bingham +* Brian McCann * Cameron Upright * Calvin Yu +* Cathy Y. Li * Cheng-Rong Lai * Chenyu Yang +* Connor Smith * CY (Chien-Ying) Chen * David Yang +* Dhananjay Shendre +* Dongxuan Fan * Dorsa Rohani +* Emily Sturman +* Emmanuel Ferdman +* Fabian Jenelten +* Felipe Mohr * Felix Yu * Gary Lvov * Giulio Romualdi +* Grzegorz Malczyk * Haoran Zhou +* Harsh Patel * HoJin Jeon * Hongwei Xiong * Hongyu Li +* Hougant Chen +* Huihua Zhao * Iretiayo Akinola * Jack Zeng * Jan Kerner * Jean Tampon +* Jeonghwan Kim * Jia Lin Yuan +* Jiakai Zhang * Jinghuan Shang * Jingzhou Liu * Jinqi Wei +* Jinyeob Kim * Johnson Sun +* Juana Du * Kaixi Bao +* Kris Wilson +* Krishna Lakhi * Kourosh Darvish +* Kousheek Chakraborty * Lionel Gulich +* Lotus Li * Louis Le Lay * Lorenz Wellhausen +* Lukas Frรถhlich * Manuel Schweiger * Masoud Moghani +* Mateo Guaman Castro +* Maurice Rahme * Michael Gussert * Michael Noseworthy +* Michael Lin +* Miguel Alonso Jr +* Mihir Kulkarni +* Mingxue Gu +* Mingyu Lee * Muhong Guo +* Narendra Dahile +* Neel Anand Jawale * Nicola Loi +* Norbert Cygiert +* Nuoyan Chen (Alvin) * Nuralem Abizov * Ori Gadot * Oyindamola Omotuyi * ร–zhan ร–zen +* Patrick Yin * Peter Du +* Philipp Reist +* Pulkit Goyal * Qian Wan +* Qingyang Jiang * Qinxi Yu +* Rafael Wiltz +* Renaud Poncelet * Renรฉ Zurbrรผgg * Ritvik Singh * Rosario Scalise +* Ryan Gresia * Ryley McCarroll +* Sahara Yuta +* Sergey Grizan * Shafeef Omar +* Shane Reetz +* Shaoshu Su +* Shaurya Dewan +* Sixiang Chen * Shundo Kishi +* Stefan Van de Mosselaer * Stephan Pleines +* Tiffany Chen +* Trushant Adeshara +* Tyler Lum +* Victor Khaustov +* Virgilio Gรณmez Lambo * Vladimir Fokow * Wei Yang +* Welf Rehberg * Xavier Nal +* Xiaodi Yuan +* Xinjie Yao +* Xinpeng Liu * Yang Jin +* Yanzi Zhu +* Yijie Guo +* Yohan Choi * Yujian Zhang -* Zhengyu Zhang +* Yun Liu +* Zehao Wang +* Zijian Li * Ziqi Fan +* Zoe McCarthy +* David Leon +* Song Yi +* Weihua Zhang +* Tsz Ki GAO +* Anke Zhao ## Acknowledgements @@ -106,4 +184,5 @@ Guidelines for modifications: * Gavriel State * Hammad Mazhar * Marco Hutter +* Yan Chang * Yashraj Narang diff --git a/LICENSE b/LICENSE index b0f7751b6cef..dee9ba551f42 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright (c) 2022-2025, The Isaac Lab Project Developers. +Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). All rights reserved. diff --git a/README.md b/README.md index 936823f98067..05c79619d667 100644 --- a/README.md +++ b/README.md @@ -4,9 +4,9 @@ # Isaac Lab -[![IsaacSim](https://img.shields.io/badge/IsaacSim-4.5.0-silver.svg)](https://docs.isaacsim.omniverse.nvidia.com/latest/index.html) -[![Python](https://img.shields.io/badge/python-3.10-blue.svg)](https://docs.python.org/3/whatsnew/3.10.html) -[![Linux platform](https://img.shields.io/badge/platform-linux--64-orange.svg)](https://releases.ubuntu.com/20.04/) +[![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/) [![pre-commit](https://img.shields.io/github/actions/workflow/status/isaac-sim/IsaacLab/pre-commit.yaml?logo=pre-commit&logoColor=white&label=pre-commit&color=brightgreen)](https://github.com/isaac-sim/IsaacLab/actions/workflows/pre-commit.yaml) [![docs status](https://img.shields.io/github/actions/workflow/status/isaac-sim/IsaacLab/docs.yaml?label=docs&color=brightgreen)](https://github.com/isaac-sim/IsaacLab/actions/workflows/docs.yaml) @@ -14,14 +14,23 @@ [![License](https://img.shields.io/badge/license-Apache--2.0-yellow.svg)](https://opensource.org/license/apache-2-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 transfer in robotics. +**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 +transfer in robotics. -Isaac Lab provides developers with a range of essential features for accurate sensor simulation, such as RTX-based cameras, LIDAR, or contact sensors. The framework's GPU acceleration enables users to run complex simulations and computations faster, which is key for iterative processes like reinforcement learning and data-intensive tasks. Moreover, Isaac Lab can run locally or be distributed across the cloud, offering flexibility for large-scale deployments. +Isaac Lab provides developers with a range of essential features for accurate sensor simulation, such as RTX-based +cameras, LIDAR, or contact sensors. The framework's GPU acceleration enables users to run complex simulations and +computations faster, which is key for iterative processes like reinforcement learning and data-intensive tasks. +Moreover, Isaac Lab can run locally or be distributed across the cloud, offering flexibility for large-scale deployments. + +A detailed description of Isaac Lab can be found in our [arXiv paper](https://arxiv.org/abs/2511.04831). ## Key Features Isaac Lab offers a comprehensive set of tools and environments designed to facilitate robot learning: -- **Robots**: A diverse collection of robots, from manipulators, quadrupeds, to humanoids, with 16 commonly available models. + +- **Robots**: A diverse collection of robots, from manipulators, quadrupeds, to humanoids, with more than 16 commonly available models. - **Environments**: Ready-to-train implementations of more than 30 environments, which can be trained with popular reinforcement learning frameworks such as RSL RL, SKRL, RL Games, or Stable Baselines. We also support multi-agent reinforcement learning. - **Physics**: Rigid bodies, articulated systems, deformable objects - **Sensors**: RGB/depth/segmentation cameras, camera annotations, IMU, contact sensors, ray casters. @@ -29,7 +38,59 @@ Isaac Lab offers a comprehensive set of tools and environments designed to facil ## Getting Started -Our [documentation page](https://isaac-sim.github.io/IsaacLab) provides everything you need to get started, including detailed tutorials and step-by-step guides. Follow these links to learn more about: +### WatCloud + TurboVNC + +For WatCloud / Slurm usage, the simplest interactive workflow is: + +1. Install TurboVNC Viewer on your local machine. +2. Start your Slurm dev session so `asd-dev-session` lands on the active compute node. +3. Forward local port `5900` to the Slurm node through `asd-dev-session`: `ssh -L 5900:localhost:5901 asd-dev-session` +4. Connect TurboVNC Viewer to `localhost:5900`. + +Minimal commands on the Slurm node: + +```bash +cd ~/IsaacLab +./docker/container.py build +./docker/container.py start +./docker/container.py enter base +``` + +Inside the container, run Isaac Lab as usual: + +```bash +./isaaclab.sh -s +``` + +or run a script: + +```bash +./isaaclab.sh -p +``` + +On your local machine, open the SSH tunnel: + +```bash +ssh -L 5900:localhost:5900 asd-dev-session +``` + +Then open TurboVNC Viewer and connect to: + +```text +localhost:5900 +``` + +Notes: + +- The container now starts TurboVNC automatically. +- The default remote desktop resolution is `1280x720`. +- If the viewer connects but Isaac is not visible yet, enter the container and start Isaac using `./isaaclab.sh -s`. +- If you change Docker or VNC startup files, rebuild the image before restarting the container. + +### Documentation + +Our [documentation page](https://isaac-sim.github.io/IsaacLab) provides everything you need to get started, including +detailed tutorials and step-by-step guides. Follow these links to learn more about: - [Installation steps](https://isaac-sim.github.io/IsaacLab/main/source/setup/installation/index.html#local-installation) - [Reinforcement learning](https://isaac-sim.github.io/IsaacLab/main/source/overview/reinforcement-learning/rl_existing_scripts.html) @@ -37,6 +98,21 @@ Our [documentation page](https://isaac-sim.github.io/IsaacLab) provides everythi - [Available environments](https://isaac-sim.github.io/IsaacLab/main/source/overview/environments.html) +## Isaac Sim Version Dependency + +Isaac Lab is built on top of Isaac Sim and requires specific versions of Isaac Sim that are compatible with each +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 / 5.1 | +| `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 We wholeheartedly welcome contributions from the community to make this framework mature and useful for everyone. @@ -45,8 +121,8 @@ These may happen as bug reports, feature requests, or code contributions. For de ## Show & Tell: Share Your Inspiration -We encourage you to utilize our [Show & Tell](https://github.com/isaac-sim/IsaacLab/discussions/categories/show-and-tell) area in the -`Discussions` section of this repository. This space is designed for you to: +We encourage you to utilize our [Show & Tell](https://github.com/isaac-sim/IsaacLab/discussions/categories/show-and-tell) +area in the `Discussions` section of this repository. This space is designed for you to: * Share the tutorials you've created * Showcase your learning content @@ -61,40 +137,54 @@ innovation in robotics and simulation. Please see the [troubleshooting](https://isaac-sim.github.io/IsaacLab/main/source/refs/troubleshooting.html) section for common fixes or [submit an issue](https://github.com/isaac-sim/IsaacLab/issues). -For issues related to Isaac Sim, we recommend checking its [documentation](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html) +For issues related to Isaac Sim, we recommend checking its [documentation](https://docs.isaacsim.omniverse.nvidia.com/latest/index.html) or opening a question on its [forums](https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/67). ## Support -* Please use GitHub [Discussions](https://github.com/isaac-sim/IsaacLab/discussions) for discussing ideas, asking questions, and requests for new features. -* Github [Issues](https://github.com/isaac-sim/IsaacLab/issues) should only be used to track executable pieces of work with a definite scope and a clear deliverable. These can be fixing bugs, documentation issues, new features, or general updates. +* Please use GitHub [Discussions](https://github.com/isaac-sim/IsaacLab/discussions) for discussing ideas, + asking questions, and requests for new features. +* Github [Issues](https://github.com/isaac-sim/IsaacLab/issues) should only be used to track executable pieces of + work with a definite scope and a clear deliverable. These can be fixing bugs, documentation issues, new features, + or general updates. ## Connect with the NVIDIA Omniverse Community -Have a project or resource you'd like to share more widely? We'd love to hear from you! Reach out to the -NVIDIA Omniverse Community team at OmniverseCommunity@nvidia.com to discuss potential opportunities -for broader dissemination of your work. +Do you have a project or resource you'd like to share more widely? We'd love to hear from you! +Reach out to the NVIDIA Omniverse Community team at OmniverseCommunity@nvidia.com to explore opportunities +to spotlight your work. -Join us in building a vibrant, collaborative ecosystem where creativity and technology intersect. Your -contributions can make a significant impact on the Isaac Lab community and beyond! +You can also join the conversation on the [Omniverse Discord](https://discord.com/invite/nvidiaomniverse) to +connect with other developers, share your projects, and help grow a vibrant, collaborative ecosystem +where creativity and technology intersect. Your contributions can make a meaningful impact on the Isaac Lab +community and beyond! ## License -The Isaac Lab framework is released under [BSD-3 License](LICENSE). The `isaaclab_mimic` extension and its corresponding standalone scripts are released under [Apache 2.0](LICENSE-mimic). The license files of its dependencies and assets are present in the [`docs/licenses`](docs/licenses) directory. +The Isaac Lab framework is released under [BSD-3 License](LICENSE). The `isaaclab_mimic` extension and its +corresponding standalone scripts are released under [Apache 2.0](LICENSE-mimic). The license files of its +dependencies and assets are present in the [`docs/licenses`](docs/licenses) directory. + +Note that Isaac Lab requires Isaac Sim, which includes components under proprietary licensing terms. Please see the [Isaac Sim license](docs/licenses/dependencies/isaacsim-license.txt) for information on Isaac Sim licensing. + +Note that the `isaaclab_mimic` extension requires cuRobo, which has proprietary licensing terms that can be found in [`docs/licenses/dependencies/cuRobo-license.txt`](docs/licenses/dependencies/cuRobo-license.txt). -## Acknowledgement -Isaac Lab development initiated from the [Orbit](https://isaac-orbit.github.io/) framework. We would appreciate if you would cite it in academic publications as well: +## Citation + +If you use Isaac Lab in your research, please cite the technical report: ``` -@article{mittal2023orbit, - author={Mittal, Mayank and Yu, Calvin and Yu, Qinxi and Liu, Jingzhou and Rudin, Nikita and Hoeller, David and Yuan, Jia Lin and Singh, Ritvik and Guo, Yunrong and Mazhar, Hammad and Mandlekar, Ajay and Babich, Buck and State, Gavriel and Hutter, Marco and Garg, Animesh}, - journal={IEEE Robotics and Automation Letters}, - title={Orbit: A Unified Simulation Framework for Interactive Robot Learning Environments}, - year={2023}, - volume={8}, - number={6}, - pages={3740-3747}, - doi={10.1109/LRA.2023.3270034} +@article{mittal2025isaaclab, + title={Isaac Lab: A GPU-Accelerated Simulation Framework for Multi-Modal Robot Learning}, + author={Mayank Mittal and Pascal Roth and James Tigue and Antoine Richard and Octi Zhang and Peter Du and Antonio Serrano-Muรฑoz and Xinjie Yao and Renรฉ Zurbrรผgg and Nikita Rudin and Lukasz Wawrzyniak and Milad Rakhsha and Alain Denzler and Eric Heiden and Ales Borovicka and Ossama Ahmed and Iretiayo Akinola and Abrar Anwar and Mark T. Carlson and Ji Yuan Feng and Animesh Garg and Renato Gasoto and Lionel Gulich and Yijie Guo and M. Gussert and Alex Hansen and Mihir Kulkarni and Chenran Li and Wei Liu and Viktor Makoviychuk and Grzegorz Malczyk and Hammad Mazhar and Masoud Moghani and Adithyavairavan Murali and Michael Noseworthy and Alexander Poddubny and Nathan Ratliff and Welf Rehberg and Clemens Schwarke and Ritvik Singh and James Latham Smith and Bingjie Tang and Ruchik Thaker and Matthew Trepte and Karl Van Wyk and Fangzhou Yu and Alex Millane and Vikram Ramasamy and Remo Steiner and Sangeeta Subramanian and Clemens Volk and CY Chen and Neel Jawale and Ashwin Varghese Kuruttukulam and Michael A. Lin and Ajay Mandlekar and Karsten Patzwaldt and John Welsh and Huihua Zhao and Fatima Anes and Jean-Francois Lafleche and Nicolas Moรซnne-Loccoz and Soowan Park and Rob Stepinski and Dirk Van Gelder and Chris Amevor and Jan Carius and Jumyung Chang and Anka He Chen and Pablo de Heras Ciechomski and Gilles Daviet and Mohammad Mohajerani and Julia von Muralt and Viktor Reutskyy and Michael Sauter and Simon Schirm and Eric L. Shi and Pierre Terdiman and Kenny Vilella and Tobias Widmer and Gordon Yeoman and Tiffany Chen and Sergey Grizan and Cathy Li and Lotus Li and Connor Smith and Rafael Wiltz and Kostas Alexis and Yan Chang and David Chu and Linxi "Jim" Fan and Farbod Farshidian and Ankur Handa and Spencer Huang and Marco Hutter and Yashraj Narang and Soha Pouya and Shiwei Sheng and Yuke Zhu and Miles Macklin and Adam Moravanszky and Philipp Reist and Yunrong Guo and David Hoeller and Gavriel State}, + journal={arXiv preprint arXiv:2511.04831}, + year={2025}, + url={https://arxiv.org/abs/2511.04831} } ``` + +## Acknowledgement + +Isaac Lab development initiated from the [Orbit](https://isaac-orbit.github.io/) framework. +We gratefully acknowledge the authors of Orbit for their foundational contributions. diff --git a/VERSION b/VERSION index e9307ca5751b..f90b1afc082f 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.2 +2.3.2 diff --git a/apps/isaaclab.python.headless.kit b/apps/isaaclab.python.headless.kit index 9470e97c15d6..03a6bf196d01 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.0.2" +version = "2.3.2" # 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 = "4.5.0" +app.version = "5.1.0" ################################## # Omniverse related dependencies # @@ -28,6 +28,10 @@ app.version = "4.5.0" "usdrt.scenegraph" = {} "omni.kit.telemetry" = {} "omni.kit.loop" = {} +# this is needed to create physics material through CreatePreviewSurfaceMaterialPrim +"omni.kit.usd.mdl" = {} +# this is used for converting assets that have the wrong units +"omni.usd.metrics.assembler.ui" = {} [settings] app.content.emptyStageOnStart = false @@ -69,6 +73,12 @@ app.hydraEngine.waitIdle = false # app.hydra.aperture.conform = 4 # in 105.1 pixels are square by default omni.replicator.asyncRendering = false +### FSD +# this .kit file is used for headless, no-rendering cases. There won't be a scene delegate +# created, but setting useFSD to false here is done to not do full fabric population, but +# instead to minimal population +app.useFabricSceneDelegate = false + # Enable Iray and pxr by setting this to "rtx,iray,pxr" renderer.enabled = "rtx" @@ -78,6 +88,12 @@ app.audio.enabled = false # Enable Vulkan - avoids torch+cu12 error on windows app.vulkan = true +# Set profiler backend to NVTX by default +app.profilerBackend = "nvtx" + +# Disables rate limit in runloop +app.runLoops.main.rateLimitEnabled=false + # hide NonToggleable Exts exts."omni.kit.window.extensions".hideNonToggleableExts = true exts."omni.kit.window.extensions".showFeatureOnly = false @@ -85,11 +101,14 @@ exts."omni.kit.window.extensions".showFeatureOnly = false # set the default ros bridge to disable on startup isaac.startup.ros_bridge_extension = "" +# disable the metrics assembler change listener, we don't want to do any runtime changes +metricsAssembler.changeListenerEnabled = false + # Extensions ############################### [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 +164,8 @@ fabricUpdateTransformations = false fabricUpdateVelocities = false fabricUpdateForceSensors = false fabricUpdateJointStates = false +### When Direct GPU mode is enabled (suppressReadback=true) use direct interop between PhysX GPU and Fabric +fabricUseGPUInterop = true # Performance improvement resourcemonitor.timeBetweenQueries = 100 @@ -194,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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +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 ddb7000b96c0..e11b89ec8367 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.0.2" +version = "2.3.2" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] @@ -32,7 +32,12 @@ cameras_enabled = true app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "4.5.0" +app.version = "5.1.0" + +### FSD +app.useFabricSceneDelegate = true +# Temporary, should be enabled by default in Kit soon +rtx.hydra.readTransformsFromFabricInRenderDelegate = true # Disable print outs on extension startup information # this only disables the app print_and_log function @@ -45,12 +50,16 @@ isaac.startup.ros_bridge_extension = "" # Disabling these settings reduces renderer VRAM usage and improves rendering performance, but at some quality cost rtx.translucency.enabled = false rtx.reflections.enabled = false +rtx.reflections.denoiser.enabled = false rtx.indirectDiffuse.enabled = false +rtx.indirectDiffuse.denoiser.enabled = false rtx-transient.dlssg.enabled = false -rtx.directLighting.sampledLighting.enabled = true -rtx.directLighting.sampledLighting.samplesPerPixel = 1 +rtx-transient.dldenoiser.enabled = false +rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = false +rtx.raytracing.subpixel.mode = 0 rtx.sceneDb.ambientLightIntensity = 1.0 -# rtx.shadows.enabled = false +rtx.shadows.enabled = false # Avoids replicator warning rtx.pathtracing.maxSamplesPerLaunch = 1000000 @@ -78,17 +87,29 @@ app.updateOrder.checkForHydraRenderComplete = 1000 app.renderer.waitIdle=true app.hydraEngine.waitIdle=true +# Forces serial processing for omni graph to avoid NCCL timeout hangs in distributed training +app.execution.debug.forceSerial = true + app.audio.enabled = false # Enable Vulkan - avoids torch+cu12 error on windows app.vulkan = true +# Set profiler backend to NVTX by default +app.profilerBackend = "nvtx" + +# Disables rate limit in runloop +app.runLoops.main.rateLimitEnabled=false + # disable replicator orchestrator for better runtime perf exts."omni.replicator.core".Orchestrator.enabled = false +# disable the metrics assembler change listener, we don't want to do any runtime changes +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" }, ] @@ -99,6 +120,8 @@ interceptSysStdOutput = false logSysStdOutput = false [settings.app.renderer] +resolution.height = 720 +resolution.width = 1280 skipWhileMinimized = false sleepMsOnFocus = 0 sleepMsOutOfFocus = 0 @@ -115,6 +138,8 @@ fabricUpdateTransformations = false fabricUpdateVelocities = false fabricUpdateForceSensors = false fabricUpdateJointStates = false +### When Direct GPU mode is enabled (suppressReadback=true) use direct interop between PhysX GPU and Fabric +fabricUseGPUInterop = true # Register extension folder from this repo in kit [settings.app.exts] @@ -137,6 +162,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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +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 f677c9f72d8a..5ab46465657f 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.0.2" +version = "2.3.2" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd"] @@ -24,12 +24,9 @@ keywords = ["experience", "app", "usd"] "isaacsim.gui.menu" = {} "isaacsim.gui.property" = {} "isaacsim.replicator.behavior" = {} -"isaacsim.robot_motion.lula" = {} -"isaacsim.robot_motion.motion_generation" = {} "isaacsim.robot.manipulators" = {} "isaacsim.robot.policy.examples" = {} "isaacsim.robot.schema" = {} -"isaacsim.robot.surface_gripper" = {} "isaacsim.robot.wheeled_robots" = {} "isaacsim.sensors.camera" = {} "isaacsim.sensors.physics" = {} @@ -41,7 +38,7 @@ keywords = ["experience", "app", "usd"] # Isaac Sim Extra "isaacsim.asset.importer.mjcf" = {} -"isaacsim.asset.importer.urdf" = {} +"isaacsim.asset.importer.urdf" = {version = "2.4.31", exact = true} "omni.physx.bundle" = {} "omni.physx.tensors" = {} "omni.replicator.core" = {} @@ -59,7 +56,6 @@ keywords = ["experience", "app", "usd"] "omni.graph.ui_nodes" = {} "omni.hydra.engine.stats" = {} "omni.hydra.rtx" = {} -"omni.kit.loop" = {} "omni.kit.mainwindow" = {} "omni.kit.manipulator.camera" = {} "omni.kit.manipulator.prim" = {} @@ -67,15 +63,13 @@ keywords = ["experience", "app", "usd"] "omni.kit.material.library" = {} "omni.kit.menu.common" = { order = 1000 } "omni.kit.menu.create" = {} -"omni.kit.menu.edit" = {} -"omni.kit.menu.file" = {} "omni.kit.menu.stage" = {} "omni.kit.menu.utils" = {} "omni.kit.primitive.mesh" = {} "omni.kit.property.bundle" = {} "omni.kit.raycast.query" = {} -"omni.kit.stage_template.core" = {} "omni.kit.stagerecorder.bundle" = {} +"omni.kit.stage_template.core" = {} "omni.kit.telemetry" = {} "omni.kit.tool.asset_importer" = {} "omni.kit.tool.collect" = {} @@ -90,10 +84,11 @@ keywords = ["experience", "app", "usd"] "omni.kit.window.console" = {} "omni.kit.window.content_browser" = {} "omni.kit.window.property" = {} +"omni.kit.window.script_editor" = {} "omni.kit.window.stage" = {} "omni.kit.window.status_bar" = {} "omni.kit.window.toolbar" = {} -"omni.physx.stageupdate" = {} +"omni.physics.stageupdate" = {} "omni.rtx.settings.core" = {} "omni.uiaudio" = {} "omni.usd.metrics.assembler.ui" = {} @@ -119,7 +114,7 @@ exts."omni.kit.material.library".ui_show_list = [ "USD Preview Surface", ] exts."omni.kit.renderer.core".present.enabled = false # Fixes MGPU stability issue -exts."omni.kit.viewport.window".windowMenu.entryCount = 2 # Allow user to create two viewports by default +exts."omni.kit.viewport.window".windowMenu.entryCount = 1 # Keep a single viewport for lighter interactive sessions exts."omni.kit.viewport.window".windowMenu.label = "" # Put Viewport menuitem under Window menu exts."omni.rtx.window.settings".window_menu = "Window" # Where to put the render settings menuitem exts."omni.usd".locking.onClose = false # reduce time it takes to close/create stage @@ -127,11 +122,29 @@ renderer.asyncInit = true # Don't block while renderer inits renderer.gpuEnumeration.glInterop.enabled = false # Improves startup speed. rendergraph.mgpu.backend = "copyQueue" # In MGPU configurations, This setting can be removed if IOMMU is disabled for better performance, copyQueue improves stability and performance when IOMMU is enabled rtx-transient.dlssg.enabled = false # DLSSG frame generation is not compatible with synthetic data generation -rtx.hydra.mdlMaterialWarmup = true # start loading the MDL shaders needed before any delegate is actually created. +rtx-transient.dldenoiser.enabled = false +rtx.hydra.mdlMaterialWarmup = false # avoid front-loading shader warmup for remote interactive sessions +rtx.translucency.enabled = false +rtx.reflections.enabled = false +rtx.reflections.denoiser.enabled = false +rtx.indirectDiffuse.enabled = false +rtx.indirectDiffuse.denoiser.enabled = false +rtx.ambientOcclusion.enabled = false +rtx.ambientOcclusion.denoiserMode = 1 +rtx.directLighting.sampledLighting.enabled = false +rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.raytracing.subpixel.mode = 0 +rtx.raytracing.cached.enabled = false +rtx.post.dlss.execMode = 0 # performance +rtx.shadows.enabled = false +renderer.multiGpu.maxGpuCount = 1 omni.replicator.asyncRendering = false # Async rendering must be disabled for SDG exts."omni.kit.test".includeTests = ["*isaac*"] # Add isaac tests to test runner foundation.verifyOsVersion.enabled = false +# disable the metrics assembler change listener, we don't want to do any runtime changes +metricsAssembler.changeListenerEnabled = false + # set the default ros bridge to disable on startup isaac.startup.ros_bridge_extension = "" @@ -163,14 +176,14 @@ show_menu_titles = true [settings.app] name = "Isaac-Sim" -version = "4.5.0" +version = "5.1.0" versionFile = "${exe-path}/VERSION" content.emptyStageOnStart = true fastShutdown = true file.ignoreUnsavedOnExit = true font.file = "${fonts}/OpenSans-SemiBold.ttf" font.size = 16 -gatherRenderResults = true # True to prevent artifacts in multiple viewport configurations, can be set to false for better performance in some cases +gatherRenderResults = false # prefer lower latency over multi-viewport correctness for remote use hangDetector.enabled = true hangDetector.timeout = 120 player.useFixedTimeStepping = true @@ -203,8 +216,8 @@ defaultCamPos.x = 5 defaultCamPos.y = 5 defaultCamPos.z = 5 defaults.fillViewport = false # default to not fill viewport -grid.enabled = true -outline.enabled = true +grid.enabled = false +outline.enabled = false boundingBoxes.enabled = false show.camera=false show.lights=false @@ -243,6 +256,10 @@ omni.replicator.asyncRendering = false app.asyncRendering = false app.asyncRenderingLowLatency = false +### FSD +app.useFabricSceneDelegate = true +rtx.hydra.readTransformsFromFabricInRenderDelegate = true + # disable replicator orchestrator for better runtime perf exts."omni.replicator.core".Orchestrator.enabled = false @@ -253,7 +270,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" }, ] @@ -293,11 +310,13 @@ fabricUpdateTransformations = false fabricUpdateVelocities = false fabricUpdateForceSensors = false fabricUpdateJointStates = false +### When Direct GPU mode is enabled (suppressReadback=true) use direct interop between PhysX GPU and Fabric +fabricUseGPUInterop = true # Asset path # 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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +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 1c52b38fd780..de7a559a3895 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.0.2" +version = "2.3.2" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] @@ -33,7 +33,12 @@ cameras_enabled = true app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "4.5.0" +app.version = "5.1.0" + +### FSD +app.useFabricSceneDelegate = true +# Temporary, should be enabled by default in Kit soon +rtx.hydra.readTransformsFromFabricInRenderDelegate = true # Disable print outs on extension startup information # this only disables the app print_and_log function @@ -46,12 +51,16 @@ isaac.startup.ros_bridge_extension = "" # Disabling these settings reduces renderer VRAM usage and improves rendering performance, but at some quality cost rtx.translucency.enabled = false rtx.reflections.enabled = false +rtx.reflections.denoiser.enabled = false rtx.indirectDiffuse.enabled = false +rtx.indirectDiffuse.denoiser.enabled = false rtx-transient.dlssg.enabled = false -rtx.directLighting.sampledLighting.enabled = true -rtx.directLighting.sampledLighting.samplesPerPixel = 1 +rtx-transient.dldenoiser.enabled = false +rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = false +rtx.raytracing.subpixel.mode = 0 rtx.sceneDb.ambientLightIntensity = 1.0 -# rtx.shadows.enabled = false +rtx.shadows.enabled = false # Avoids replicator warning rtx.pathtracing.maxSamplesPerLaunch = 1000000 @@ -84,6 +93,9 @@ app.audio.enabled = false # disable replicator orchestrator for better runtime perf exts."omni.replicator.core".Orchestrator.enabled = false +# disable the metrics assembler change listener, we don't want to do any runtime changes +metricsAssembler.changeListenerEnabled = false + [settings.physics] updateToUsd = false updateParticlesToUsd = false @@ -96,10 +108,12 @@ fabricUpdateTransformations = false fabricUpdateVelocities = false fabricUpdateForceSensors = false fabricUpdateJointStates = false +### When Direct GPU mode is enabled (suppressReadback=true) use direct interop between PhysX GPU and Fabric +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" }, ] @@ -110,6 +124,8 @@ interceptSysStdOutput = false logSysStdOutput = false [settings.app.renderer] +resolution.height = 720 +resolution.width = 1280 skipWhileMinimized = false sleepMsOnFocus = 0 sleepMsOutOfFocus = 0 @@ -135,6 +151,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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +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 new file mode 100644 index 000000000000..57b78c1b2907 --- /dev/null +++ b/apps/isaaclab.python.xr.openxr.headless.kit @@ -0,0 +1,64 @@ +## +# Adapted from: apps/isaaclab.python.xr.openxr.kit +## + +[package] +title = "Isaac Lab Python OpenXR Headless" +description = "An app for running Isaac Lab with OpenXR in headless mode" +version = "2.3.2" + +# That makes it browsable in UI with "experience" filter +keywords = ["experience", "app", "usd", "headless"] + +[settings] +# Note: This path was adapted to be respective to the kit-exe file location +app.versionFile = "${exe-path}/VERSION" +app.folder = "${exe-path}/" +app.name = "Isaac-Sim" +app.version = "5.1.0" + +### FSD +app.useFabricSceneDelegate = true +# Temporary, should be enabled by default in Kit soon +rtx.hydra.readTransformsFromFabricInRenderDelegate = true + +# 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 +cameras_enabled = true + +[dependencies] +"isaaclab.python.xr.openxr" = {} + +[settings] +xr.profile.ar.enabled = true + +[settings.app.python] +# These disable the kit app from also printing out python output, which gets confusing +interceptSysStdOutput = false +logSysStdOutput = false + +# Register extension folder from this repo in kit +[settings.app.exts] +folders = [ + "${exe-path}/exts", # kit extensions + "${exe-path}/extscore", # kit core extensions + "${exe-path}/../exts", # isaac extensions + "${exe-path}/../extsDeprecated", # deprecated isaac extensions + "${exe-path}/../extscache", # isaac cache extensions + "${exe-path}/../extsPhysics", # isaac physics extensions + "${exe-path}/../isaacsim/exts", # isaac extensions for pip + "${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions + "${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip + "${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip + "${app}", # needed to find other app files + "${app}/../source", # needed to find extensions in Isaac Lab +] + +[settings] +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 f9b7cad8005f..8fe9980e50b6 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.0.2" +version = "2.3.2" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd"] @@ -15,30 +15,57 @@ keywords = ["experience", "app", "usd"] app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "4.5.0" +app.version = "5.1.0" ### async rendering settings -omni.replicator.asyncRendering = true +# omni.replicator.asyncRendering needs to be false for external camera rendering +omni.replicator.asyncRendering = false app.asyncRendering = true app.asyncRenderingLowLatency = true # For XR, set this back to default "#define OMNI_MAX_DEVICE_GROUP_DEVICE_COUNT 16" renderer.multiGpu.maxGpuCount = 16 +renderer.gpuEnumeration.glInterop.enabled = true # Allow Kit XR OpenXR to render headless + +### FSD +app.useFabricSceneDelegate = true +# Temporary, should be enabled by default in Kit soon +rtx.hydra.readTransformsFromFabricInRenderDelegate = true + +# xr optimizations +xr.skipInputDeviceUSDWrites = true +'rtx-transient'.resourcemanager.enableTextureStreaming = false [dependencies] -"isaaclab.python.rendering" = {} -"isaacsim.xr.openxr" = {} +"isaaclab.python" = {} # Kit extensions "omni.kit.xr.system.openxr" = {} "omni.kit.xr.profile.ar" = {} +[settings.isaaclab] +# This is used to check that this experience file is loaded when using cameras +cameras_enabled = true + [settings] +app.xr.enabled = true +# Set profiler backend to NVTX by default +app.profilerBackend = "nvtx" + # xr settings xr.ui.enabled = false -xrstage.profile.ar.anchorMode = "active camera" xr.depth.aov = "GBufferDepth" -defaults.xr.profile.ar.renderQuality = "off" +defaults.xr.profile.ar.anchorMode = "custom anchor" +rtx.rendermode = "RaytracedLighting" +persistent.xr.profile.ar.renderQuality = "performance" +persistent.xr.profile.ar.render.nearPlane = 0.15 +xr.openxr.components."omni.kit.xr.openxr.ext.hand_tracking".enabled = true +xr.openxr.components."isaacsim.xr.openxr.hand_tracking".enabled = true + +[settings.app.python] +# These disable the kit app from also printing out python output, which gets confusing +interceptSysStdOutput = false +logSysStdOutput = false # Register extension folder from this repo in kit [settings.app.exts] @@ -61,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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" -persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +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/extension.toml b/apps/isaacsim_4_5/extension.toml new file mode 100644 index 000000000000..f0668b9184d1 --- /dev/null +++ b/apps/isaacsim_4_5/extension.toml @@ -0,0 +1 @@ +# This is not an extension diff --git a/apps/isaacsim_4_5/isaaclab.python.headless.kit b/apps/isaacsim_4_5/isaaclab.python.headless.kit new file mode 100644 index 000000000000..0fb9eaeffff3 --- /dev/null +++ b/apps/isaacsim_4_5/isaaclab.python.headless.kit @@ -0,0 +1,202 @@ +## +# Adapted from: _isaac_sim/apps/omni.isaac.sim.python.gym.headless.kit +## + +[package] +title = "Isaac Lab Python Headless" +description = "An app for running Isaac Lab headlessly" +version = "2.3.2" + +# That makes it browsable in UI with "experience" filter +keywords = ["experience", "app", "isaaclab", "python", "headless"] + +[settings] +# Note: This path was adapted to be respective to the kit-exe file location +app.versionFile = "${exe-path}/VERSION" +app.folder = "${exe-path}/" +app.name = "Isaac-Sim" +app.version = "4.5.0" + +################################## +# Omniverse related dependencies # +################################## +[dependencies] +"omni.physx" = {} +"omni.physx.tensors" = {} +"omni.physx.fabric" = {} +"omni.warp.core" = {} +"usdrt.scenegraph" = {} +"omni.kit.telemetry" = {} +"omni.kit.loop" = {} + +[settings] +app.content.emptyStageOnStart = false + +# Disable print outs on extension startup information +# this only disables the app print_and_log function +app.enableStdoutOutput = false + +# default viewport is fill +app.runLoops.rendering_0.fillResolution = false +exts."omni.kit.window.viewport".blockingGetViewportDrawable = false + +# Fix PlayButtonGroup error +exts."omni.kit.widget.toolbar".PlayButton.enabled = false + +# disable replicator orchestrator for better runtime perf +exts."omni.replicator.core".Orchestrator.enabled = false + +[settings.app.settings] +persistent = true +dev_build = false +fabricDefaultStageFrameHistoryCount = 3 # needed for omni.syntheticdata TODO105 still true? + +[settings.app.python] +# These disable the kit app from also printing out python output, which gets confusing +interceptSysStdOutput = false +logSysStdOutput = false + +[settings] +# MGPU is always on, you can turn it from the settings, and force this off to save even more resource if you +# only want to use a single GPU on your MGPU system +# False for Isaac Sim +renderer.multiGpu.enabled = true +renderer.multiGpu.autoEnable = true +'rtx-transient'.resourcemanager.enableTextureStreaming = true +app.asyncRendering = false +app.asyncRenderingLowLatency = false +app.hydraEngine.waitIdle = false +# app.hydra.aperture.conform = 4 # in 105.1 pixels are square by default +omni.replicator.asyncRendering = false + +# Enable Iray and pxr by setting this to "rtx,iray,pxr" +renderer.enabled = "rtx" + +# Avoid warning on shutdown from audio context +app.audio.enabled = false + +# Enable Vulkan - avoids torch+cu12 error on windows +app.vulkan = true + +# Set profiler backend to NVTX by default +app.profilerBackend = "nvtx" + +# hide NonToggleable Exts +exts."omni.kit.window.extensions".hideNonToggleableExts = true +exts."omni.kit.window.extensions".showFeatureOnly = false + +# set the default ros bridge to disable on startup +isaac.startup.ros_bridge_extension = "" + +# Extensions +############################### +[settings.exts."omni.kit.registry.nucleus"] +registries = [ + { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/106/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" }, +] + +[settings.app.extensions] +skipPublishVerification = false +registryEnabled = true + +[settings.crashreporter.data] +experience = "Isaac Sim" + +[settings.persistent] +app.file.recentFiles = [] +app.stage.upAxis = "Z" +app.stage.movePrimInPlace = false +app.stage.instanceableOnCreatingReference = false +app.stage.materialStrength = "weakerThanDescendants" + +app.transform.gizmoUseSRT = true +app.viewport.grid.scale = 1.0 +app.viewport.pickingMode = "kind:model.ALL" +app.viewport.camMoveVelocity = 0.05 # 5 m/s +app.viewport.gizmo.scale = 0.01 # scaled to meters +app.viewport.previewOnPeek = false +app.viewport.snapToSurface = false +app.viewport.displayOptions = 31951 # Disable Frame Rate and Resolution by default +app.window.uiStyle = "NvidiaDark" +app.primCreation.DefaultXformOpType = "Scale, Orient, Translate" +app.primCreation.DefaultXformOpOrder="xformOp:translate, xformOp:orient, xformOp:scale" +app.primCreation.typedDefaults.camera.clippingRange = [0.01, 10000000.0] +simulation.minFrameRate = 15 +simulation.defaultMetersPerUnit = 1.0 +omnigraph.updateToUsd = false +omnigraph.useSchemaPrims = true +omnigraph.disablePrimNodes = true +omni.replicator.captureOnPlay = true +omnihydra.useSceneGraphInstancing = true +renderer.startupMessageDisplayed = true # hides the IOMMU popup window + +# Make Detail panel visible by default +app.omniverse.content_browser.options_menu.show_details = true +app.omniverse.filepicker.options_menu.show_details = true + +[settings.physics] +updateToUsd = false +updateParticlesToUsd = false +updateVelocitiesToUsd = false +updateForceSensorsToUsd = false +outputVelocitiesLocalSpace = false +useFastCache = false +visualizationDisplayJoints = false +fabricUpdateTransformations = false +fabricUpdateVelocities = false +fabricUpdateForceSensors = false +fabricUpdateJointStates = false + +# Performance improvement +resourcemonitor.timeBetweenQueries = 100 + +# Register extension folder from this repo in kit +[settings.app.exts] +folders = [ + "${exe-path}/exts", # kit extensions + "${exe-path}/extscore", # kit core extensions + "${exe-path}/../exts", # isaac extensions + "${exe-path}/../extsDeprecated", # deprecated isaac extensions + "${exe-path}/../extscache", # isaac cache extensions + "${exe-path}/../extsPhysics", # isaac physics extensions + "${exe-path}/../isaacsim/exts", # isaac extensions for pip + "${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions + "${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip + "${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip + "${app}", # needed to find other app files + "${app}/../../source", # needed to find extensions in Isaac Lab +] + +[settings.ngx] +enabled=true # Enable this for DLSS + +######################## +# Isaac Sim Extensions # +######################## +[dependencies] +"isaacsim.simulation_app" = {} +"isaacsim.core.api" = {} +"isaacsim.core.cloner" = {} +"isaacsim.core.utils" = {} +"isaacsim.core.version" = {} + +######################## +# Isaac Lab Extensions # +######################## + +# Load Isaac Lab extensions last +"isaaclab" = {order = 1000} +"isaaclab_assets" = {order = 1000} +"isaaclab_tasks" = {order = 1000} +"isaaclab_mimic" = {order = 1000} +"isaaclab_rl" = {order = 1000} + +# Asset path +# 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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" diff --git a/apps/isaacsim_4_5/isaaclab.python.headless.rendering.kit b/apps/isaacsim_4_5/isaaclab.python.headless.rendering.kit new file mode 100644 index 000000000000..10b3efc84bfc --- /dev/null +++ b/apps/isaacsim_4_5/isaaclab.python.headless.rendering.kit @@ -0,0 +1,145 @@ +## +# Adapted from: https://github.com/NVIDIA-Omniverse/OmniIsaacGymEnvs/blob/main/apps/omni.isaac.sim.python.gym.camera.kit +# +# This app file designed specifically towards vision-based RL tasks. It provides necessary settings to enable +# multiple cameras to be rendered each frame. Additional settings are also applied to increase performance when +# rendering cameras across multiple environments. +## + +[package] +title = "Isaac Lab Python Headless Camera" +description = "An app for running Isaac Lab headlessly with rendering enabled" +version = "2.3.2" + +# That makes it browsable in UI with "experience" filter +keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] + +[dependencies] +# Isaac Lab minimal app +"isaaclab.python.headless" = {} +"omni.replicator.core" = {} + +# Rendering +"omni.kit.material.library" = {} +"omni.kit.viewport.rtx" = {} + +[settings.isaaclab] +# This is used to check that this experience file is loaded when using cameras +cameras_enabled = true + +[settings] +# Note: This path was adapted to be respective to the kit-exe file location +app.versionFile = "${exe-path}/VERSION" +app.folder = "${exe-path}/" +app.name = "Isaac-Sim" +app.version = "4.5.0" + +# Disable print outs on extension startup information +# this only disables the app print_and_log function +app.enableStdoutOutput = false + +# set the default ros bridge to disable on startup +isaac.startup.ros_bridge_extension = "" + +# Flags for better rendering performance +# Disabling these settings reduces renderer VRAM usage and improves rendering performance, but at some quality cost +rtx.translucency.enabled = false +rtx.reflections.enabled = false +rtx.indirectDiffuse.enabled = false +rtx-transient.dlssg.enabled = false +rtx.directLighting.sampledLighting.enabled = true +rtx.directLighting.sampledLighting.samplesPerPixel = 1 +rtx.sceneDb.ambientLightIntensity = 1.0 +# rtx.shadows.enabled = false + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 + +# Disable present thread to improve performance +exts."omni.renderer.core".present.enabled=false + +# Disabling these settings reduces renderer VRAM usage and improves rendering performance, but at some quality cost +rtx.raytracing.cached.enabled = false +rtx.ambientOcclusion.enabled = false + +# Set the DLSS model +rtx.post.dlss.execMode = 0 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids unnecessary GPU context initialization +renderer.multiGpu.maxGpuCount=1 + +# Force synchronous rendering to improve training results +omni.replicator.asyncRendering = false + +# Avoids frame offset issue +app.updateOrder.checkForHydraRenderComplete = 1000 +app.renderer.waitIdle=true +app.hydraEngine.waitIdle=true + +app.audio.enabled = false + +# Enable Vulkan - avoids torch+cu12 error on windows +app.vulkan = true + +# Set profiler backend to NVTX by default +app.profilerBackend = "nvtx" + +# disable replicator orchestrator for better runtime perf +exts."omni.replicator.core".Orchestrator.enabled = 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/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" }, +] + +[settings.app.python] +# These disable the kit app from also printing out python output, which gets confusing +interceptSysStdOutput = false +logSysStdOutput = false + +[settings.app.renderer] +skipWhileMinimized = false +sleepMsOnFocus = 0 +sleepMsOutOfFocus = 0 + +[settings.physics] +updateToUsd = false +updateParticlesToUsd = false +updateVelocitiesToUsd = false +updateForceSensorsToUsd = false +outputVelocitiesLocalSpace = false +useFastCache = false +visualizationDisplayJoints = false +fabricUpdateTransformations = false +fabricUpdateVelocities = false +fabricUpdateForceSensors = false +fabricUpdateJointStates = false + +# Register extension folder from this repo in kit +[settings.app.exts] +folders = [ + "${exe-path}/exts", # kit extensions + "${exe-path}/extscore", # kit core extensions + "${exe-path}/../exts", # isaac extensions + "${exe-path}/../extsDeprecated", # deprecated isaac extensions + "${exe-path}/../extscache", # isaac cache extensions + "${exe-path}/../extsPhysics", # isaac physics extensions + "${exe-path}/../isaacsim/exts", # isaac extensions for pip + "${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions + "${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip + "${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip + "${app}", # needed to find other app files + "${app}/../../source", # needed to find extensions in Isaac Lab +] + +# Asset path +# 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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" diff --git a/apps/isaacsim_4_5/isaaclab.python.kit b/apps/isaacsim_4_5/isaaclab.python.kit new file mode 100644 index 000000000000..5fc9b1effd9d --- /dev/null +++ b/apps/isaacsim_4_5/isaaclab.python.kit @@ -0,0 +1,301 @@ +## +# Adapted from: _isaac_sim/apps/isaacsim.exp.base.kit +## + +[package] +title = "Isaac Lab Python" +description = "An app for running Isaac Lab" +version = "2.3.2" + +# That makes it browsable in UI with "experience" filter +keywords = ["experience", "app", "usd"] + +[dependencies] +# Isaac Sim extensions +"isaacsim.app.about" = {} +"isaacsim.asset.browser" = {} +"isaacsim.core.api" = {} +"isaacsim.core.cloner" = {} +"isaacsim.core.nodes" = {} +"isaacsim.core.simulation_manager" = {} +"isaacsim.core.throttling" = {} +"isaacsim.core.utils" = {} +"isaacsim.core.version" = {} +"isaacsim.gui.menu" = {} +"isaacsim.gui.property" = {} +"isaacsim.replicator.behavior" = {} +"isaacsim.robot.manipulators" = {} +"isaacsim.robot.policy.examples" = {} +"isaacsim.robot.schema" = {} +"isaacsim.robot.surface_gripper" = {} +"isaacsim.robot.wheeled_robots" = {} +"isaacsim.sensors.camera" = {} +"isaacsim.sensors.physics" = {} +"isaacsim.sensors.physx" = {} +"isaacsim.sensors.rtx" = {} +"isaacsim.simulation_app" = {} +"isaacsim.storage.native" = {} +"isaacsim.util.debug_draw" = {} + +# Isaac Sim Extra +"isaacsim.asset.importer.mjcf" = {} +"isaacsim.asset.importer.urdf" = {} +"omni.physx.bundle" = {} +"omni.physx.tensors" = {} +"omni.replicator.core" = {} +"omni.replicator.replicator_yaml" = {} +"omni.syntheticdata" = {} +"semantics.schema.editor" = {} +"semantics.schema.property" = {} + +# Kit based editor extensions +"omni.anim.curve.core" = {} +"omni.graph.action" = {} +"omni.graph.core" = {} +"omni.graph.nodes" = {} +"omni.graph.scriptnode" = {} +"omni.graph.ui_nodes" = {} +"omni.hydra.engine.stats" = {} +"omni.hydra.rtx" = {} +"omni.kit.loop" = {} +"omni.kit.mainwindow" = {} +"omni.kit.manipulator.camera" = {} +"omni.kit.manipulator.prim" = {} +"omni.kit.manipulator.selection" = {} +"omni.kit.material.library" = {} +"omni.kit.menu.common" = { order = 1000 } +"omni.kit.menu.create" = {} +"omni.kit.menu.edit" = {} +"omni.kit.menu.file" = {} +"omni.kit.menu.stage" = {} +"omni.kit.menu.utils" = {} +"omni.kit.primitive.mesh" = {} +"omni.kit.property.bundle" = {} +"omni.kit.raycast.query" = {} +"omni.kit.stage_template.core" = {} +"omni.kit.stagerecorder.bundle" = {} +"omni.kit.telemetry" = {} +"omni.kit.tool.asset_importer" = {} +"omni.kit.tool.collect" = {} +"omni.kit.viewport.legacy_gizmos" = {} +"omni.kit.viewport.menubar.camera" = {} +"omni.kit.viewport.menubar.display" = {} +"omni.kit.viewport.menubar.lighting" = {} +"omni.kit.viewport.menubar.render" = {} +"omni.kit.viewport.menubar.settings" = {} +"omni.kit.viewport.scene_camera_model" = {} +"omni.kit.viewport.window" = {} +"omni.kit.window.console" = {} +"omni.kit.window.content_browser" = {} +"omni.kit.window.property" = {} +"omni.kit.window.stage" = {} +"omni.kit.window.status_bar" = {} +"omni.kit.window.toolbar" = {} +"omni.physx.stageupdate" = {} +"omni.rtx.settings.core" = {} +"omni.uiaudio" = {} +"omni.usd.metrics.assembler.ui" = {} +"omni.usd.schema.metrics.assembler" = {} +"omni.warp.core" = {} + +######################## +# Isaac Lab Extensions # +######################## + +# Load Isaac Lab extensions last +"isaaclab" = {order = 1000} +"isaaclab_assets" = {order = 1000} +"isaaclab_tasks" = {order = 1000} +"isaaclab_mimic" = {order = 1000} +"isaaclab_rl" = {order = 1000} + +[settings] +exts."omni.kit.material.library".ui_show_list = [ + "OmniPBR", + "OmniGlass", + "OmniSurface", + "USD Preview Surface", +] +exts."omni.kit.renderer.core".present.enabled = false # Fixes MGPU stability issue +exts."omni.kit.viewport.window".windowMenu.entryCount = 2 # Allow user to create two viewports by default +exts."omni.kit.viewport.window".windowMenu.label = "" # Put Viewport menuitem under Window menu +exts."omni.rtx.window.settings".window_menu = "Window" # Where to put the render settings menuitem +exts."omni.usd".locking.onClose = false # reduce time it takes to close/create stage +renderer.asyncInit = true # Don't block while renderer inits +renderer.gpuEnumeration.glInterop.enabled = false # Improves startup speed. +rendergraph.mgpu.backend = "copyQueue" # In MGPU configurations, This setting can be removed if IOMMU is disabled for better performance, copyQueue improves stability and performance when IOMMU is enabled +rtx-transient.dlssg.enabled = false # DLSSG frame generation is not compatible with synthetic data generation +rtx.hydra.mdlMaterialWarmup = true # start loading the MDL shaders needed before any delegate is actually created. +omni.replicator.asyncRendering = false # Async rendering must be disabled for SDG +exts."omni.kit.test".includeTests = ["*isaac*"] # Add isaac tests to test runner +foundation.verifyOsVersion.enabled = false + +# set the default ros bridge to disable on startup +isaac.startup.ros_bridge_extension = "" + +# Disable for base application +[settings."filter:platform"."windows-x86_64"] +isaac.startup.ros_bridge_extension = "" +[settings."filter:platform"."linux-x86_64"] +isaac.startup.ros_bridge_extension = "" + +# menu styling +[settings.exts."omni.kit.menu.utils"] +logDeprecated = false +margin_size = [18, 3] +tick_spacing = [10, 6] +margin_size_posttick = [0, 3] +separator_size = [14, 10] +root_spacing = 3 +post_label_spaces = 6 +color_tick_enabled = 0xFFFAC434 +color_separator = 0xFF7E7E7E +color_label_enabled = 0xFFEEEEEE +menu_title_color = 0xFF202020 +menu_title_line_color = 0xFF5E5E5E +menu_title_text_color = 0xFF8F8F8F +menu_title_text_height = 24 +menu_title_close_color = 0xFFC6C6C6 +indent_all_ticks = false +show_menu_titles = true + +[settings.app] +name = "Isaac-Sim" +version = "4.5.0" +versionFile = "${exe-path}/VERSION" +content.emptyStageOnStart = true +fastShutdown = true +file.ignoreUnsavedOnExit = true +font.file = "${fonts}/OpenSans-SemiBold.ttf" +font.size = 16 +gatherRenderResults = true # True to prevent artifacts in multiple viewport configurations, can be set to false for better performance in some cases +hangDetector.enabled = true +hangDetector.timeout = 120 +player.useFixedTimeStepping = true +settings.fabricDefaultStageFrameHistoryCount = 3 # needed for omni.syntheticdata TODO105 still true? +settings.persistent = true # settings are persistent for this app + +vulkan = true # Explicitly enable Vulkan (on by default on Linux, off by default on Windows) +### async rendering settings +asyncRendering = false +asyncRenderingLowLatency = false + +[settings.app.window] +iconPath = "${isaacsim.simulation_app}/data/omni.isaac.sim.png" +title = "Isaac Sim" + +[settings.app.python] +# These disable the kit app from also printing out python output, which gets confusing +interceptSysStdOutput = false +logSysStdOutput = false + +[settings.app.renderer] +resolution.height = 720 +resolution.width = 1280 +skipWhileMinimized = false # python app does not throttle +sleepMsOnFocus = 0 # python app does not throttle +sleepMsOutOfFocus = 0 # python app does not throttle + +[settings.app.viewport] +defaultCamPos.x = 5 +defaultCamPos.y = 5 +defaultCamPos.z = 5 +defaults.fillViewport = false # default to not fill viewport +grid.enabled = true +outline.enabled = true +boundingBoxes.enabled = false +show.camera=false +show.lights=false + +[settings.telemetry] +enableAnonymousAppName = true # Anonymous Kit application usage telemetry +enableAnonymousData = true # Anonymous Kit application usage telemetry + +[settings.persistent] +app.primCreation.DefaultXformOpOrder = "xformOp:translate, xformOp:orient, xformOp:scale" +app.primCreation.DefaultXformOpType = "Scale, Orient, Translate" +app.primCreation.typedDefaults.camera.clippingRange = [0.01, 10000000.0] # Meters default +app.primCreation.DefaultXformOpPrecision = "Double" +app.primCreation.DefaultRotationOrder = "ZYX" +app.primCreation.PrimCreationWithDefaultXformOps = true +app.stage.timeCodeRange = [0, 1000000] +app.stage.upAxis = "Z" # Isaac Sim default Z up +app.viewport.camMoveVelocity = 0.05 # Meters default +app.viewport.gizmo.scale = 0.01 # Meters default +app.viewport.grid.scale = 1.0 # Meters default +app.viewport.camShowSpeedOnStart = false # Hide camera speed on startup +app.omniverse.gamepadCameraControl = false # Disable gamepad control for camera by default +exts."omni.anim.navigation.core".navMesh.config.autoRebakeOnChanges = false +exts."omni.anim.navigation.core".navMesh.viewNavMesh = false +physics.visualizationDisplayJoints = false # improves performance +physics.visualizationSimulationOutput = false # improves performance +physics.resetOnStop = true # Physics state is reset on stop +renderer.startupMessageDisplayed = true # hides the IOMMU popup window +resourcemonitor.timeBetweenQueries = 100 # improves performance +simulation.defaultMetersPerUnit = 1.0 # Meters default +omni.replicator.captureOnPlay = true + +[settings] +### async rendering settings +omni.replicator.asyncRendering = false +app.asyncRendering = false +app.asyncRenderingLowLatency = false + +# disable replicator orchestrator for better runtime perf +exts."omni.replicator.core".Orchestrator.enabled = false + +[settings.app.livestream] +outDirectory = "${data}" + +# Extensions +############################### +[settings.exts."omni.kit.registry.nucleus"] +registries = [ + { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/106/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" }, +] + +[settings.app.extensions] +skipPublishVerification = false +registryEnabled = true + +# Register extension folder from this repo in kit +[settings.app.exts] +folders = [ + "${exe-path}/exts", # kit extensions + "${exe-path}/extscore", # kit core extensions + "${exe-path}/../exts", # isaac extensions + "${exe-path}/../extsDeprecated", # deprecated isaac extensions + "${exe-path}/../extscache", # isaac cache extensions + "${exe-path}/../extsPhysics", # isaac physics extensions + "${exe-path}/../isaacsim/exts", # isaac extensions for pip + "${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions + "${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip + "${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip + "${app}", # needed to find other app files + "${app}/../../source", # needed to find extensions in Isaac Lab +] + +[settings.physics] +autoPopupSimulationOutputWindow = false +updateToUsd = false +updateVelocitiesToUsd = false +updateParticlesToUsd = false +updateVelocitiesToUsd = false +updateForceSensorsToUsd = false +outputVelocitiesLocalSpace = false +useFastCache = false +visualizationDisplayJoints = false +fabricUpdateTransformations = false +fabricUpdateVelocities = false +fabricUpdateForceSensors = false +fabricUpdateJointStates = false + +# Asset path +# 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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" diff --git a/apps/isaacsim_4_5/isaaclab.python.rendering.kit b/apps/isaacsim_4_5/isaaclab.python.rendering.kit new file mode 100644 index 000000000000..ad234141fd9b --- /dev/null +++ b/apps/isaacsim_4_5/isaaclab.python.rendering.kit @@ -0,0 +1,140 @@ +## +# Adapted from: https://github.com/NVIDIA-Omniverse/OmniIsaacGymEnvs/blob/main/apps/omni.isaac.sim.python.gym.camera.kit +# +# This app file designed specifically towards vision-based RL tasks. It provides necessary settings to enable +# multiple cameras to be rendered each frame. Additional settings are also applied to increase performance when +# rendering cameras across multiple environments. +## + +[package] +title = "Isaac Lab Python Camera" +description = "An app for running Isaac Lab with rendering enabled" +version = "2.3.2" + +# That makes it browsable in UI with "experience" filter +keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] + +[dependencies] +# Isaac Lab minimal app +"isaaclab.python" = {} + +# PhysX +"omni.kit.property.physx" = {} + +# Rendering +"omni.kit.material.library" = {} + +[settings.isaaclab] +# This is used to check that this experience file is loaded when using cameras +cameras_enabled = true + +[settings] +# Note: This path was adapted to be respective to the kit-exe file location +app.versionFile = "${exe-path}/VERSION" +app.folder = "${exe-path}/" +app.name = "Isaac-Sim" +app.version = "4.5.0" + +# Disable print outs on extension startup information +# this only disables the app print_and_log function +app.enableStdoutOutput = false + +# set the default ros bridge to disable on startup +isaac.startup.ros_bridge_extension = "" + +# Flags for better rendering performance +# Disabling these settings reduces renderer VRAM usage and improves rendering performance, but at some quality cost +rtx.translucency.enabled = false +rtx.reflections.enabled = false +rtx.indirectDiffuse.enabled = false +rtx-transient.dlssg.enabled = false +rtx.directLighting.sampledLighting.enabled = true +rtx.directLighting.sampledLighting.samplesPerPixel = 1 +rtx.sceneDb.ambientLightIntensity = 1.0 +# rtx.shadows.enabled = false + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 + +# Disable present thread to improve performance +exts."omni.renderer.core".present.enabled=false + +# Disabling these settings reduces renderer VRAM usage and improves rendering performance, but at some quality cost +rtx.raytracing.cached.enabled = false +rtx.ambientOcclusion.enabled = false + +# Set the DLSS model +rtx.post.dlss.execMode = 0 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids unnecessary GPU context initialization +renderer.multiGpu.maxGpuCount=1 + +# Force synchronous rendering to improve training results +omni.replicator.asyncRendering = false + +# Avoids frame offset issue +app.updateOrder.checkForHydraRenderComplete = 1000 +app.renderer.waitIdle=true +app.hydraEngine.waitIdle=true + +app.audio.enabled = false + +# disable replicator orchestrator for better runtime perf +exts."omni.replicator.core".Orchestrator.enabled = false + +[settings.physics] +updateToUsd = false +updateParticlesToUsd = false +updateVelocitiesToUsd = false +updateForceSensorsToUsd = false +outputVelocitiesLocalSpace = false +useFastCache = false +visualizationDisplayJoints = false +fabricUpdateTransformations = false +fabricUpdateVelocities = false +fabricUpdateForceSensors = false +fabricUpdateJointStates = 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/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" }, +] + +[settings.app.python] +# These disable the kit app from also printing out python output, which gets confusing +interceptSysStdOutput = false +logSysStdOutput = false + +[settings.app.renderer] +skipWhileMinimized = false +sleepMsOnFocus = 0 +sleepMsOutOfFocus = 0 + +# Register extension folder from this repo in kit +[settings.app.exts] +folders = [ + "${exe-path}/exts", # kit extensions + "${exe-path}/extscore", # kit core extensions + "${exe-path}/../exts", # isaac extensions + "${exe-path}/../extsDeprecated", # deprecated isaac extensions + "${exe-path}/../extscache", # isaac cache extensions + "${exe-path}/../extsPhysics", # isaac physics extensions + "${exe-path}/../isaacsim/exts", # isaac extensions for pip + "${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions + "${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip + "${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip + "${app}", # needed to find other app files + "${app}/../../source", # needed to find extensions in Isaac Lab +] + +# Asset path +# 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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" diff --git a/apps/isaacsim_4_5/isaaclab.python.xr.openxr.headless.kit b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.headless.kit new file mode 100644 index 000000000000..82f7e5cd62ab --- /dev/null +++ b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.headless.kit @@ -0,0 +1,41 @@ +## +# Adapted from: apps/isaaclab.python.xr.openxr.kit +## + +[package] +title = "Isaac Lab Python OpenXR Headless" +description = "An app for running Isaac Lab with OpenXR in headless mode" +version = "2.3.2" + +# That makes it browsable in UI with "experience" filter +keywords = ["experience", "app", "usd", "headless"] + +[settings] +# Note: This path was adapted to be respective to the kit-exe file location +app.versionFile = "${exe-path}/VERSION" +app.folder = "${exe-path}/" +app.name = "Isaac-Sim" +app.version = "4.5.0" + +[dependencies] +"isaaclab.python.xr.openxr" = {} + +[settings] +xr.profile.ar.enabled = true + +# Register extension folder from this repo in kit +[settings.app.exts] +folders = [ + "${exe-path}/exts", # kit extensions + "${exe-path}/extscore", # kit core extensions + "${exe-path}/../exts", # isaac extensions + "${exe-path}/../extsDeprecated", # deprecated isaac extensions + "${exe-path}/../extscache", # isaac cache extensions + "${exe-path}/../extsPhysics", # isaac physics extensions + "${exe-path}/../isaacsim/exts", # isaac extensions for pip + "${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions + "${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip + "${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip + "${app}", # needed to find other app files + "${app}/../../source", # needed to find extensions in Isaac Lab +] diff --git a/apps/isaacsim_4_5/isaaclab.python.xr.openxr.kit b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.kit new file mode 100644 index 000000000000..d27e5c444c21 --- /dev/null +++ b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.kit @@ -0,0 +1,71 @@ +## +# Adapted from: _isaac_sim/apps/isaacsim.exp.xr.openxr.kit +## + +[package] +title = "Isaac Lab Python OpenXR" +description = "An app for running Isaac Lab with OpenXR" +version = "2.3.2" + +# That makes it browsable in UI with "experience" filter +keywords = ["experience", "app", "usd"] + +[settings] +# Note: This path was adapted to be respective to the kit-exe file location +app.versionFile = "${exe-path}/VERSION" +app.folder = "${exe-path}/" +app.name = "Isaac-Sim" +app.version = "4.5.0" + +### async rendering settings +omni.replicator.asyncRendering = true +app.asyncRendering = true +app.asyncRenderingLowLatency = true + +# For XR, set this back to default "#define OMNI_MAX_DEVICE_GROUP_DEVICE_COUNT 16" +renderer.multiGpu.maxGpuCount = 16 +renderer.gpuEnumeration.glInterop.enabled = true # Allow Kit XR OpenXR to render headless + +[dependencies] +"isaaclab.python" = {} +"isaacsim.xr.openxr" = {} + +# Kit extensions +"omni.kit.xr.system.openxr" = {} +"omni.kit.xr.profile.ar" = {} + +[settings] +app.xr.enabled = true + +# xr settings +xr.ui.enabled = false +xr.depth.aov = "GBufferDepth" +defaults.xr.profile.ar.renderQuality = "off" +defaults.xr.profile.ar.anchorMode = "custom anchor" +rtx.rendermode = "RaytracedLighting" +persistent.xr.profile.ar.render.nearPlane = 0.15 + +# Register extension folder from this repo in kit +[settings.app.exts] +folders = [ + "${exe-path}/exts", # kit extensions + "${exe-path}/extscore", # kit core extensions + "${exe-path}/../exts", # isaac extensions + "${exe-path}/../extsDeprecated", # deprecated isaac extensions + "${exe-path}/../extscache", # isaac cache extensions + "${exe-path}/../extsPhysics", # isaac physics extensions + "${exe-path}/../isaacsim/exts", # isaac extensions for pip + "${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions + "${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip + "${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip + "${app}", # needed to find other app files + "${app}/../../source", # needed to find extensions in Isaac Lab +] + +# Asset path +# 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 = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.cloud = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" +persistent.isaac.asset_root.nvidia = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5" diff --git a/apps/isaacsim_4_5/rendering_modes/balanced.kit b/apps/isaacsim_4_5/rendering_modes/balanced.kit new file mode 100644 index 000000000000..ee92625fd7e7 --- /dev/null +++ b/apps/isaacsim_4_5/rendering_modes/balanced.kit @@ -0,0 +1,36 @@ +rtx.translucency.enabled = false + +rtx.reflections.enabled = false +rtx.reflections.denoiser.enabled = true + +# this will be ignored when RR (dldenoiser) is enabled +# rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = true + +rtx.sceneDb.ambientLightIntensity = 1.0 + +rtx.shadows.enabled = true + +rtx.indirectDiffuse.enabled = false +rtx.indirectDiffuse.denoiser.enabled = true + +# rtx.domeLight.upperLowerStrategy = 3 + +rtx.ambientOcclusion.enabled = false +rtx.ambientOcclusion.denoiserMode = 1 + +rtx.raytracing.subpixel.mode = 0 +rtx.raytracing.cached.enabled = true + +# DLSS frame gen does not yet support tiled camera well +rtx-transient.dlssg.enabled = false +rtx-transient.dldenoiser.enabled = true + +# Set the DLSS model +rtx.post.dlss.execMode = 1 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 + +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 diff --git a/apps/isaacsim_4_5/rendering_modes/performance.kit b/apps/isaacsim_4_5/rendering_modes/performance.kit new file mode 100644 index 000000000000..3cfe6e8c0e2c --- /dev/null +++ b/apps/isaacsim_4_5/rendering_modes/performance.kit @@ -0,0 +1,35 @@ +rtx.translucency.enabled = false + +rtx.reflections.enabled = false +rtx.reflections.denoiser.enabled = false + +rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = false + +rtx.sceneDb.ambientLightIntensity = 1.0 + +rtx.shadows.enabled = true + +rtx.indirectDiffuse.enabled = false +rtx.indirectDiffuse.denoiser.enabled = false + +rtx.domeLight.upperLowerStrategy = 3 + +rtx.ambientOcclusion.enabled = false +rtx.ambientOcclusion.denoiserMode = 1 + +rtx.raytracing.subpixel.mode = 0 +rtx.raytracing.cached.enabled = false + +# DLSS frame gen does not yet support tiled camera well +rtx-transient.dlssg.enabled = false +rtx-transient.dldenoiser.enabled = false + +# Set the DLSS model +rtx.post.dlss.execMode = 0 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 + +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 diff --git a/apps/isaacsim_4_5/rendering_modes/quality.kit b/apps/isaacsim_4_5/rendering_modes/quality.kit new file mode 100644 index 000000000000..8e966ddfd3b7 --- /dev/null +++ b/apps/isaacsim_4_5/rendering_modes/quality.kit @@ -0,0 +1,36 @@ +rtx.translucency.enabled = true + +rtx.reflections.enabled = true +rtx.reflections.denoiser.enabled = true + +# this will be ignored when RR (dldenoiser) is enabled +# rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = true + +rtx.sceneDb.ambientLightIntensity = 1.0 + +rtx.shadows.enabled = true + +rtx.indirectDiffuse.enabled = true +rtx.indirectDiffuse.denoiser.enabled = true + +# rtx.domeLight.upperLowerStrategy = 4 + +rtx.ambientOcclusion.enabled = true +rtx.ambientOcclusion.denoiserMode = 0 + +rtx.raytracing.subpixel.mode = 1 +rtx.raytracing.cached.enabled = true + +# DLSS frame gen does not yet support tiled camera well +rtx-transient.dlssg.enabled = false +rtx-transient.dldenoiser.enabled = true + +# Set the DLSS model +rtx.post.dlss.execMode = 2 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 + +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 diff --git a/apps/isaacsim_4_5/rendering_modes/xr.kit b/apps/isaacsim_4_5/rendering_modes/xr.kit new file mode 100644 index 000000000000..8cfc2c988d78 --- /dev/null +++ b/apps/isaacsim_4_5/rendering_modes/xr.kit @@ -0,0 +1,35 @@ +rtx.translucency.enabled = true + +rtx.reflections.enabled = true +rtx.reflections.denoiser.enabled = true + +rtx.directLighting.sampledLighting.denoisingTechnique = 5 +rtx.directLighting.sampledLighting.enabled = true + +rtx.sceneDb.ambientLightIntensity = 1.0 + +rtx.shadows.enabled = true + +rtx.indirectDiffuse.enabled = true +rtx.indirectDiffuse.denoiser.enabled = true + +rtx.domeLight.upperLowerStrategy = 4 + +rtx.ambientOcclusion.enabled = true +rtx.ambientOcclusion.denoiserMode = 0 + +rtx.raytracing.subpixel.mode = 1 +rtx.raytracing.cached.enabled = true + +# DLSS frame gen does not yet support tiled camera well +rtx-transient.dlssg.enabled = false +rtx-transient.dldenoiser.enabled = true + +# Set the DLSS model +rtx.post.dlss.execMode = 2 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 + +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 diff --git a/apps/rendering_modes/balanced.kit b/apps/rendering_modes/balanced.kit new file mode 100644 index 000000000000..ee92625fd7e7 --- /dev/null +++ b/apps/rendering_modes/balanced.kit @@ -0,0 +1,36 @@ +rtx.translucency.enabled = false + +rtx.reflections.enabled = false +rtx.reflections.denoiser.enabled = true + +# this will be ignored when RR (dldenoiser) is enabled +# rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = true + +rtx.sceneDb.ambientLightIntensity = 1.0 + +rtx.shadows.enabled = true + +rtx.indirectDiffuse.enabled = false +rtx.indirectDiffuse.denoiser.enabled = true + +# rtx.domeLight.upperLowerStrategy = 3 + +rtx.ambientOcclusion.enabled = false +rtx.ambientOcclusion.denoiserMode = 1 + +rtx.raytracing.subpixel.mode = 0 +rtx.raytracing.cached.enabled = true + +# DLSS frame gen does not yet support tiled camera well +rtx-transient.dlssg.enabled = false +rtx-transient.dldenoiser.enabled = true + +# Set the DLSS model +rtx.post.dlss.execMode = 1 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 + +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 diff --git a/apps/rendering_modes/extension.toml b/apps/rendering_modes/extension.toml new file mode 100644 index 000000000000..f0668b9184d1 --- /dev/null +++ b/apps/rendering_modes/extension.toml @@ -0,0 +1 @@ +# This is not an extension diff --git a/apps/rendering_modes/performance.kit b/apps/rendering_modes/performance.kit new file mode 100644 index 000000000000..3cfe6e8c0e2c --- /dev/null +++ b/apps/rendering_modes/performance.kit @@ -0,0 +1,35 @@ +rtx.translucency.enabled = false + +rtx.reflections.enabled = false +rtx.reflections.denoiser.enabled = false + +rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = false + +rtx.sceneDb.ambientLightIntensity = 1.0 + +rtx.shadows.enabled = true + +rtx.indirectDiffuse.enabled = false +rtx.indirectDiffuse.denoiser.enabled = false + +rtx.domeLight.upperLowerStrategy = 3 + +rtx.ambientOcclusion.enabled = false +rtx.ambientOcclusion.denoiserMode = 1 + +rtx.raytracing.subpixel.mode = 0 +rtx.raytracing.cached.enabled = false + +# DLSS frame gen does not yet support tiled camera well +rtx-transient.dlssg.enabled = false +rtx-transient.dldenoiser.enabled = false + +# Set the DLSS model +rtx.post.dlss.execMode = 0 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 + +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 diff --git a/apps/rendering_modes/quality.kit b/apps/rendering_modes/quality.kit new file mode 100644 index 000000000000..8e966ddfd3b7 --- /dev/null +++ b/apps/rendering_modes/quality.kit @@ -0,0 +1,36 @@ +rtx.translucency.enabled = true + +rtx.reflections.enabled = true +rtx.reflections.denoiser.enabled = true + +# this will be ignored when RR (dldenoiser) is enabled +# rtx.directLighting.sampledLighting.denoisingTechnique = 0 +rtx.directLighting.sampledLighting.enabled = true + +rtx.sceneDb.ambientLightIntensity = 1.0 + +rtx.shadows.enabled = true + +rtx.indirectDiffuse.enabled = true +rtx.indirectDiffuse.denoiser.enabled = true + +# rtx.domeLight.upperLowerStrategy = 4 + +rtx.ambientOcclusion.enabled = true +rtx.ambientOcclusion.denoiserMode = 0 + +rtx.raytracing.subpixel.mode = 1 +rtx.raytracing.cached.enabled = true + +# DLSS frame gen does not yet support tiled camera well +rtx-transient.dlssg.enabled = false +rtx-transient.dldenoiser.enabled = true + +# Set the DLSS model +rtx.post.dlss.execMode = 2 # can be 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto) + +# Avoids replicator warning +rtx.pathtracing.maxSamplesPerLaunch = 1000000 + +# Avoids silent trimming of tiles +rtx.viewTile.limit = 1000000 diff --git a/docker/.env.base b/docker/.env.base index 8cf575758ab5..b1871cf6d84c 100644 --- a/docker/.env.base +++ b/docker/.env.base @@ -6,11 +6,21 @@ ACCEPT_EULA=Y # NVIDIA Isaac Sim base image ISAACSIM_BASE_IMAGE=nvcr.io/nvidia/isaac-sim -# NVIDIA Isaac Sim version to use (e.g. 4.5.0) -ISAACSIM_VERSION=4.5.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 DOCKER_ISAACLAB_PATH=/workspace/isaaclab # Docker user directory - by default this is the root user's home directory DOCKER_USER_HOME=/root +# Docker image and container name suffix (default "", set by the container_interface.py script) +# Example: "-custom" +DOCKER_NAME_SUFFIX="" + +# Display configuration for remote desktop sessions. +DISPLAY_WIDTH=1280 +DISPLAY_HEIGHT=720 +DISPLAY_DEPTH=24 +VNC_DISPLAY=1 +VNC_PORT=5900 diff --git a/docker/.env.cloudxr-runtime b/docker/.env.cloudxr-runtime new file mode 100644 index 000000000000..65b6d1373ac3 --- /dev/null +++ b/docker/.env.cloudxr-runtime @@ -0,0 +1,8 @@ +### +# General settings +### + +# 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.1 diff --git a/docker/.env.ros2 b/docker/.env.ros2 index b7b255dc4f2c..609704f45674 100644 --- a/docker/.env.ros2 +++ b/docker/.env.ros2 @@ -9,3 +9,6 @@ RMW_IMPLEMENTATION=rmw_fastrtps_cpp FASTRTPS_DEFAULT_PROFILES_FILE=${DOCKER_USER_HOME}/.ros/fastdds.xml # Path to cyclonedds.xml file to use (only needed when using cyclonedds) CYCLONEDDS_URI=${DOCKER_USER_HOME}/.ros/cyclonedds.xml +# Docker image and container name suffix (default "", set by the container_interface.py script) +# Example: "-custom" +DOCKER_NAME_SUFFIX="" diff --git a/docker/Dockerfile.base b/docker/Dockerfile.base index ea211b1e1ef1..dbc474e6f2b4 100644 --- a/docker/Dockerfile.base +++ b/docker/Dockerfile.base @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -16,7 +16,7 @@ ENV ISAACSIM_VERSION=${ISAACSIM_VERSION_ARG} SHELL ["/bin/bash", "-c"] # Adds labels to the Dockerfile -LABEL version="1.1" +LABEL version="2.1.1" LABEL description="Dockerfile for building and running the Isaac Lab framework inside Isaac Sim container image." # Arguments @@ -42,14 +42,40 @@ RUN --mount=type=cache,target=/var/cache/apt \ build-essential \ cmake \ git \ + gpg \ libglib2.0-0 \ - ncurses-term && \ + ncurses-term \ + wget && \ + apt -y autoremove && apt clean autoclean && \ + rm -rf /var/lib/apt/lists/* + +# Install TurboVNC and a lightweight desktop for remote visualization. +RUN --mount=type=cache,target=/var/cache/apt \ + set -eux && \ + wget -q -O- https://packagecloud.io/dcommander/turbovnc/gpgkey | \ + gpg --dearmor > /etc/apt/trusted.gpg.d/TurboVNC.gpg && \ + wget -q -O /etc/apt/sources.list.d/TurboVNC.list \ + https://raw.githubusercontent.com/TurboVNC/repo/main/TurboVNC.list && \ + apt-get update && apt-get install -y --no-install-recommends \ + dbus-x11 \ + openbox \ + pcmanfm \ + turbovnc \ + xauth \ + xterm && \ apt -y autoremove && apt clean autoclean && \ rm -rf /var/lib/apt/lists/* # Copy the Isaac Lab directory (files to exclude are defined in .dockerignore) COPY ../ ${ISAACLAB_PATH} +# Ensure isaaclab.sh has execute permissions +RUN chmod +x ${ISAACLAB_PATH}/isaaclab.sh + +# Install container-side helpers for the TurboVNC session. +RUN chmod +x ${ISAACLAB_PATH}/docker/start-turbovnc.sh && \ + chmod +x ${ISAACLAB_PATH}/docker/turbovnc-xstartup.sh + # Set up a symbolic link between the installed Isaac Sim root folder and _isaac_sim in the Isaac Lab directory RUN ln -sf ${ISAACSIM_ROOT_PATH} ${ISAACLAB_PATH}/_isaac_sim @@ -87,6 +113,9 @@ RUN touch /bin/nvidia-smi && \ RUN --mount=type=cache,target=${DOCKER_USER_HOME}/.cache/pip \ ${ISAACLAB_PATH}/isaaclab.sh --install +# HACK: Remove install of quadprog dependency +RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip uninstall -y quadprog + # aliasing isaaclab.sh and python for convenience RUN echo "export ISAACLAB_PATH=${ISAACLAB_PATH}" >> ${HOME}/.bashrc && \ echo "alias isaaclab=${ISAACLAB_PATH}/isaaclab.sh" >> ${HOME}/.bashrc && \ @@ -95,7 +124,9 @@ RUN echo "export ISAACLAB_PATH=${ISAACLAB_PATH}" >> ${HOME}/.bashrc && \ echo "alias pip='${ISAACLAB_PATH}/_isaac_sim/python.sh -m pip'" >> ${HOME}/.bashrc && \ echo "alias pip3='${ISAACLAB_PATH}/_isaac_sim/python.sh -m pip'" >> ${HOME}/.bashrc && \ echo "alias tensorboard='${ISAACLAB_PATH}/_isaac_sim/python.sh ${ISAACLAB_PATH}/_isaac_sim/tensorboard'" >> ${HOME}/.bashrc && \ - echo "export TZ=$(date +%Z)" >> ${HOME}/.bashrc + echo "export TZ=$(date +%Z)" >> ${HOME}/.bashrc && \ + echo "shopt -s histappend" >> /root/.bashrc && \ + echo "PROMPT_COMMAND='history -a'" >> /root/.bashrc # make working directory as the Isaac Lab directory # this is the default directory when the container is run diff --git a/docker/Dockerfile.curobo b/docker/Dockerfile.curobo new file mode 100644 index 000000000000..0830adebf18b --- /dev/null +++ b/docker/Dockerfile.curobo @@ -0,0 +1,155 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Nvidia Dockerfiles: https://github.com/NVIDIA-Omniverse/IsaacSim-dockerfiles +# Please check above link for license information. + +# Base image +ARG ISAACSIM_BASE_IMAGE_ARG +ARG ISAACSIM_VERSION_ARG +FROM ${ISAACSIM_BASE_IMAGE_ARG}:${ISAACSIM_VERSION_ARG} AS base +ENV ISAACSIM_VERSION=${ISAACSIM_VERSION_ARG} + +# Set default RUN shell to bash +SHELL ["/bin/bash", "-c"] + +# Adds labels to the Dockerfile +LABEL version="2.1.1" +LABEL description="Dockerfile for building and running the Isaac Lab framework inside Isaac Sim container image." + +# Arguments +# Path to Isaac Sim root folder +ARG ISAACSIM_ROOT_PATH_ARG +ENV ISAACSIM_ROOT_PATH=${ISAACSIM_ROOT_PATH_ARG} +# Path to the Isaac Lab directory +ARG ISAACLAB_PATH_ARG +ENV ISAACLAB_PATH=${ISAACLAB_PATH_ARG} +# Home dir of docker user, typically '/root' +ARG DOCKER_USER_HOME_ARG +ENV DOCKER_USER_HOME=${DOCKER_USER_HOME_ARG} + +# Set environment variables +ENV LANG=C.UTF-8 +ENV DEBIAN_FRONTEND=noninteractive + +USER root + +# Install dependencies and remove cache +RUN --mount=type=cache,target=/var/cache/apt \ + apt-get update && apt-get install -y --no-install-recommends \ + build-essential \ + cmake \ + git \ + libglib2.0-0 \ + ncurses-term \ + wget && \ + apt -y autoremove && apt clean autoclean && \ + rm -rf /var/lib/apt/lists/* + +# Detect Ubuntu version and install CUDA 12.8 via NVIDIA network repo (cuda-keyring) +RUN set -euo pipefail && \ + . /etc/os-release && \ + case "$ID" in \ + ubuntu) \ + case "$VERSION_ID" in \ + "20.04") cuda_repo="ubuntu2004";; \ + "22.04") cuda_repo="ubuntu2204";; \ + "24.04") cuda_repo="ubuntu2404";; \ + *) echo "Unsupported Ubuntu $VERSION_ID"; exit 1;; \ + esac ;; \ + *) echo "Unsupported base OS: $ID"; exit 1 ;; \ + esac && \ + apt-get update && apt-get install -y --no-install-recommends wget gnupg ca-certificates && \ + wget -q https://developer.download.nvidia.com/compute/cuda/repos/${cuda_repo}/x86_64/cuda-keyring_1.1-1_all.deb && \ + dpkg -i cuda-keyring_1.1-1_all.deb && \ + rm -f cuda-keyring_1.1-1_all.deb && \ + wget -q https://developer.download.nvidia.com/compute/cuda/repos/${cuda_repo}/x86_64/cuda-${cuda_repo}.pin && \ + mv cuda-${cuda_repo}.pin /etc/apt/preferences.d/cuda-repository-pin-600 && \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + cuda-toolkit-12-8 \ + libcudnn9-cuda-12 \ + libcusparselt0 \ + libnccl2 \ + libnccl-dev \ + libnvjitlink-12-8 && \ + apt-get -y autoremove && apt-get clean && rm -rf /var/lib/apt/lists/* + + +ENV CUDA_HOME=/usr/local/cuda-12.8 +ENV PATH=${CUDA_HOME}/bin:${PATH} +ENV LD_LIBRARY_PATH=${CUDA_HOME}/lib64:${LD_LIBRARY_PATH} +ENV TORCH_CUDA_ARCH_LIST=8.0+PTX + +# Copy the Isaac Lab directory (files to exclude are defined in .dockerignore) +COPY ../ ${ISAACLAB_PATH} + +# Ensure isaaclab.sh has execute permissions +RUN chmod +x ${ISAACLAB_PATH}/isaaclab.sh + +# Set up a symbolic link between the installed Isaac Sim root folder and _isaac_sim in the Isaac Lab directory +RUN ln -sf ${ISAACSIM_ROOT_PATH} ${ISAACLAB_PATH}/_isaac_sim + +# Install toml dependency +RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install toml + +# Install apt dependencies for extensions that declare them in their extension.toml +RUN --mount=type=cache,target=/var/cache/apt \ + ${ISAACLAB_PATH}/isaaclab.sh -p ${ISAACLAB_PATH}/tools/install_deps.py apt ${ISAACLAB_PATH}/source && \ + apt -y autoremove && apt clean autoclean && \ + rm -rf /var/lib/apt/lists/* + +# for singularity usage, have to create the directories that will binded +RUN mkdir -p ${ISAACSIM_ROOT_PATH}/kit/cache && \ + mkdir -p ${DOCKER_USER_HOME}/.cache/ov && \ + mkdir -p ${DOCKER_USER_HOME}/.cache/pip && \ + mkdir -p ${DOCKER_USER_HOME}/.cache/nvidia/GLCache && \ + mkdir -p ${DOCKER_USER_HOME}/.nv/ComputeCache && \ + mkdir -p ${DOCKER_USER_HOME}/.nvidia-omniverse/logs && \ + mkdir -p ${DOCKER_USER_HOME}/.local/share/ov/data && \ + mkdir -p ${DOCKER_USER_HOME}/Documents + +# for singularity usage, create NVIDIA binary placeholders +RUN touch /bin/nvidia-smi && \ + touch /bin/nvidia-debugdump && \ + touch /bin/nvidia-persistenced && \ + touch /bin/nvidia-cuda-mps-control && \ + touch /bin/nvidia-cuda-mps-server && \ + touch /etc/localtime && \ + mkdir -p /var/run/nvidia-persistenced && \ + touch /var/run/nvidia-persistenced/socket + +# HACK: Remove pre-bundled torch BEFORE installing Isaac Lab dependencies. +# This forces isaaclab.sh --install to install torch fresh to site-packages, +# rather than skipping because it detects the pre-bundled version. +RUN rm -rf ${ISAACSIM_ROOT_PATH}/exts/omni.isaac.ml_archive/pip_prebundle/torch* + +# installing Isaac Lab dependencies +# use pip caching to avoid reinstalling large packages +RUN --mount=type=cache,target=${DOCKER_USER_HOME}/.cache/pip \ + ${ISAACLAB_PATH}/isaaclab.sh --install + +# HACK: Uninstall quadprog as it causes issues with some reinforcement learning frameworks +RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip uninstall -y quadprog + +# Install cuRobo from source (pinned commit); needs CUDA env and Torch +RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-build-isolation \ + "nvidia-curobo @ git+https://github.com/NVlabs/curobo.git@ebb71702f3f70e767f40fd8e050674af0288abe8" + +# aliasing isaaclab.sh and python for convenience +RUN echo "export ISAACLAB_PATH=${ISAACLAB_PATH}" >> ${HOME}/.bashrc && \ + echo "alias isaaclab=${ISAACLAB_PATH}/isaaclab.sh" >> ${HOME}/.bashrc && \ + echo "alias python=${ISAACLAB_PATH}/_isaac_sim/python.sh" >> ${HOME}/.bashrc && \ + echo "alias python3=${ISAACLAB_PATH}/_isaac_sim/python.sh" >> ${HOME}/.bashrc && \ + echo "alias pip='${ISAACLAB_PATH}/_isaac_sim/python.sh -m pip'" >> ${HOME}/.bashrc && \ + echo "alias pip3='${ISAACLAB_PATH}/_isaac_sim/python.sh -m pip'" >> ${HOME}/.bashrc && \ + echo "alias tensorboard='${ISAACLAB_PATH}/_isaac_sim/python.sh ${ISAACLAB_PATH}/_isaac_sim/tensorboard'" >> ${HOME}/.bashrc && \ + echo "export TZ=$(date +%Z)" >> ${HOME}/.bashrc && \ + echo "shopt -s histappend" >> /root/.bashrc && \ + echo "PROMPT_COMMAND='history -a'" >> /root/.bashrc + +# make working directory as the Isaac Lab directory +# this is the default directory when the container is run +WORKDIR ${ISAACLAB_PATH} diff --git a/docker/Dockerfile.ros2 b/docker/Dockerfile.ros2 index 8c35bffe4ece..2e00fc7ec396 100644 --- a/docker/Dockerfile.ros2 +++ b/docker/Dockerfile.ros2 @@ -1,6 +1,10 @@ # Everything past this stage is to install # ROS2 Humble -FROM isaac-lab-base AS ros2 + +# What is the docker name suffix for the base image to load? (defaults to empty string) +ARG DOCKER_NAME_SUFFIX="" + +FROM isaac-lab-base${DOCKER_NAME_SUFFIX} AS ros2 # Which ROS2 apt package to install ARG ROS2_APT_PACKAGE diff --git a/docker/container.py b/docker/container.py index 766a40faee98..ab92d816ffac 100755 --- a/docker/container.py +++ b/docker/container.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -46,9 +46,30 @@ def parse_cli_args() -> argparse.Namespace: " '.env.base' in their provided order." ), ) + parent_parser.add_argument( + "--suffix", + nargs="?", + default=None, + help=( + "Optional docker image and container name suffix. Defaults to None, in which case, the docker name" + " suffix is set to the empty string. A hyphen is inserted in between the profile and the suffix if" + ' the suffix is a nonempty string. For example, if "base" is passed to profile, and "custom" is' + " passed to suffix, then the produced docker image and container will be named ``isaac-lab-base-custom``." + ), + ) + parent_parser.add_argument( + "--info", + action="store_true", + help="Print the container interface information. This is useful for debugging purposes.", + ) # Actual command definition begins here subparsers = parser.add_subparsers(dest="command", required=True) + subparsers.add_parser( + "build", + help="Build the docker image without creating the container.", + parents=[parent_parser], + ) subparsers.add_parser( "start", help="Build the docker image and create the container in detached mode.", @@ -90,11 +111,29 @@ def main(args: argparse.Namespace): # creating container interface ci = ContainerInterface( - context_dir=Path(__file__).resolve().parent, profile=args.profile, yamls=args.files, envs=args.env_files + context_dir=Path(__file__).resolve().parent, + profile=args.profile, + yamls=args.files, + envs=args.env_files, + suffix=args.suffix, ) + if args.info: + print("[INFO] Printing container interface information...\n") + ci.print_info() + return print(f"[INFO] Using container profile: {ci.profile}") - if args.command == "start": + if args.command == "build": + # check if x11 forwarding is enabled + x11_outputs = x11_utils.x11_check(ci.statefile) + # if x11 forwarding is enabled, add the x11 yaml and environment variables + if x11_outputs is not None: + (x11_yaml, x11_envar) = x11_outputs + ci.add_yamls += x11_yaml + ci.environ.update(x11_envar) + # build the image + ci.build() + elif args.command == "start": # check if x11 forwarding is enabled x11_outputs = x11_utils.x11_check(ci.statefile) # if x11 forwarding is enabled, add the x11 yaml and environment variables diff --git a/docker/container.sh b/docker/container.sh index c4dc31cb8be5..f6fc2af49d61 100755 --- a/docker/container.sh +++ b/docker/container.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/docker/docker-compose.cloudxr-runtime.patch.yaml b/docker/docker-compose.cloudxr-runtime.patch.yaml new file mode 100644 index 000000000000..5615aed29ac7 --- /dev/null +++ b/docker/docker-compose.cloudxr-runtime.patch.yaml @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +services: + cloudxr-runtime: + image: ${CLOUDXR_RUNTIME_BASE_IMAGE_ARG}:${CLOUDXR_RUNTIME_VERSION_ARG} + ports: + - "48010:48010/tcp" # signaling + - "47998:47998/udp" # media + - "47999:47999/udp" # media + - "48000:48000/udp" # media + - "48005:48005/udp" # media + - "48008:48008/udp" # media + - "48012:48012/udp" # media + healthcheck: + test: ["CMD", "test", "-S", "/openxr/run/ipc_cloudxr"] + interval: 1s + timeout: 1s + retries: 10 + start_period: 5s + environment: + - ACCEPT_EULA=${ACCEPT_EULA} + volumes: + - openxr-volume:/openxr:rw + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [ gpu ] + + isaac-lab-base: + environment: + - XDG_RUNTIME_DIR=/openxr/run + - XR_RUNTIME_JSON=/openxr/share/openxr/1/openxr_cloudxr.json + volumes: + - openxr-volume:/openxr:rw + depends_on: + - cloudxr-runtime + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [ gpu ] + +volumes: + openxr-volume: diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 26c924ec42f3..8889ced22df2 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -1,5 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Here we set the parts that would -# be re-used between services to an +# be reused between services to an # extension field # https://docs.docker.com/compose/compose-file/compose-file-v3/#extension-fields x-default-isaac-lab-volumes: &default-isaac-lab-volumes @@ -32,6 +37,9 @@ x-default-isaac-lab-volumes: &default-isaac-lab-volumes - type: volume source: isaac-docs target: ${DOCKER_USER_HOME}/Documents + - type: bind + source: ../../humanoid + target: /workspace/humanoid # This overlay allows changes on the local files to # be reflected within the container immediately - type: bind @@ -60,10 +68,15 @@ x-default-isaac-lab-volumes: &default-isaac-lab-volumes - type: volume source: isaac-lab-data target: ${DOCKER_ISAACLAB_PATH}/data_storage + # This volume is used to store the history of the bash shell + - type: bind + source: .isaac-lab-docker-history + target: ${DOCKER_USER_HOME}/.bash_history x-default-isaac-lab-environment: &default-isaac-lab-environment - ISAACSIM_PATH=${DOCKER_ISAACLAB_PATH}/_isaac_sim - OMNI_KIT_ALLOW_ROOT=1 + - DISPLAY=:${VNC_DISPLAY}.0 x-default-isaac-lab-deploy: &default-isaac-lab-deploy resources: @@ -87,14 +100,15 @@ services: - ISAACSIM_ROOT_PATH_ARG=${DOCKER_ISAACSIM_ROOT_PATH} - ISAACLAB_PATH_ARG=${DOCKER_ISAACLAB_PATH} - DOCKER_USER_HOME_ARG=${DOCKER_USER_HOME} - image: isaac-lab-base - container_name: isaac-lab-base + image: isaac-lab-base${DOCKER_NAME_SUFFIX-} + container_name: isaac-lab-base${DOCKER_NAME_SUFFIX-} environment: *default-isaac-lab-environment volumes: *default-isaac-lab-volumes - network_mode: host deploy: *default-isaac-lab-deploy + ports: + - "5900:5900" # This is the entrypoint for the container - entrypoint: bash + entrypoint: /workspace/isaaclab/docker/start-turbovnc.sh stdin_open: true tty: true @@ -113,14 +127,18 @@ services: # avoid a warning message when building only the base profile # with the .env.base file - ROS2_APT_PACKAGE=${ROS2_APT_PACKAGE:-NONE} - image: isaac-lab-ros2 - container_name: isaac-lab-ros2 + # Make sure that the correct Docker Name Suffix is being passed to the dockerfile, to know which base image to + # start from. + - DOCKER_NAME_SUFFIX=${DOCKER_NAME_SUFFIX-} + image: isaac-lab-ros2${DOCKER_NAME_SUFFIX-} + container_name: isaac-lab-ros2${DOCKER_NAME_SUFFIX-} environment: *default-isaac-lab-environment volumes: *default-isaac-lab-volumes - network_mode: host deploy: *default-isaac-lab-deploy + ports: + - "5900:5900" # This is the entrypoint for the container - entrypoint: bash + entrypoint: /workspace/isaaclab/docker/start-turbovnc.sh stdin_open: true tty: true diff --git a/docker/start-turbovnc.sh b/docker/start-turbovnc.sh new file mode 100644 index 000000000000..c22cf09951d8 --- /dev/null +++ b/docker/start-turbovnc.sh @@ -0,0 +1,32 @@ +#!/usr/bin/env bash + +set -euo pipefail + +VNC_DISPLAY="${VNC_DISPLAY:-1}" +DISPLAY_NUM=":${VNC_DISPLAY}" +DISPLAY_WIDTH="${DISPLAY_WIDTH:-1280}" +DISPLAY_HEIGHT="${DISPLAY_HEIGHT:-720}" +DISPLAY_DEPTH="${DISPLAY_DEPTH:-24}" +VNC_PORT="${VNC_PORT:-5900}" +VNC_GEOMETRY="${DISPLAY_WIDTH}x${DISPLAY_HEIGHT}" +VNC_XSTARTUP="${ISAACLAB_PATH}/docker/turbovnc-xstartup.sh" +VNC_PASSWD_DIR="${DOCKER_USER_HOME}/.vnc" +VNC_LOG_FILE="${VNC_PASSWD_DIR}/$(hostname)${DISPLAY_NUM}.log" + +mkdir -p "${VNC_PASSWD_DIR}" +chmod 700 "${VNC_PASSWD_DIR}" + +# TurboVNC refuses to start if stale lock files exist from a previous run. +vncserver -kill "${DISPLAY_NUM}" >/dev/null 2>&1 || true +rm -rf "/tmp/.X${VNC_DISPLAY}-lock" "/tmp/.X11-unix/X${VNC_DISPLAY}" + +/opt/TurboVNC/bin/vncserver "${DISPLAY_NUM}" \ + -SecurityTypes None \ + -rfbport "${VNC_PORT}" \ + -geometry "${VNC_GEOMETRY}" \ + -depth "${DISPLAY_DEPTH}" \ + -xstartup "${VNC_XSTARTUP}" + +# Keep the container alive while still surfacing VNC session logs. +touch "${VNC_LOG_FILE}" +exec tail -F "${VNC_LOG_FILE}" diff --git a/docker/test/test_docker.py b/docker/test/test_docker.py new file mode 100644 index 000000000000..85fd66348f85 --- /dev/null +++ b/docker/test/test_docker.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import subprocess +from pathlib import Path + +import pytest + + +def start_stop_docker(profile, suffix): + """Test starting and stopping docker profile with suffix.""" + environ = os.environ + context_dir = Path(__file__).resolve().parent.parent + + # generate parameters for the arguments + if suffix != "": + container_name = f"isaac-lab-{profile}-{suffix}" + suffix_args = ["--suffix", suffix] + else: + container_name = f"isaac-lab-{profile}" + suffix_args = [] + + run_kwargs = { + "check": False, + "capture_output": True, + "text": True, + "cwd": context_dir, + "env": environ, + } + + # start the container + docker_start = subprocess.run(["python", "container.py", "start", profile] + suffix_args, **run_kwargs) + assert docker_start.returncode == 0 + + # verify that the container is running + docker_running_true = subprocess.run(["docker", "ps"], **run_kwargs) + assert docker_running_true.returncode == 0 + assert container_name in docker_running_true.stdout + + # stop the container + docker_stop = subprocess.run(["python", "container.py", "stop", profile] + suffix_args, **run_kwargs) + assert docker_stop.returncode == 0 + + # verify that the container has stopped + docker_running_false = subprocess.run(["docker", "ps"], **run_kwargs) + assert docker_running_false.returncode == 0 + assert container_name not in docker_running_false.stdout + + +@pytest.mark.parametrize( + "profile,suffix", + [ + ("base", ""), + ("base", "test"), + ("ros2", ""), + ("ros2", "test"), + ], +) +def test_docker_profiles(profile, suffix): + """Test starting and stopping docker profiles with and without suffixes.""" + start_stop_docker(profile, suffix) diff --git a/docker/turbovnc-xstartup.sh b/docker/turbovnc-xstartup.sh new file mode 100644 index 000000000000..8d016936a234 --- /dev/null +++ b/docker/turbovnc-xstartup.sh @@ -0,0 +1,13 @@ +#!/usr/bin/env bash + +unset SESSION_MANAGER +unset DBUS_SESSION_BUS_ADDRESS + +export XDG_RUNTIME_DIR="${XDG_RUNTIME_DIR:-/tmp/runtime-root}" +mkdir -p "${XDG_RUNTIME_DIR}" +chmod 700 "${XDG_RUNTIME_DIR}" + +# Keep the desktop minimal to reduce remote rendering overhead. +pcmanfm --desktop >/tmp/pcmanfm.log 2>&1 & +xterm >/tmp/xterm.log 2>&1 & +exec openbox-session >/tmp/openbox-session.log 2>&1 diff --git a/docker/utils/__init__.py b/docker/utils/__init__.py index d6f44a2dcb6e..4d29d62425fa 100644 --- a/docker/utils/__init__.py +++ b/docker/utils/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/docker/utils/container_interface.py b/docker/utils/container_interface.py index 83d8c4128460..f8b3eb07ee22 100644 --- a/docker/utils/container_interface.py +++ b/docker/utils/container_interface.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -24,19 +24,30 @@ def __init__( yamls: list[str] | None = None, envs: list[str] | None = None, statefile: StateFile | None = None, + suffix: str | None = None, ): """Initialize the container interface with the given parameters. Args: - context_dir: The context directory for Docker operations. - profile: The profile name for the container. Defaults to "base". - yamls: A list of yaml files to extend ``docker-compose.yaml`` settings. These are extended in the order - they are provided. - envs: A list of environment variable files to extend the ``.env.base`` file. These are extended in the order - they are provided. - statefile: An instance of the :class:`Statefile` class to manage state variables. Defaults to None, in + context_dir: + The context directory for Docker operations. + profile: + The profile name for the container. Defaults to "base". + yamls: + A list of yaml files to extend ``docker-compose.yaml`` settings. These are extended in the order + they are provided. Defaults to None, in which case no additional yaml files are added. + envs: + A list of environment variable files to extend the ``.env.base`` file. These are extended in the order + they are provided. Defaults to None, in which case no additional environment variable files are added. + statefile: + An instance of the :class:`Statefile` class to manage state variables. Defaults to None, in which case a new configuration object is created by reading the configuration file at the path ``context_dir/.container.cfg``. + suffix: + Optional docker image and container name suffix. Defaults to None, in which case, the docker name + suffix is set to the empty string. A hyphen is inserted in between the profile and the suffix if + the suffix is a nonempty string. For example, if "base" is passed to profile, and "custom" is + passed to suffix, then the produced docker image and container will be named ``isaac-lab-base-custom``. """ # set the context directory self.context_dir = context_dir @@ -55,17 +66,50 @@ def __init__( # but not a real profile self.profile = "base" - self.container_name = f"isaac-lab-{self.profile}" - self.image_name = f"isaac-lab-{self.profile}:latest" + # set the docker image and container name suffix + if suffix is None or suffix == "": + # if no name suffix is given, default to the empty string as the name suffix + self.suffix = "" + else: + # insert a hyphen before the suffix if a suffix is given + self.suffix = f"-{suffix}" + + # set names for easier reference + self.base_service_name = "isaac-lab-base" + self.service_name = f"isaac-lab-{self.profile}" + self.container_name = f"{self.service_name}{self.suffix}" + self.image_name = f"{self.service_name}{self.suffix}:latest" - # keep the environment variables from the current environment - self.environ = os.environ + # keep the environment variables from the current environment, + # except make sure that the docker name suffix is set from the script + self.environ = os.environ.copy() + self.environ["DOCKER_NAME_SUFFIX"] = self.suffix # resolve the image extension through the passed yamls and envs self._resolve_image_extension(yamls, envs) # load the environment variables from the .env files self._parse_dot_vars() + def print_info(self): + """Print the container interface information.""" + print("=" * 60) + print(f"{'DOCKER CONTAINER INFO':^60}") # Centered title + print("=" * 60) + + print(f"{'Profile:':25} {self.profile}") + print(f"{'Suffix:':25} {self.suffix}") + print(f"{'Service Name:':25} {self.service_name}") + print(f"{'Image Name:':25} {self.image_name}") + print(f"{'Container Name:':25} {self.container_name}") + + print("-" * 60) + print(f"{'Docker Compose Arguments':^60}") + print("-" * 60) + print(f"{'YAMLs:':25} {' '.join(self.add_yamls)}") + print(f"{'Profiles:':25} {' '.join(self.add_profiles)}") + print(f"{'Env Files:':25} {' '.join(self.add_env_files)}") + print("=" * 60) + """ Operations. """ @@ -93,41 +137,65 @@ def does_image_exist(self) -> bool: result = subprocess.run(["docker", "image", "inspect", self.image_name], capture_output=True, text=True) return result.returncode == 0 + def build(self): + """Build the Docker image.""" + print("[INFO] Building the docker image for the profile 'base'...\n") + # build the image for the base profile + cmd = ( + ["docker", "compose"] + + ["--file", "docker-compose.yaml"] + + ["--profile", "base"] + + ["--env-file", ".env.base"] + + ["build", self.base_service_name] + ) + subprocess.run(cmd, check=False, cwd=self.context_dir, env=self.environ) + print("[INFO] Finished building the docker image for the profile 'base'.\n") + + # build the image for the profile + if self.profile != "base": + print(f"[INFO] Building the docker image for the profile '{self.profile}'...\n") + cmd = ( + ["docker", "compose"] + + self.add_yamls + + self.add_profiles + + self.add_env_files + + ["build", self.service_name] + ) + subprocess.run(cmd, check=False, cwd=self.context_dir, env=self.environ) + print(f"[INFO] Finished building the docker image for the profile '{self.profile}'.\n") + def start(self): """Build and start the Docker container using the Docker compose command.""" print( f"[INFO] Building the docker image and starting the container '{self.container_name}' in the" " background...\n" ) + # Check if the container history file exists + container_history_file = self.context_dir / ".isaac-lab-docker-history" + if not container_history_file.exists(): + # Create the file with sticky bit on the group + container_history_file.touch(mode=0o2644, exist_ok=True) - # build the image for the base profile - subprocess.run( - [ - "docker", - "compose", - "--file", - "docker-compose.yaml", - "--env-file", - ".env.base", - "build", - "isaac-lab-base", - ], - check=False, - cwd=self.context_dir, - env=self.environ, - ) + # build the image for the base profile if not running base (up will build base already if profile is base) + if self.profile != "base": + cmd = ( + ["docker", "compose"] + + ["--file", "docker-compose.yaml"] + + ["--profile", "base"] + + ["--env-file", ".env.base"] + + ["build", self.base_service_name] + ) + subprocess.run(cmd, check=False, cwd=self.context_dir, env=self.environ) - # build the image for the profile - subprocess.run( + # start the container and build the image if not available + cmd = ( ["docker", "compose"] + self.add_yamls + self.add_profiles + self.add_env_files - + ["up", "--detach", "--build", "--remove-orphans"], - check=False, - cwd=self.context_dir, - env=self.environ, + + ["up", "--detach", "--build", "--remove-orphans"] ) + subprocess.run(cmd, check=False, cwd=self.context_dir, env=self.environ) def enter(self): """Enter the running container by executing a bash shell. @@ -137,34 +205,29 @@ def enter(self): """ if self.is_container_running(): print(f"[INFO] Entering the existing '{self.container_name}' container in a bash session...\n") - subprocess.run([ - "docker", - "exec", - "--interactive", - "--tty", - *(["-e", f"DISPLAY={os.environ['DISPLAY']}"] if "DISPLAY" in os.environ else []), - f"{self.container_name}", - "bash", - ]) + cmd = ( + ["docker", "exec", "--interactive", "--tty"] + + (["-e", f"DISPLAY={os.environ['DISPLAY']}"] if "DISPLAY" in os.environ else []) + + [self.container_name, "bash"] + ) + subprocess.run(cmd) else: raise RuntimeError(f"The container '{self.container_name}' is not running.") def stop(self): - """Stop the running container using the Docker compose command. - - Raises: - RuntimeError: If the container is not running. - """ + """Stop the running container using the Docker compose command.""" if self.is_container_running(): print(f"[INFO] Stopping the launched docker container '{self.container_name}'...\n") - subprocess.run( - ["docker", "compose"] + self.add_yamls + self.add_profiles + self.add_env_files + ["down"], - check=False, - cwd=self.context_dir, - env=self.environ, + # stop running services + cmd = ( + ["docker", "compose"] + self.add_yamls + self.add_profiles + self.add_env_files + ["down", "--volumes"] ) + subprocess.run(cmd, check=False, cwd=self.context_dir, env=self.environ) else: - raise RuntimeError(f"Can't stop container '{self.container_name}' as it is not running.") + print( + f"[INFO] Can't stop container '{self.container_name}' as it is not running." + " To check if the container is running, run 'docker ps' or 'docker container ls'.\n" + ) def copy(self, output_dir: Path | None = None): """Copy artifacts from the running container to the host machine. @@ -202,15 +265,8 @@ def copy(self, output_dir: Path | None = None): # copy the artifacts for container_path, host_path in artifacts.items(): - subprocess.run( - [ - "docker", - "cp", - f"isaac-lab-{self.profile}:{container_path}/", - f"{host_path}", - ], - check=False, - ) + cmd = ["docker", "cp", f"{self.container_name}:{container_path}/", host_path] + subprocess.run(cmd, check=False, cwd=self.context_dir, env=self.environ) print("\n[INFO] Finished copying the artifacts from the container.") else: raise RuntimeError(f"The container '{self.container_name}' is not running.") @@ -234,20 +290,16 @@ def config(self, output_yaml: Path | None = None): output = [] # run the docker compose config command to generate the configuration - subprocess.run( - ["docker", "compose"] + self.add_yamls + self.add_profiles + self.add_env_files + ["config"] + output, - check=False, - cwd=self.context_dir, - env=self.environ, - ) + cmd = ["docker", "compose"] + self.add_yamls + self.add_profiles + self.add_env_files + ["config"] + output + subprocess.run(cmd, check=False, cwd=self.context_dir, env=self.environ) """ Helper functions. """ def _resolve_image_extension(self, yamls: list[str] | None = None, envs: list[str] | None = None): - """ - Resolve the image extension by setting up YAML files, profiles, and environment files for the Docker compose command. + """Resolve the image extension by setting up YAML files, profiles, and environment files for the + Docker compose command. Args: yamls: A list of yaml files to extend ``docker-compose.yaml`` settings. These are extended in the order diff --git a/docker/utils/state_file.py b/docker/utils/state_file.py index d3bab18b80f9..505f272f4101 100644 --- a/docker/utils/state_file.py +++ b/docker/utils/state_file.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/docker/utils/x11_utils.py b/docker/utils/x11_utils.py index c9e5fc259423..4d7d0e3639f3 100644 --- a/docker/utils/x11_utils.py +++ b/docker/utils/x11_utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/docker/x11.yaml b/docker/x11.yaml index 73840f31de39..bd9b22f16b70 100644 --- a/docker/x11.yaml +++ b/docker/x11.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + services: isaac-lab-base: environment: diff --git a/docs/Makefile b/docs/Makefile index ce33dad50335..0bff236671c8 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 bd564df8e2f4..248e14c3f89c 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -20,14 +20,16 @@ sys.path.insert(0, os.path.abspath("../source/isaaclab")) sys.path.insert(0, os.path.abspath("../source/isaaclab/isaaclab")) +sys.path.insert(0, os.path.abspath("../source/isaaclab_assets")) +sys.path.insert(0, os.path.abspath("../source/isaaclab_assets/isaaclab_assets")) sys.path.insert(0, os.path.abspath("../source/isaaclab_tasks")) sys.path.insert(0, os.path.abspath("../source/isaaclab_tasks/isaaclab_tasks")) sys.path.insert(0, os.path.abspath("../source/isaaclab_rl")) sys.path.insert(0, os.path.abspath("../source/isaaclab_rl/isaaclab_rl")) sys.path.insert(0, os.path.abspath("../source/isaaclab_mimic")) sys.path.insert(0, os.path.abspath("../source/isaaclab_mimic/isaaclab_mimic")) -sys.path.insert(0, os.path.abspath("../source/isaaclab_assets")) -sys.path.insert(0, os.path.abspath("../source/isaaclab_assets/isaaclab_assets")) +sys.path.insert(0, os.path.abspath("../source/isaaclab_contrib")) +sys.path.insert(0, os.path.abspath("../source/isaaclab_contrib/isaaclab_contrib")) # -- Project information ----------------------------------------------------- @@ -87,6 +89,17 @@ # TODO: Enable this by default once we have fixed all the warnings # nitpicky = True +nitpick_ignore = [ + ("py:obj", "slice(None)"), +] + +nitpick_ignore_regex = [ + (r"py:.*", r"pxr.*"), # we don't have intersphinx mapping for pxr + (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" @@ -112,10 +125,12 @@ intersphinx_mapping = { "python": ("https://docs.python.org/3", None), "numpy": ("https://numpy.org/doc/stable/", None), - "torch": ("https://pytorch.org/docs/stable/", None), - "isaac": ("https://docs.omniverse.nvidia.com/py/isaacsim", None), + "trimesh": ("https://trimesh.org/", None), + "torch": ("https://docs.pytorch.org/docs/stable/", 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), + "omniverse": ("https://docs.omniverse.nvidia.com/dev-guide/latest", None), } # Add any paths that contain templates here, relative to this directory. @@ -144,20 +159,15 @@ "omni.client", "omni.physx", "omni.physics", + "usdrt", "pxr.PhysxSchema", "pxr.PhysicsSchemaTools", "omni.replicator", - "omni.isaac.core", - "omni.isaac.kit", - "omni.isaac.cloner", - "omni.isaac.urdf", - "omni.isaac.version", - "omni.isaac.motion_generation", - "omni.isaac.ui", "isaacsim", "isaacsim.core.api", "isaacsim.core.cloner", "isaacsim.core.version", + "isaacsim.core.utils", "isaacsim.robot_motion.motion_generation", "isaacsim.gui.components", "isaacsim.asset.importer.urdf", @@ -178,6 +188,15 @@ "tensordict", "trimesh", "toml", + "pink", + "pinocchio", + "nvidia.srl", + "flatdict", + "IPython", + "cv2", + "imageio", + "ipywidgets", + "mpl_toolkits", ] # List of zero or more Sphinx-specific warning categories to be squelched (i.e., @@ -225,6 +244,7 @@ html_css_files = ["custom.css"] html_theme_options = { + "path_to_docs": "docs/", "collapse_navigation": True, "repository_url": "https://github.com/isaac-sim/IsaacLab", "use_repository_button": True, @@ -247,7 +267,7 @@ { "name": "Isaac Sim", "url": "https://developer.nvidia.com/isaac-sim", - "icon": "https://img.shields.io/badge/IsaacSim-4.5.0-silver.svg", + "icon": "https://img.shields.io/badge/IsaacSim-5.1.0-silver.svg", "type": "url", }, { @@ -267,7 +287,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 f46590df20ee..97b5f851e082 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -33,11 +33,11 @@ Isaac lab is developed with specific robot assets that are now **Batteries-inclu - **Humanoids**: Unitree H1, Unitree G1 - **Quadcopter**: Crazyflie -The platform is also designed so that you can add your own robots! please refer to the +The platform is also designed so that you can add your own robots! Please refer to the :ref:`how-to` section for details. -For more information about the framework, please refer to the `paper `_ -:cite:`mittal2023orbit`. For clarifications on NVIDIA Isaac ecosystem, please check out the +For more information about the framework, please refer to the `technical report `_ +:cite:`mittal2025isaaclab`. For clarifications on NVIDIA Isaac ecosystem, please check out the :ref:`isaac-lab-ecosystem` section. .. figure:: source/_static/tasks.jpg @@ -48,69 +48,99 @@ For more information about the framework, please refer to the `paper `_ framework. We would appreciate if you would cite it in academic publications as well: +Citation +======== + +If you use Isaac Lab in your research, please cite our technical report: .. code:: bibtex - @article{mittal2023orbit, - author={Mittal, Mayank and Yu, Calvin and Yu, Qinxi and Liu, Jingzhou and Rudin, Nikita and Hoeller, David and Yuan, Jia Lin and Singh, Ritvik and Guo, Yunrong and Mazhar, Hammad and Mandlekar, Ajay and Babich, Buck and State, Gavriel and Hutter, Marco and Garg, Animesh}, - journal={IEEE Robotics and Automation Letters}, - title={Orbit: A Unified Simulation Framework for Interactive Robot Learning Environments}, - year={2023}, - volume={8}, - number={6}, - pages={3740-3747}, - doi={10.1109/LRA.2023.3270034} + @article{mittal2025isaaclab, + title={Isaac Lab: A GPU-Accelerated Simulation Framework for Multi-Modal Robot Learning}, + author={Mayank Mittal and Pascal Roth and James Tigue and Antoine Richard and Octi Zhang and Peter Du and Antonio Serrano-Muรฑoz and Xinjie Yao and Renรฉ Zurbrรผgg and Nikita Rudin and Lukasz Wawrzyniak and Milad Rakhsha and Alain Denzler and Eric Heiden and Ales Borovicka and Ossama Ahmed and Iretiayo Akinola and Abrar Anwar and Mark T. Carlson and Ji Yuan Feng and Animesh Garg and Renato Gasoto and Lionel Gulich and Yijie Guo and M. Gussert and Alex Hansen and Mihir Kulkarni and Chenran Li and Wei Liu and Viktor Makoviychuk and Grzegorz Malczyk and Hammad Mazhar and Masoud Moghani and Adithyavairavan Murali and Michael Noseworthy and Alexander Poddubny and Nathan Ratliff and Welf Rehberg and Clemens Schwarke and Ritvik Singh and James Latham Smith and Bingjie Tang and Ruchik Thaker and Matthew Trepte and Karl Van Wyk and Fangzhou Yu and Alex Millane and Vikram Ramasamy and Remo Steiner and Sangeeta Subramanian and Clemens Volk and CY Chen and Neel Jawale and Ashwin Varghese Kuruttukulam and Michael A. Lin and Ajay Mandlekar and Karsten Patzwaldt and John Welsh and Huihua Zhao and Fatima Anes and Jean-Francois Lafleche and Nicolas Moรซnne-Loccoz and Soowan Park and Rob Stepinski and Dirk Van Gelder and Chris Amevor and Jan Carius and Jumyung Chang and Anka He Chen and Pablo de Heras Ciechomski and Gilles Daviet and Mohammad Mohajerani and Julia von Muralt and Viktor Reutskyy and Michael Sauter and Simon Schirm and Eric L. Shi and Pierre Terdiman and Kenny Vilella and Tobias Widmer and Gordon Yeoman and Tiffany Chen and Sergey Grizan and Cathy Li and Lotus Li and Connor Smith and Rafael Wiltz and Kostas Alexis and Yan Chang and David Chu and Linxi "Jim" Fan and Farbod Farshidian and Ankur Handa and Spencer Huang and Marco Hutter and Yashraj Narang and Soha Pouya and Shiwei Sheng and Yuke Zhu and Miles Macklin and Adam Moravanszky and Philipp Reist and Yunrong Guo and David Hoeller and Gavriel State}, + journal={arXiv preprint arXiv:2511.04831}, + year={2025}, + url={https://arxiv.org/abs/2511.04831} } +Acknowledgement +=============== + +Isaac Lab development initiated from the `Orbit `_ framework. +We gratefully acknowledge the authors of Orbit for their foundational contributions. + + Table of Contents ================= .. toctree:: - :maxdepth: 2 - :caption: Getting Started + :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 + + +.. toctree:: + :maxdepth: 2 + :caption: Getting Started + :titlesonly: + + source/setup/quickstart + source/overview/own-project/index + source/setup/walkthrough/index + source/tutorials/index + source/how-to/index + source/overview/developer-guide/index + .. toctree:: :maxdepth: 3 :caption: Overview :titlesonly: - source/overview/developer-guide/index + source/overview/core-concepts/index source/overview/environments source/overview/reinforcement-learning/index - source/overview/teleop_imitation + source/overview/imitation-learning/index source/overview/showroom source/overview/simple_agents + .. toctree:: :maxdepth: 2 :caption: Features source/features/hydra source/features/multi_gpu + source/features/population_based_training Tiled Rendering source/features/ray source/features/reproducibility + +.. toctree:: + :maxdepth: 3 + :caption: Experimental Features + + source/experimental-features/bleeding-edge + source/experimental-features/newton-physics-integration/index + .. toctree:: :maxdepth: 1 :caption: Resources :titlesonly: - source/tutorials/index - source/how-to/index - source/deployment/index + source/setup/installation/cloud_installation + source/policy_deployment/index .. toctree:: :maxdepth: 1 @@ -131,7 +161,7 @@ Table of Contents :maxdepth: 1 :caption: References - source/refs/reference_architecture/index + source/refs/additional_resources source/refs/contributing source/refs/troubleshooting diff --git a/docs/licenses/assets/valkyrie-license b/docs/licenses/assets/valkyrie-license new file mode 100644 index 000000000000..8421ac1013df --- /dev/null +++ b/docs/licenses/assets/valkyrie-license @@ -0,0 +1,249 @@ +NASA OPEN SOURCE AGREEMENT VERSION 1.3 + +THIS OPEN SOURCE AGREEMENT ("AGREEMENT") DEFINES THE RIGHTS OF USE, +REPRODUCTION, DISTRIBUTION, MODIFICATION AND REDISTRIBUTION OF CERTAIN +COMPUTER SOFTWARE ORIGINALLY RELEASED BY THE UNITED STATES GOVERNMENT +AS REPRESENTED BY THE GOVERNMENT AGENCY LISTED BELOW ("GOVERNMENT +AGENCY"). THE UNITED STATES GOVERNMENT, AS REPRESENTED BY GOVERNMENT +AGENCY, IS AN INTENDED THIRD-PARTY BENEFICIARY OF ALL SUBSEQUENT +DISTRIBUTIONS OR REDISTRIBUTIONS OF THE SUBJECT SOFTWARE. ANYONE WHO +USES, REPRODUCES, DISTRIBUTES, MODIFIES OR REDISTRIBUTES THE SUBJECT +SOFTWARE, AS DEFINED HEREIN, OR ANY PART THEREOF, IS, BY THAT ACTION, +ACCEPTING IN FULL THE RESPONSIBILITIES AND OBLIGATIONS CONTAINED IN +THIS AGREEMENT. + +Government Agency: National Aeronautics and Space Administration (NASA) +Government Agency Original Software Designation: GSC-16256-1 +Government Agency Original Software Title: "ISTP CDF Skeleton Editor" +User Registration Requested. Please Visit https://spdf.gsfc.nasa.gov/ +Government Agency Point of Contact for Original Software: + NASA-SPDF-Support@nasa.onmicrosoft.com + + +1. DEFINITIONS + +A. "Contributor" means Government Agency, as the developer of the +Original Software, and any entity that makes a Modification. +B. "Covered Patents" mean patent claims licensable by a Contributor +that are necessarily infringed by the use or sale of its Modification +alone or when combined with the Subject Software. +C. "Display" means the showing of a copy of the Subject Software, +either directly or by means of an image, or any other device. +D. "Distribution" means conveyance or transfer of the Subject +Software, regardless of means, to another. +E. "Larger Work" means computer software that combines Subject +Software, or portions thereof, with software separate from the Subject +Software that is not governed by the terms of this Agreement. +F. "Modification" means any alteration of, including addition to or +deletion from, the substance or structure of either the Original +Software or Subject Software, and includes derivative works, as that +term is defined in the Copyright Statute, 17 USC 101. However, the +act of including Subject Software as part of a Larger Work does not in +and of itself constitute a Modification. +G. "Original Software" means the computer software first released +under this Agreement by Government Agency with Government Agency +designation "GSC-16256-1"" and entitled +"ISTP CDF Skeleton Editor", including source code, +object code and accompanying documentation, if any. +H. "Recipient" means anyone who acquires the Subject Software under +this Agreement, including all Contributors. +I. "Redistribution" means Distribution of the Subject Software after a +Modification has been made. +J. "Reproduction" means the making of a counterpart, image or copy of +the Subject Software. +K. "Sale" means the exchange of the Subject Software for money or +equivalent value. +L. "Subject Software" means the Original Software, Modifications, or +any respective parts thereof. +M. "Use" means the application or employment of the Subject Software +for any purpose. + +2. GRANT OF RIGHTS + +A. Under Non-Patent Rights: Subject to the terms and conditions of +this Agreement, each Contributor, with respect to its own contribution +to the Subject Software, hereby grants to each Recipient a +non-exclusive, world-wide, royalty-free license to engage in the +following activities pertaining to the Subject Software: + +1. Use +2. Distribution +3. Reproduction +4. Modification +5. Redistribution +6. Display + +B. Under Patent Rights: Subject to the terms and conditions of this +Agreement, each Contributor, with respect to its own contribution to +the Subject Software, hereby grants to each Recipient under Covered +Patents a non-exclusive, world-wide, royalty-free license to engage in +the following activities pertaining to the Subject Software: + +1. Use +2. Distribution +3. Reproduction +4. Sale +5. Offer for Sale + +C. The rights granted under Paragraph B. also apply to the combination +of a Contributor's Modification and the Subject Software if, at the +time the Modification is added by the Contributor, the addition of +such Modification causes the combination to be covered by the Covered +Patents. It does not apply to any other combinations that include a +Modification. + +D. The rights granted in Paragraphs A. and B. allow the Recipient to +sublicense those same rights. Such sublicense must be under the same +terms and conditions of this Agreement. + +3. OBLIGATIONS OF RECIPIENT + +A. Distribution or Redistribution of the Subject Software must be made +under this Agreement except for additions covered under paragraph 3H. + +1. Whenever a Recipient distributes or redistributes the Subject + Software, a copy of this Agreement must be included with each copy + of the Subject Software; and +2. If Recipient distributes or redistributes the Subject Software in + any form other than source code, Recipient must also make the + source code freely available, and must provide with each copy of + the Subject Software information on how to obtain the source code + in a reasonable manner on or through a medium customarily used for + software exchange. + +B. Each Recipient must ensure that the following copyright notice +appears prominently in the Subject Software: + +Copyright (c) 2006 United States Government as represented by the +National Aeronautics and Space Administration. No copyright is claimed +in the United States under Title 17, U.S.Code. All Other Rights Reserved. + +C. Each Contributor must characterize its alteration of the Subject +Software as a Modification and must identify itself as the originator +of its Modification in a manner that reasonably allows subsequent +Recipients to identify the originator of the Modification. In +fulfillment of these requirements, Contributor must include a file +(e.g., a change log file) that describes the alterations made and the +date of the alterations, identifies Contributor as originator of the +alterations, and consents to characterization of the alterations as a +Modification, for example, by including a statement that the +Modification is derived, directly or indirectly, from Original +Software provided by Government Agency. Once consent is granted, it +may not thereafter be revoked. + +D. A Contributor may add its own copyright notice to the Subject +Software. Once a copyright notice has been added to the Subject +Software, a Recipient may not remove it without the express permission +of the Contributor who added the notice. + +E. A Recipient may not make any representation in the Subject Software +or in any promotional, advertising or other material that may be +construed as an endorsement by Government Agency or by any prior +Recipient of any product or service provided by Recipient, or that may +seek to obtain commercial advantage by the fact of Government Agency's +or a prior Recipient's participation in this Agreement. + +F. In an effort to track usage and maintain accurate records of the +Subject Software, each Recipient, upon receipt of the Subject +Software, is requested to register with Government Agency by visiting +the following website: https://opensource.gsfc.nasa.gov/. Recipient's +name and personal information shall be used for statistical purposes +only. Once a Recipient makes a Modification available, it is requested +that the Recipient inform Government Agency at the web site provided +above how to access the Modification. + +G. Each Contributor represents that that its Modification is believed +to be Contributor's original creation and does not violate any +existing agreements, regulations, statutes or rules, and further that +Contributor has sufficient rights to grant the rights conveyed by this +Agreement. + +H. A Recipient may choose to offer, and to charge a fee for, warranty, +support, indemnity and/or liability obligations to one or more other +Recipients of the Subject Software. A Recipient may do so, however, +only on its own behalf and not on behalf of Government Agency or any +other Recipient. Such a Recipient must make it absolutely clear that +any such warranty, support, indemnity and/or liability obligation is +offered by that Recipient alone. Further, such Recipient agrees to +indemnify Government Agency and every other Recipient for any +liability incurred by them as a result of warranty, support, indemnity +and/or liability offered by such Recipient. + +I. A Recipient may create a Larger Work by combining Subject Software +with separate software not governed by the terms of this agreement and +distribute the Larger Work as a single product. In such case, the +Recipient must make sure Subject Software, or portions thereof, +included in the Larger Work is subject to this Agreement. + +J. Notwithstanding any provisions contained herein, Recipient is +hereby put on notice that export of any goods or technical data from +the United States may require some form of export license from the +U.S. Government. Failure to obtain necessary export licenses may +result in criminal liability under U.S. laws. Government Agency +neither represents that a license shall not be required nor that, if +required, it shall be issued. Nothing granted herein provides any +such export license. + +4. DISCLAIMER OF WARRANTIES AND LIABILITIES; WAIVER AND INDEMNIFICATION + +A. No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY +WARRANTY OF ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, +INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE +WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM +INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE ERROR +FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO +THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, +CONSTITUTE AN ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT +OF ANY RESULTS, RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY +OTHER APPLICATIONS RESULTING FROM USE OF THE SUBJECT SOFTWARE. +FURTHER, GOVERNMENT AGENCY DISCLAIMS ALL WARRANTIES AND LIABILITIES +REGARDING THIRD-PARTY SOFTWARE, IF PRESENT IN THE ORIGINAL SOFTWARE, +AND DISTRIBUTES IT "AS IS." + +B. Waiver and Indemnity: RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS +AGAINST THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND +SUBCONTRACTORS, AS WELL AS ANY PRIOR RECIPIENT. IF RECIPIENT'S USE OF +THE SUBJECT SOFTWARE RESULTS IN ANY LIABILITIES, DEMANDS, DAMAGES, +EXPENSES OR LOSSES ARISING FROM SUCH USE, INCLUDING ANY DAMAGES FROM +PRODUCTS BASED ON, OR RESULTING FROM, RECIPIENT'S USE OF THE SUBJECT +SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD HARMLESS THE UNITED +STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL AS ANY +PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW. RECIPIENT'S SOLE +REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE, UNILATERAL +TERMINATION OF THIS AGREEMENT. + + +5. GENERAL TERMS + +A. Termination: This Agreement and the rights granted hereunder will +terminate automatically if a Recipient fails to comply with these +terms and conditions, and fails to cure such noncompliance within +thirty (30) days of becoming aware of such noncompliance. Upon +termination, a Recipient agrees to immediately cease use and +distribution of the Subject Software. All sublicenses to the Subject +Software properly granted by the breaching Recipient shall survive any +such termination of this Agreement. + +B. Severability: If any provision of this Agreement is invalid or +unenforceable under applicable law, it shall not affect the validity +or enforceability of the remainder of the terms of this Agreement. + +C. Applicable Law: This Agreement shall be subject to United States +federal law only for all purposes, including, but not limited to, +determining the validity of this Agreement, the meaning of its +provisions and the rights, obligations and remedies of the parties. + +D. Entire Understanding: This Agreement constitutes the entire +understanding and agreement of the parties relating to release of the +Subject Software and may not be superseded, modified or amended except +by further written agreement duly executed by the parties. + +E. Binding Authority: By accepting and using the Subject Software +under this Agreement, a Recipient affirms its authority to bind the +Recipient to all terms and conditions of this Agreement and that that +Recipient hereby agrees to all terms and conditions herein. + +F. Point of Contact: Any Recipient contact with Government Agency is +to be directed to the designated representative as follows: +NASA-SPDF-Support@nasa.onmicrosoft.com. diff --git a/docs/licenses/dependencies/Farama-Notifications-license.txt b/docs/licenses/dependencies/Farama-Notifications-license.txt new file mode 100644 index 000000000000..44a6bbb5b5c2 --- /dev/null +++ b/docs/licenses/dependencies/Farama-Notifications-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Farama Foundation + +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/GitPython-license.txt b/docs/licenses/dependencies/GitPython-license.txt new file mode 100644 index 000000000000..ba8a219fe1f2 --- /dev/null +++ b/docs/licenses/dependencies/GitPython-license.txt @@ -0,0 +1,29 @@ +Copyright (C) 2008, 2009 Michael Trier and contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +* Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. + +* 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. + +* Neither the name of the GitPython project 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 +OWNER 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/InquirerPy-license.txt b/docs/licenses/dependencies/InquirerPy-license.txt new file mode 100644 index 000000000000..7f22fe2c5cf4 --- /dev/null +++ b/docs/licenses/dependencies/InquirerPy-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2020 Kevin Zhuang + +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/absl-py-license.txt b/docs/licenses/dependencies/absl-py-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/absl-py-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/accessible-pygments-license.txt b/docs/licenses/dependencies/accessible-pygments-license.txt new file mode 100644 index 000000000000..6779ad46534b --- /dev/null +++ b/docs/licenses/dependencies/accessible-pygments-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2022, Quansight Labs +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. diff --git a/docs/licenses/dependencies/alabaster-license.txt b/docs/licenses/dependencies/alabaster-license.txt new file mode 100644 index 000000000000..8f1c1979d3d8 --- /dev/null +++ b/docs/licenses/dependencies/alabaster-license.txt @@ -0,0 +1,12 @@ +Copyright (c) 2020 Jeff Forcier. + +Based on original work copyright (c) 2011 Kenneth Reitz and copyright (c) 2010 Armin Ronacher. + +Some rights reserved. + +Redistribution and use in source and binary forms of the theme, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +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. +The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission. +THIS THEME 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 OWNER 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 THEME, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/docs/licenses/dependencies/antlr4_python3_runtime-license.txt b/docs/licenses/dependencies/antlr4_python3_runtime-license.txt new file mode 100644 index 000000000000..5d276941558a --- /dev/null +++ b/docs/licenses/dependencies/antlr4_python3_runtime-license.txt @@ -0,0 +1,28 @@ +Copyright (c) 2012-2022 The ANTLR Project. 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 name of copyright holders 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 REGENTS 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/anyio-license.txt b/docs/licenses/dependencies/anyio-license.txt new file mode 100644 index 000000000000..104eebf5a300 --- /dev/null +++ b/docs/licenses/dependencies/anyio-license.txt @@ -0,0 +1,20 @@ +The MIT License (MIT) + +Copyright (c) 2018 Alex Grรถnholm + +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/anytree-license.txt b/docs/licenses/dependencies/anytree-license.txt new file mode 100644 index 000000000000..8dada3edaf50 --- /dev/null +++ b/docs/licenses/dependencies/anytree-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/assimp-license.txt b/docs/licenses/dependencies/assimp-license.txt new file mode 100644 index 000000000000..c66fa94f441e --- /dev/null +++ b/docs/licenses/dependencies/assimp-license.txt @@ -0,0 +1,78 @@ +Open Asset Import Library (assimp) + +Copyright (c) 2006-2021, assimp team +All rights reserved. + +Redistribution and use of this software in source and binary forms, +with or without modification, are permitted provided that the +following conditions are met: + +* Redistributions of source code must retain the above + copyright notice, this list of conditions and the + following disclaimer. + +* 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. + +* Neither the name of the assimp team, nor the names of its + contributors may be used to endorse or promote products + derived from this software without specific prior + written permission of the assimp team. + +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 +OWNER 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. + + + +****************************************************************************** + +AN EXCEPTION applies to all files in the ./test/models-nonbsd folder. +These are 3d models for testing purposes, from various free sources +on the internet. They are - unless otherwise stated - copyright of +their respective creators, which may impose additional requirements +on the use of their work. For any of these models, see +.source.txt for more legal information. Contact us if you +are a copyright holder and believe that we credited you improperly or +if you don't want your files to appear in the repository. + + +****************************************************************************** + +Poly2Tri Copyright (c) 2009-2010, Poly2Tri Contributors +http://code.google.com/p/poly2tri/ + +All rights reserved. +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +* 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. +* Neither the name of Poly2Tri 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 OWNER 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/asttokens-license.txt b/docs/licenses/dependencies/asttokens-license.txt new file mode 100644 index 000000000000..8dada3edaf50 --- /dev/null +++ b/docs/licenses/dependencies/asttokens-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/attrs-license b/docs/licenses/dependencies/attrs-license new file mode 100644 index 000000000000..2bd6453d255e --- /dev/null +++ b/docs/licenses/dependencies/attrs-license @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2015 Hynek Schlawack and the attrs 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/babel-license.txt b/docs/licenses/dependencies/babel-license.txt new file mode 100644 index 000000000000..f31575ec773b --- /dev/null +++ b/docs/licenses/dependencies/babel-license.txt @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2014-present Sebastian McKenzie and other 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/beautifulsoup4-license.txt b/docs/licenses/dependencies/beautifulsoup4-license.txt new file mode 100644 index 000000000000..d668d13f041e --- /dev/null +++ b/docs/licenses/dependencies/beautifulsoup4-license.txt @@ -0,0 +1,26 @@ +Beautiful Soup is made available under the MIT license: + + Copyright (c) 2004-2012 Leonard Richardson + + 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, DAMMIT. + +Beautiful Soup incorporates code from the html5lib library, which is +also made available under the MIT license. diff --git a/docs/licenses/dependencies/boost-license.txt b/docs/licenses/dependencies/boost-license.txt new file mode 100644 index 000000000000..36b7cd93cdfb --- /dev/null +++ b/docs/licenses/dependencies/boost-license.txt @@ -0,0 +1,23 @@ +Boost Software License - Version 1.0 - August 17th, 2003 + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +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, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN 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/botocore-license.txt b/docs/licenses/dependencies/botocore-license.txt new file mode 100644 index 000000000000..d9a10c0d8e86 --- /dev/null +++ b/docs/licenses/dependencies/botocore-license.txt @@ -0,0 +1,176 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/docs/licenses/dependencies/certifi-license.txt b/docs/licenses/dependencies/certifi-license.txt new file mode 100644 index 000000000000..62b076cdee58 --- /dev/null +++ b/docs/licenses/dependencies/certifi-license.txt @@ -0,0 +1,20 @@ +This package contains a modified version of ca-bundle.crt: + +ca-bundle.crt -- Bundle of CA Root Certificates + +This is a bundle of X.509 certificates of public Certificate Authorities +(CA). These were automatically extracted from Mozilla's root certificates +file (certdata.txt). This file can be found in the mozilla source tree: +https://hg.mozilla.org/mozilla-central/file/tip/security/nss/lib/ckfw/builtins/certdata.txt +It contains the certificates in PEM format and therefore +can be directly used with curl / libcurl / php_curl, or with +an Apache+mod_ssl webserver for SSL client authentication. +Just configure this file as the SSLCACertificateFile.# + +***** BEGIN LICENSE BLOCK ***** +This Source Code Form is subject to the terms of the Mozilla Public License, +v. 2.0. If a copy of the MPL was not distributed with this file, You can obtain +one at http://mozilla.org/MPL/2.0/. + +***** END LICENSE BLOCK ***** +@(#) $RCSfile: certdata.txt,v $ $Revision: 1.80 $ $Date: 2011/11/03 15:11:58 $ diff --git a/docs/licenses/dependencies/charset-normalizer-license.txt b/docs/licenses/dependencies/charset-normalizer-license.txt new file mode 100644 index 000000000000..9725772c7967 --- /dev/null +++ b/docs/licenses/dependencies/charset-normalizer-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2025 TAHRI Ahmed R. + +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/click-license.txt b/docs/licenses/dependencies/click-license.txt new file mode 100644 index 000000000000..d12a84918698 --- /dev/null +++ b/docs/licenses/dependencies/click-license.txt @@ -0,0 +1,28 @@ +Copyright 2014 Pallets + +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. diff --git a/docs/licenses/dependencies/cloudpickle-license.txt b/docs/licenses/dependencies/cloudpickle-license.txt new file mode 100644 index 000000000000..d112c4806aa9 --- /dev/null +++ b/docs/licenses/dependencies/cloudpickle-license.txt @@ -0,0 +1,32 @@ +This module was extracted from the `cloud` package, developed by +PiCloud, Inc. + +Copyright (c) 2015, Cloudpickle contributors. +Copyright (c) 2012, Regents of the University of California. +Copyright (c) 2009 PiCloud, Inc. http://www.picloud.com. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * 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. + * Neither the name of the University of California, Berkeley 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. diff --git a/docs/licenses/dependencies/cmeel-license.txt b/docs/licenses/dependencies/cmeel-license.txt new file mode 100644 index 000000000000..664756eae98c --- /dev/null +++ b/docs/licenses/dependencies/cmeel-license.txt @@ -0,0 +1,25 @@ +BSD 2-Clause License + +Copyright (c) 2022, Guilhem Saurel +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/colorama-license.txt b/docs/licenses/dependencies/colorama-license.txt new file mode 100644 index 000000000000..3105888ec149 --- /dev/null +++ b/docs/licenses/dependencies/colorama-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2010 Jonathan Hartley +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* Neither the name of the copyright holders, nor those 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. diff --git a/docs/licenses/dependencies/comm-license.txt b/docs/licenses/dependencies/comm-license.txt new file mode 100644 index 000000000000..eee1b58d9ed0 --- /dev/null +++ b/docs/licenses/dependencies/comm-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2022, Jupyter +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. diff --git a/docs/licenses/dependencies/console-bridge-license.txt b/docs/licenses/dependencies/console-bridge-license.txt new file mode 100644 index 000000000000..574ef0790290 --- /dev/null +++ b/docs/licenses/dependencies/console-bridge-license.txt @@ -0,0 +1,25 @@ +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * 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. + + * 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. diff --git a/docs/licenses/dependencies/contourpy-license.txt b/docs/licenses/dependencies/contourpy-license.txt new file mode 100644 index 000000000000..93e41fb6965a --- /dev/null +++ b/docs/licenses/dependencies/contourpy-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2021-2025, ContourPy Developers. +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. diff --git a/docs/licenses/dependencies/cuRobo-license.txt b/docs/licenses/dependencies/cuRobo-license.txt new file mode 100644 index 000000000000..2b76a56cbf86 --- /dev/null +++ b/docs/licenses/dependencies/cuRobo-license.txt @@ -0,0 +1,93 @@ +NVIDIA ISAAC LAB ADDITIONAL SOFTWARE AND MATERIALS LICENSE + +IMPORTANT NOTICE โ€“ PLEASE READ AND AGREE BEFORE USING THE SOFTWARE + +This software license agreement ("Agreement") is a legal agreement between you, whether an individual or entity, ("you") and NVIDIA Corporation ("NVIDIA") and governs the use of the NVIDIA cuRobo and related software and materials that NVIDIA delivers to you under this Agreement ("Software"). NVIDIA and you are each a "party" and collectively the "parties." + +By using the Software, you are affirming that you have read and agree to this Agreement. + +If you don't accept all the terms and conditions below, do not use the Software. + +1. License Grant. The Software made available by NVIDIA to you is licensed, not sold. Subject to the terms of this Agreement, NVIDIA grants you a limited, non-exclusive, revocable, non-transferable, and non-sublicensable (except as expressly granted in this Agreement), license to install and use copies of the Software together with NVIDIA Isaac Lab in systems with NVIDIA GPUs ("Purpose"). + +2. License Restrictions. Your license to use the Software is restricted as stated in this Section 2 ("License Restrictions"). You will cooperate with NVIDIA and, upon NVIDIA's written request, you will confirm in writing and provide reasonably requested information to verify your compliance with the terms of this Agreement. You may not: + +2.1 Use the Software for any purpose other than the Purpose, and for clarity use of NVIDIA cuRobo apart from use with Isaac Lab is outside of the Purpose; + +2.2 Sell, rent, sublicense, transfer, distribute or otherwise make available to others (except authorized users as stated in Section 3 ("Authorized Users")) any portion of the Software, except as expressly granted in Section 1 ("License Grant"); + +2.3 Reverse engineer, decompile, or disassemble the Software components provided in binary form, nor attempt in any other manner to obtain source code of such Software; + +2.4 Modify or create derivative works of the Software; + +2.5 Change or remove copyright or other proprietary notices in the Software; + +2.6 Bypass, disable, or circumvent any technical limitation, encryption, security, digital rights management or authentication mechanism in the Software; + +2.7 Use the Software in any manner that would cause them to become subject to an open source software license, subject to the terms in Section 7 ("Components Under Other Licenses"); or + +2.8 Use the Software in violation of any applicable law or regulation in relevant jurisdictions. + +3. Authorized Users. You may allow employees and contractors of your entity or of your subsidiary(ies), and for educational institutions also enrolled students, to internally access and use the Software as authorized by this Agreement from your secure network to perform the work authorized by this Agreement on your behalf. You are responsible for the compliance with the terms of this Agreement by your authorized users. Any act or omission that if committed by you would constitute a breach of this Agreement will be deemed to constitute a breach of this Agreement if committed by your authorized users. + +4. Pre-Release. Software versions identified as alpha, beta, preview, early access or otherwise as pre-release ("Pre-Release") may not be fully functional, may contain errors or design flaws, and may have reduced or different security, privacy, availability and reliability standards relative to NVIDIA commercial offerings. You use Pre-Release Software at your own risk. NVIDIA did not design or test the Software for use in production or business-critical systems. NVIDIA may choose not to make available a commercial version of Pre-Release Software. NVIDIA may also choose to abandon development and terminate the availability of Pre-Release Software at any time without liability. + +5. Updates. NVIDIA may at any time and at its option, change, discontinue, or deprecate any part, or all, of the Software, or change or remove features or functionality, or make available patches, workarounds or other updates to the Software. Unless the updates are provided with their separate governing terms, they are deemed part of the Software licensed to you under this Agreement, and your continued use of the Software is deemed acceptance of such changes. + +6. Components Under Other Licenses. The Software may include or be distributed with components provided with separate legal notices or terms that accompany the components, such as open source software licenses and other license terms ("Other Licenses"). The components are subject to the applicable Other Licenses, including any proprietary notices, disclaimers, requirements and extended use rights; except that this Agreement will prevail regarding the use of third-party open source software, unless a third-party open source software license requires its license terms to prevail. Open source software license means any software, data or documentation subject to any license identified as an open source license by the Open Source Initiative (http://opensource.org), Free Software Foundation (http://www.fsf.org) or other similar open source organization or listed by the Software Package Data Exchange (SPDX) Workgroup under the Linux Foundation (http://www.spdx.org). + +7. Ownership. The Software, including all intellectual property rights, is and will remain the sole and exclusive property of NVIDIA or its licensors. Except as expressly granted in this Agreement, (a) NVIDIA reserves all rights, interests and remedies in connection with the Software, and (b) no other license or right is granted to you by implication, estoppel or otherwise. + +8. Feedback. You may, but you are not obligated to, provide suggestions, requests, fixes, modifications, enhancements, or other feedback regarding the Software (collectively, "Feedback"). Feedback, even if designated as confidential by you, will not create any confidentiality obligation for NVIDIA or its affiliates. If you provide Feedback, you grant NVIDIA, its affiliates and its designees a non-exclusive, perpetual, irrevocable, sublicensable, worldwide, royalty-free, fully paid-up and transferable license, under your intellectual property rights, to publicly perform, publicly display, reproduce, use, make, have made, sell, offer for sale, distribute (through multiple tiers of distribution), import, create derivative works of and otherwise commercialize and exploit the Feedback at NVIDIA's discretion. + +9. Term and Termination. + +9.1 Term and Termination for Convenience. This license ends by July 31, 2026 or earlier at your choice if you finished using the Software for the Purpose. Either party may terminate this Agreement at any time with thirty (30) days' advance written notice to the other party. + +9.2 Termination for Cause. If you commence or participate in any legal proceeding against NVIDIA with respect to the Software, this Agreement will terminate immediately without notice. Either party may terminate this Agreement for cause if: + +(a) The other party fails to cure a material breach of this Agreement within ten (10) days of the non-breaching party's written notice of the breach; or + +(b) the other party breaches its confidentiality obligations or license rights under this Agreement, which termination will be effective immediately upon written notice. + +9.3 Effect of Termination. Upon any expiration or termination of this Agreement, you will promptly stop using and return, delete or destroy NVIDIA confidential information and all Software received under this Agreement. Upon written request, you will certify in writing that you have complied with your obligations under this Section 9.3 ("Effect of Termination"). + +9.4 Survival. Section 5 ("Updates"), Section 6 ("Components Under Other Licenses"), Section 7 ("Ownership"), Section 8 ("Feedback"), Section 9.3 ("Effect of Termination"), Section 9.4 ("Survival"), Section 10 ("Disclaimer of Warranties"), Section 11 ("Limitation of Liability"), Section 12 ("Use in Mission Critical Applications"), Section 13 ("Governing Law and Jurisdiction") and Section 14 ("General") will survive any expiration or termination of this Agreement. + +10. Disclaimer of Warranties. THE SOFTWARE IS PROVIDED BY NVIDIA AS-IS AND WITH ALL FAULTS. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW, NVIDIA DISCLAIMS ALL WARRANTIES AND REPRESENTATIONS OF ANY KIND, WHETHER EXPRESS, IMPLIED OR STATUTORY, RELATING TO OR ARISING UNDER THIS AGREEMENT, INCLUDING, WITHOUT LIMITATION, THE WARRANTIES OF TITLE, NONINFRINGEMENT, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, USAGE OF TRADE AND COURSE OF DEALING. NVIDIA DOES NOT WARRANT OR ASSUME RESPONSIBILITY FOR THE ACCURACY OR COMPLETENESS OF ANY THIRD-PARTY INFORMATION, TEXT, GRAPHICS, LINKS CONTAINED IN THE SOFTWARE. WITHOUT LIMITING THE FOREGOING, NVIDIA DOES NOT WARRANT THAT THE SOFTWARE WILL MEET YOUR REQUIREMENTS, ANY DEFECTS OR ERRORS WILL BE CORRECTED, ANY CERTAIN CONTENT WILL BE AVAILABLE; OR THAT THE SOFTWARE IS FREE OF VIRUSES OR OTHER HARMFUL COMPONENTS. NO INFORMATION OR ADVICE GIVEN BY NVIDIA WILL IN ANY WAY INCREASE THE SCOPE OF ANY WARRANTY EXPRESSLY PROVIDED IN THIS AGREEMENT. + +11. Limitations of Liability. + +11.1 EXCLUSIONS. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT WILL NVIDIA BE LIABLE FOR ANY (I) INDIRECT, PUNITIVE, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES, OR (II) DAMAGES FOR (A) THE COST OF PROCURING SUBSTITUTE GOODS, OR (B) LOSS OF PROFITS, REVENUES, USE, DATA OR GOODWILL ARISING OUT OF OR RELATED TO THIS AGREEMENT, WHETHER BASED ON BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE), STRICT LIABILITY, OR OTHERWISE, AND EVEN IF NVIDIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES AND EVEN IF A PARTY'S REMEDIES FAIL THEIR ESSENTIAL PURPOSE. + +11.2 DAMAGES CAP. ADDITIONALLY, TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW, NVIDIA'S TOTAL CUMULATIVE AGGREGATE LIABILITY FOR ANY AND ALL LIABILITIES, OBLIGATIONS OR CLAIMS ARISING OUT OF OR RELATED TO THIS AGREEMENT WILL NOT EXCEED FIVE U.S. DOLLARS (US$5). + +12. Use in Mission Critical Applications. You acknowledge that the Software provided under this Agreement is not designed or tested by NVIDIA for use in any system or application where the use or failure of such system or application developed with NVIDIA's Software could result in injury, death or catastrophic damage (each, a "Mission Critical Application"). Examples of Mission Critical Applications include use in avionics, navigation, autonomous vehicle applications, AI solutions for automotive products, military, medical, life support or other mission-critical or life-critical applications. NVIDIA will not be liable to you or any third party, in whole or in part, for any claims or damages arising from these uses. You are solely responsible for ensuring that systems and applications developed with the Software include sufficient safety and redundancy features and comply with all applicable legal and regulatory standards and requirements. + +13. Governing Law and Jurisdiction. This Agreement will be governed in all respects by the laws of the United States and the laws of the State of Delaware, without regard to conflict of laws principles or the United Nations Convention on Contracts for the International Sale of Goods. The state and federal courts residing in Santa Clara County, California will have exclusive jurisdiction over any dispute or claim arising out of or related to this Agreement, and the parties irrevocably consent to personal jurisdiction and venue in those courts; except that either party may apply for injunctive remedies or an equivalent type of urgent legal relief in any jurisdiction. + +14. General. + +14.1 Indemnity. By using the Software you agree to defend, indemnify and hold harmless NVIDIA and its affiliates and their respective officers, directors, employees and agents from and against any claims, disputes, demands, liabilities, damages, losses, costs and expenses arising out of or in any way connected with (i) products or services that have been developed or deployed with or use the Software, or claims that they violate laws, or infringe, violate, or misappropriate any third party right; or (ii) your use of the Software in breach of the terms of this Agreement. + +14.2 Independent Contractors. The parties are independent contractors, and this Agreement does not create a joint venture, partnership, agency, or other form of business association between the parties. Neither party will have the power to bind the other party or incur any obligation on its behalf without the other party's prior written consent. Nothing in this Agreement prevents either party from participating in similar arrangements with third parties. + +14.3 No Assignment. NVIDIA may assign, delegate or transfer its rights or obligations under this Agreement by any means or operation of law. You may not, without NVIDIA's prior written consent, assign, delegate or transfer any of your rights or obligations under this Agreement by any means or operation of law, and any attempt to do so is null and void. + +14.4 No Waiver. No failure or delay by a party to enforce any term or obligation of this Agreement will operate as a waiver by that party, or prevent the enforcement of such term or obligation later. + +14.5 Trade Compliance. You agree to comply with all applicable export, import, trade and economic sanctions laws and regulations, as amended, including without limitation U.S. Export Administration Regulations and Office of Foreign Assets Control regulations. You confirm (a) your understanding that export or reexport of certain NVIDIA products or technologies may require a license or other approval from appropriate authorities and (b) that you will not export or reexport any products or technology, directly or indirectly, without first obtaining any required license or other approval from appropriate authorities, (i) to any countries that are subject to any U.S. or local export restrictions (currently including, but not necessarily limited to, Belarus, Cuba, Iran, North Korea, Russia, Syria, the Region of Crimea, Donetsk People's Republic Region and Luhansk People's Republic Region); (ii) to any end-user who you know or have reason to know will utilize them in the design, development or production of nuclear, chemical or biological weapons, missiles, rocket systems, unmanned air vehicles capable of a maximum range of at least 300 kilometers, regardless of payload, or intended for military end-use, or any weapons of mass destruction; (iii) to any end-user who has been prohibited from participating in the U.S. or local export transactions by any governing authority; or (iv) to any known military or military-intelligence end-user or for any known military or military-intelligence end-use in accordance with U.S. trade compliance laws and regulations. + +14.6 Government Rights. The Software, documentation and technology ("Protected Items") are "Commercial products" as this term is defined at 48 C.F.R. 2.101, consisting of "commercial computer software" and "commercial computer software documentation" as such terms are used in, respectively, 48 C.F.R. 12.212 and 48 C.F.R. 227.7202 & 252.227-7014(a)(1). Before any Protected Items are supplied to the U.S. Government, you will (i) inform the U.S. Government in writing that the Protected Items are and must be treated as commercial computer software and commercial computer software documentation developed at private expense; (ii) inform the U.S. Government that the Protected Items are provided subject to the terms of the Agreement; and (iii) mark the Protected Items as commercial computer software and commercial computer software documentation developed at private expense. In no event will you permit the U.S. Government to acquire rights in Protected Items beyond those specified in 48 C.F.R. 52.227-19(b)(1)-(2) or 252.227-7013(c) except as expressly approved by NVIDIA in writing. + +14.7 Notices. Please direct your legal notices or other correspondence to legalnotices@nvidia.com with a copy mailed to NVIDIA Corporation, 2788 San Tomas Expressway, Santa Clara, California 95051, United States of America, Attention: Legal Department. If NVIDIA needs to contact you, you consent to receive the notices by email and agree that such notices will satisfy any legal communication requirements. + +14.8 Severability. If a court of competent jurisdiction rules that a provision of this Agreement is unenforceable, that provision will be deemed modified to the extent necessary to make it enforceable and the remainder of this Agreement will continue in full force and effect. + +14.9 Construction. The headings in the Agreement are included solely for convenience and are not intended to affect the meaning or interpretation of the Agreement. As required by the context of the Agreement, the singular of a term includes the plural and vice versa. + +14.10 Amendment. Any amendment to this Agreement must be in writing and signed by authorized representatives of both parties. + +14.11 Entire Agreement. Regarding the subject matter of this Agreement, the parties agree that (a) this Agreement constitutes the entire and exclusive agreement between the parties and supersedes all prior and contemporaneous communications and (b) any additional or different terms or conditions, whether contained in purchase orders, order acknowledgments, invoices or otherwise, will not be binding and are null and void. + +(v. August 15, 2025) diff --git a/docs/licenses/dependencies/cycler-license.txt b/docs/licenses/dependencies/cycler-license.txt new file mode 100644 index 000000000000..539c7c1f9c7c --- /dev/null +++ b/docs/licenses/dependencies/cycler-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2015, matplotlib project +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* Neither the name of the matplotlib project 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. diff --git a/docs/licenses/dependencies/decorator-license.txt b/docs/licenses/dependencies/decorator-license.txt new file mode 100644 index 000000000000..3e01d05b189d --- /dev/null +++ b/docs/licenses/dependencies/decorator-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2005-2025, Michele Simionato +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + +* Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +* 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 +HOLDERS 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/dex-retargeting-license.txt b/docs/licenses/dependencies/dex-retargeting-license.txt new file mode 100644 index 000000000000..673ea4b65ddf --- /dev/null +++ b/docs/licenses/dependencies/dex-retargeting-license.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2023 Yuzhe Qin + +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/docker-pycreds-license.txt b/docs/licenses/dependencies/docker-pycreds-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/docker-pycreds-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/egl_probe-license.txt b/docs/licenses/dependencies/egl_probe-license.txt new file mode 100644 index 000000000000..934eaa87bb98 --- /dev/null +++ b/docs/licenses/dependencies/egl_probe-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Stanford Vision 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. diff --git a/docs/licenses/dependencies/eigenpy-license.txt b/docs/licenses/dependencies/eigenpy-license.txt new file mode 100644 index 000000000000..c9a52bd35e1c --- /dev/null +++ b/docs/licenses/dependencies/eigenpy-license.txt @@ -0,0 +1,26 @@ +BSD 2-Clause License + +Copyright (c) 2014-2020, 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/executing-license.txt b/docs/licenses/dependencies/executing-license.txt new file mode 100644 index 000000000000..473e36e246ed --- /dev/null +++ b/docs/licenses/dependencies/executing-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019 Alex Hall + +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/filelock-license.txt b/docs/licenses/dependencies/filelock-license.txt new file mode 100644 index 000000000000..cf1ab25da034 --- /dev/null +++ b/docs/licenses/dependencies/filelock-license.txt @@ -0,0 +1,24 @@ +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +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 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. + +For more information, please refer to diff --git a/docs/licenses/dependencies/flaky-license.txt b/docs/licenses/dependencies/flaky-license.txt new file mode 100644 index 000000000000..167ec4d66df8 --- /dev/null +++ b/docs/licenses/dependencies/flaky-license.txt @@ -0,0 +1,166 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + +TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + +1. Definitions. + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + +2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + +3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + +4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + +6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + +7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + +8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + +9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + +END OF TERMS AND CONDITIONS diff --git a/docs/licenses/dependencies/flatdict-license.txt b/docs/licenses/dependencies/flatdict-license.txt new file mode 100644 index 000000000000..b0e19d425848 --- /dev/null +++ b/docs/licenses/dependencies/flatdict-license.txt @@ -0,0 +1,25 @@ +Copyright (c) 2013-2020 Gavin M. Roy +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + * 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. + * 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. diff --git a/docs/licenses/dependencies/fonttools-license.txt b/docs/licenses/dependencies/fonttools-license.txt new file mode 100644 index 000000000000..cc633905d333 --- /dev/null +++ b/docs/licenses/dependencies/fonttools-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2017 Just van Rossum + +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/fsspec-license.txt b/docs/licenses/dependencies/fsspec-license.txt new file mode 100644 index 000000000000..67590a5e5be5 --- /dev/null +++ b/docs/licenses/dependencies/fsspec-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2018, Martin Durant +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/docs/licenses/dependencies/gitdb-license.txt b/docs/licenses/dependencies/gitdb-license.txt new file mode 100644 index 000000000000..c4986edc27e0 --- /dev/null +++ b/docs/licenses/dependencies/gitdb-license.txt @@ -0,0 +1,42 @@ +Copyright (C) 2010, 2011 Sebastian Thiel and contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +* Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. + +* 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. + +* Neither the name of the GitDB project 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 +OWNER 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. + + +Additional Licenses +------------------- +The files at +gitdb/test/fixtures/packs/pack-11fdfa9e156ab73caae3b6da867192221f2089c2.idx +and +gitdb/test/fixtures/packs/pack-11fdfa9e156ab73caae3b6da867192221f2089c2.pack +are licensed under GNU GPL as part of the git source repository, +see http://en.wikipedia.org/wiki/Git_%28software%29 for more information. + +They are not required for the actual operation, which is why they are not found +in the distribution package. diff --git a/docs/licenses/dependencies/grpcio-license.txt b/docs/licenses/dependencies/grpcio-license.txt new file mode 100644 index 000000000000..b44484922db1 --- /dev/null +++ b/docs/licenses/dependencies/grpcio-license.txt @@ -0,0 +1,609 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +----------------------------------------------------------- + +BSD 3-Clause License + +Copyright 2016, Google Inc. + +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. + +----------------------------------------------------------- + +Mozilla Public License Version 2.0 +================================== + +1. Definitions +-------------- + +1.1. "Contributor" + means each individual or legal entity that creates, contributes to + the creation of, or owns Covered Software. + +1.2. "Contributor Version" + means the combination of the Contributions of others (if any) used + by a Contributor and that particular Contributor's Contribution. + +1.3. "Contribution" + means Covered Software of a particular Contributor. + +1.4. "Covered Software" + means Source Code Form to which the initial Contributor has attached + the notice in Exhibit A, the Executable Form of such Source Code + Form, and Modifications of such Source Code Form, in each case + including portions thereof. + +1.5. "Incompatible With Secondary Licenses" + means + + (a) that the initial Contributor has attached the notice described + in Exhibit B to the Covered Software; or + + (b) that the Covered Software was made available under the terms of + version 1.1 or earlier of the License, but not also under the + terms of a Secondary License. + +1.6. "Executable Form" + means any form of the work other than Source Code Form. + +1.7. "Larger Work" + means a work that combines Covered Software with other material, in + a separate file or files, that is not Covered Software. + +1.8. "License" + means this document. + +1.9. "Licensable" + means having the right to grant, to the maximum extent possible, + whether at the time of the initial grant or subsequently, any and + all of the rights conveyed by this License. + +1.10. "Modifications" + means any of the following: + + (a) any file in Source Code Form that results from an addition to, + deletion from, or modification of the contents of Covered + Software; or + + (b) any new file in Source Code Form that contains any Covered + Software. + +1.11. "Patent Claims" of a Contributor + means any patent claim(s), including without limitation, method, + process, and apparatus claims, in any patent Licensable by such + Contributor that would be infringed, but for the grant of the + License, by the making, using, selling, offering for sale, having + made, import, or transfer of either its Contributions or its + Contributor Version. + +1.12. "Secondary License" + means either the GNU General Public License, Version 2.0, the GNU + Lesser General Public License, Version 2.1, the GNU Affero General + Public License, Version 3.0, or any later versions of those + licenses. + +1.13. "Source Code Form" + means the form of the work preferred for making modifications. + +1.14. "You" (or "Your") + means an individual or a legal entity exercising rights under this + License. For legal entities, "You" includes any entity that + controls, is controlled by, or is under common control with You. For + purposes of this definition, "control" means (a) the power, direct + or indirect, to cause the direction or management of such entity, + whether by contract or otherwise, or (b) ownership of more than + fifty percent (50%) of the outstanding shares or beneficial + ownership of such entity. + +2. License Grants and Conditions +-------------------------------- + +2.1. Grants + +Each Contributor hereby grants You a world-wide, royalty-free, +non-exclusive license: + +(a) under intellectual property rights (other than patent or trademark) + Licensable by such Contributor to use, reproduce, make available, + modify, display, perform, distribute, and otherwise exploit its + Contributions, either on an unmodified basis, with Modifications, or + as part of a Larger Work; and + +(b) under Patent Claims of such Contributor to make, use, sell, offer + for sale, have made, import, and otherwise transfer either its + Contributions or its Contributor Version. + +2.2. Effective Date + +The licenses granted in Section 2.1 with respect to any Contribution +become effective for each Contribution on the date the Contributor first +distributes such Contribution. + +2.3. Limitations on Grant Scope + +The licenses granted in this Section 2 are the only rights granted under +this License. No additional rights or licenses will be implied from the +distribution or licensing of Covered Software under this License. +Notwithstanding Section 2.1(b) above, no patent license is granted by a +Contributor: + +(a) for any code that a Contributor has removed from Covered Software; + or + +(b) for infringements caused by: (i) Your and any other third party's + modifications of Covered Software, or (ii) the combination of its + Contributions with other software (except as part of its Contributor + Version); or + +(c) under Patent Claims infringed by Covered Software in the absence of + its Contributions. + +This License does not grant any rights in the trademarks, service marks, +or logos of any Contributor (except as may be necessary to comply with +the notice requirements in Section 3.4). + +2.4. Subsequent Licenses + +No Contributor makes additional grants as a result of Your choice to +distribute the Covered Software under a subsequent version of this +License (see Section 10.2) or under the terms of a Secondary License (if +permitted under the terms of Section 3.3). + +2.5. Representation + +Each Contributor represents that the Contributor believes its +Contributions are its original creation(s) or it has sufficient rights +to grant the rights to its Contributions conveyed by this License. + +2.6. Fair Use + +This License is not intended to limit any rights You have under +applicable copyright doctrines of fair use, fair dealing, or other +equivalents. + +2.7. Conditions + +Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted +in Section 2.1. + +3. Responsibilities +------------------- + +3.1. Distribution of Source Form + +All distribution of Covered Software in Source Code Form, including any +Modifications that You create or to which You contribute, must be under +the terms of this License. You must inform recipients that the Source +Code Form of the Covered Software is governed by the terms of this +License, and how they can obtain a copy of this License. You may not +attempt to alter or restrict the recipients' rights in the Source Code +Form. + +3.2. Distribution of Executable Form + +If You distribute Covered Software in Executable Form then: + +(a) such Covered Software must also be made available in Source Code + Form, as described in Section 3.1, and You must inform recipients of + the Executable Form how they can obtain a copy of such Source Code + Form by reasonable means in a timely manner, at a charge no more + than the cost of distribution to the recipient; and + +(b) You may distribute such Executable Form under the terms of this + License, or sublicense it under different terms, provided that the + license for the Executable Form does not attempt to limit or alter + the recipients' rights in the Source Code Form under this License. + +3.3. Distribution of a Larger Work + +You may create and distribute a Larger Work under terms of Your choice, +provided that You also comply with the requirements of this License for +the Covered Software. If the Larger Work is a combination of Covered +Software with a work governed by one or more Secondary Licenses, and the +Covered Software is not Incompatible With Secondary Licenses, this +License permits You to additionally distribute such Covered Software +under the terms of such Secondary License(s), so that the recipient of +the Larger Work may, at their option, further distribute the Covered +Software under the terms of either this License or such Secondary +License(s). + +3.4. Notices + +You may not remove or alter the substance of any license notices +(including copyright notices, patent notices, disclaimers of warranty, +or limitations of liability) contained within the Source Code Form of +the Covered Software, except that You may alter any license notices to +the extent required to remedy known factual inaccuracies. + +3.5. Application of Additional Terms + +You may choose to offer, and to charge a fee for, warranty, support, +indemnity or liability obligations to one or more recipients of Covered +Software. However, You may do so only on Your own behalf, and not on +behalf of any Contributor. You must make it absolutely clear that any +such warranty, support, indemnity, or liability obligation is offered by +You alone, and You hereby agree to indemnify every Contributor for any +liability incurred by such Contributor as a result of warranty, support, +indemnity or liability terms You offer. You may include additional +disclaimers of warranty and limitations of liability specific to any +jurisdiction. + +4. Inability to Comply Due to Statute or Regulation +--------------------------------------------------- + +If it is impossible for You to comply with any of the terms of this +License with respect to some or all of the Covered Software due to +statute, judicial order, or regulation then You must: (a) comply with +the terms of this License to the maximum extent possible; and (b) +describe the limitations and the code they affect. Such description must +be placed in a text file included with all distributions of the Covered +Software under this License. Except to the extent prohibited by statute +or regulation, such description must be sufficiently detailed for a +recipient of ordinary skill to be able to understand it. + +5. Termination +-------------- + +5.1. The rights granted under this License will terminate automatically +if You fail to comply with any of its terms. However, if You become +compliant, then the rights granted under this License from a particular +Contributor are reinstated (a) provisionally, unless and until such +Contributor explicitly and finally terminates Your grants, and (b) on an +ongoing basis, if such Contributor fails to notify You of the +non-compliance by some reasonable means prior to 60 days after You have +come back into compliance. Moreover, Your grants from a particular +Contributor are reinstated on an ongoing basis if such Contributor +notifies You of the non-compliance by some reasonable means, this is the +first time You have received notice of non-compliance with this License +from such Contributor, and You become compliant prior to 30 days after +Your receipt of the notice. + +5.2. If You initiate litigation against any entity by asserting a patent +infringement claim (excluding declaratory judgment actions, +counter-claims, and cross-claims) alleging that a Contributor Version +directly or indirectly infringes any patent, then the rights granted to +You by any and all Contributors for the Covered Software under Section +2.1 of this License shall terminate. + +5.3. In the event of termination under Sections 5.1 or 5.2 above, all +end user license agreements (excluding distributors and resellers) which +have been validly granted by You or Your distributors under this License +prior to termination shall survive termination. + +************************************************************************ +* * +* 6. Disclaimer of Warranty * +* ------------------------- * +* * +* Covered Software is provided under this License on an "as is" * +* basis, without warranty of any kind, either expressed, implied, or * +* statutory, including, without limitation, warranties that the * +* Covered Software is free of defects, merchantable, fit for a * +* particular purpose or non-infringing. The entire risk as to the * +* quality and performance of the Covered Software is with You. * +* Should any Covered Software prove defective in any respect, You * +* (not any Contributor) assume the cost of any necessary servicing, * +* repair, or correction. This disclaimer of warranty constitutes an * +* essential part of this License. No use of any Covered Software is * +* authorized under this License except under this disclaimer. * +* * +************************************************************************ + +************************************************************************ +* * +* 7. Limitation of Liability * +* -------------------------- * +* * +* Under no circumstances and under no legal theory, whether tort * +* (including negligence), contract, or otherwise, shall any * +* Contributor, or anyone who distributes Covered Software as * +* permitted above, be liable to You for any direct, indirect, * +* special, incidental, or consequential damages of any character * +* including, without limitation, damages for lost profits, loss of * +* goodwill, work stoppage, computer failure or malfunction, or any * +* and all other commercial damages or losses, even if such party * +* shall have been informed of the possibility of such damages. This * +* limitation of liability shall not apply to liability for death or * +* personal injury resulting from such party's negligence to the * +* extent applicable law prohibits such limitation. Some * +* jurisdictions do not allow the exclusion or limitation of * +* incidental or consequential damages, so this exclusion and * +* limitation may not apply to You. * +* * +************************************************************************ + +8. Litigation +------------- + +Any litigation relating to this License may be brought only in the +courts of a jurisdiction where the defendant maintains its principal +place of business and such litigation shall be governed by laws of that +jurisdiction, without reference to its conflict-of-law provisions. +Nothing in this Section shall prevent a party's ability to bring +cross-claims or counter-claims. + +9. Miscellaneous +---------------- + +This License represents the complete agreement concerning the subject +matter hereof. If any provision of this License is held to be +unenforceable, such provision shall be reformed only to the extent +necessary to make it enforceable. Any law or regulation which provides +that the language of a contract shall be construed against the drafter +shall not be used to construe this License against a Contributor. + +10. Versions of the License +--------------------------- + +10.1. New Versions + +Mozilla Foundation is the license steward. Except as provided in Section +10.3, no one other than the license steward has the right to modify or +publish new versions of this License. Each version will be given a +distinguishing version number. + +10.2. Effect of New Versions + +You may distribute the Covered Software under the terms of the version +of the License under which You originally received the Covered Software, +or under the terms of any subsequent version published by the license +steward. + +10.3. Modified Versions + +If you create software not governed by this License, and you want to +create a new license for such software, you may create and use a +modified version of this License if you rename the license and remove +any references to the name of the license steward (except to note that +such modified license differs from this License). + +10.4. Distributing Source Code Form that is Incompatible With Secondary +Licenses + +If You choose to distribute Source Code Form that is Incompatible With +Secondary Licenses under the terms of this version of the License, the +notice described in Exhibit B of this License must be attached. + +Exhibit A - Source Code Form License Notice +------------------------------------------- + + This Source Code Form is subject to the terms of the Mozilla Public + License, v. 2.0. If a copy of the MPL was not distributed with this + file, You can obtain one at http://mozilla.org/MPL/2.0/. + +If it is not possible or desirable to put the notice in a particular +file, then You may include the notice in a location (such as a LICENSE +file in a relevant directory) where a recipient would be likely to look +for such a notice. + +You may add additional accurate notices of copyright ownership. + +Exhibit B - "Incompatible With Secondary Licenses" Notice +--------------------------------------------------------- + + This Source Code Form is "Incompatible With Secondary Licenses", as + defined by the Mozilla Public License, v. 2.0. diff --git a/docs/licenses/dependencies/gym-notices-license.txt b/docs/licenses/dependencies/gym-notices-license.txt new file mode 100644 index 000000000000..96f1555dfe57 --- /dev/null +++ b/docs/licenses/dependencies/gym-notices-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2018 The Python Packaging Authority + +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/hf-xet-license.txt b/docs/licenses/dependencies/hf-xet-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/hf-xet-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/hpp-fcl-license.txt b/docs/licenses/dependencies/hpp-fcl-license.txt new file mode 100644 index 000000000000..c548a9f23e1c --- /dev/null +++ b/docs/licenses/dependencies/hpp-fcl-license.txt @@ -0,0 +1,34 @@ +Software License Agreement (BSD License) + + Copyright (c) 2008-2014, Willow Garage, Inc. + Copyright (c) 2014-2015, Open Source Robotics Foundation + 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: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * 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. + * Neither the name of Open Source Robotics Foundation 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 OWNER 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/huggingface-hub-license.txt b/docs/licenses/dependencies/huggingface-hub-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/huggingface-hub-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/idna-license.txt b/docs/licenses/dependencies/idna-license.txt new file mode 100644 index 000000000000..47e7567ab2e7 --- /dev/null +++ b/docs/licenses/dependencies/idna-license.txt @@ -0,0 +1,13 @@ +BSD 3-Clause License + +Copyright (c) 2013-2025, Kim Davies and contributors. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +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. + +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. diff --git a/docs/licenses/dependencies/imageio-license.txt b/docs/licenses/dependencies/imageio-license.txt new file mode 100644 index 000000000000..471943242b42 --- /dev/null +++ b/docs/licenses/dependencies/imageio-license.txt @@ -0,0 +1,23 @@ +Copyright (c) 2014-2022, imageio developers +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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/imageio_ffmpeg-license.txt b/docs/licenses/dependencies/imageio_ffmpeg-license.txt new file mode 100644 index 000000000000..6d27cf66867f --- /dev/null +++ b/docs/licenses/dependencies/imageio_ffmpeg-license.txt @@ -0,0 +1,25 @@ +BSD 2-Clause License + +Copyright (c) 2019-2025, imageio +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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/imagesize-license.txt b/docs/licenses/dependencies/imagesize-license.txt new file mode 100644 index 000000000000..b0df45f8a5ae --- /dev/null +++ b/docs/licenses/dependencies/imagesize-license.txt @@ -0,0 +1,8 @@ +The MIT License (MIT) +Copyright ยฉ 2016 Yoshiki Shibukawa + +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/iniconfig-license.txt b/docs/licenses/dependencies/iniconfig-license.txt new file mode 100644 index 000000000000..46f4b2846fd7 --- /dev/null +++ b/docs/licenses/dependencies/iniconfig-license.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2010 - 2023 Holger Krekel and others + +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/ipython-license.txt b/docs/licenses/dependencies/ipython-license.txt new file mode 100644 index 000000000000..d4bb8d39dfe4 --- /dev/null +++ b/docs/licenses/dependencies/ipython-license.txt @@ -0,0 +1,33 @@ +BSD 3-Clause License + +- Copyright (c) 2008-Present, IPython Development Team +- Copyright (c) 2001-2007, Fernando Perez +- Copyright (c) 2001, Janko Hauser +- Copyright (c) 2001, Nathaniel Gray + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/docs/licenses/dependencies/ipywidgets-license.txt b/docs/licenses/dependencies/ipywidgets-license.txt new file mode 100644 index 000000000000..deb2c38c8ecf --- /dev/null +++ b/docs/licenses/dependencies/ipywidgets-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2015 Project Jupyter Contributors +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. diff --git a/docs/licenses/dependencies/isaacsim-license.txt b/docs/licenses/dependencies/isaacsim-license.txt index 80cff4a45145..0454ece1bed6 100644 --- a/docs/licenses/dependencies/isaacsim-license.txt +++ b/docs/licenses/dependencies/isaacsim-license.txt @@ -1,13 +1,188 @@ -Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +The Isaac Sim software in this repository is covered under the Apache 2.0 +License terms below. -NVIDIA CORPORATION and its licensors retain all intellectual property -and proprietary rights in and to this software, related documentation -and any modifications thereto. Any use, reproduction, disclosure or -distribution of this software and related documentation without an express -license agreement from NVIDIA CORPORATION is strictly prohibited. +Building or using the software requires additional components licenced +under other terms. These additional components include dependencies such +as the Omniverse Kit SDK, as well as 3D models and textures. -Note: Licenses for assets such as Robots and Props used within these environments can be found inside their respective folders on the Nucleus server where they are hosted. +License terms for these additional NVIDIA owned and licensed components +can be found here: + https://docs.nvidia.com/NVIDIA-IsaacSim-Additional-Software-and-Materials-License.pdf -For more information: https://docs.omniverse.nvidia.com/app_isaacsim/common/NVIDIA_Omniverse_License_Agreement.html +Any open source dependencies downloaded during the build process are owned +by their respective owners and licensed under their respective terms. -For sub-dependencies of Isaac Sim: https://docs.omniverse.nvidia.com/app_isaacsim/common/licenses.html + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. diff --git a/docs/licenses/dependencies/jinja2-license.txt b/docs/licenses/dependencies/jinja2-license.txt new file mode 100644 index 000000000000..c37cae49ec77 --- /dev/null +++ b/docs/licenses/dependencies/jinja2-license.txt @@ -0,0 +1,28 @@ +Copyright 2007 Pallets + +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. diff --git a/docs/licenses/dependencies/jmespath-license.txt b/docs/licenses/dependencies/jmespath-license.txt new file mode 100644 index 000000000000..9c520c6bbff8 --- /dev/null +++ b/docs/licenses/dependencies/jmespath-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2013 Amazon.com, Inc. or its affiliates. All Rights Reserved + +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/joblib-license.txt b/docs/licenses/dependencies/joblib-license.txt new file mode 100644 index 000000000000..910537bd3341 --- /dev/null +++ b/docs/licenses/dependencies/joblib-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2008-2021, The joblib developers. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/docs/licenses/dependencies/jsonschema-license.txt b/docs/licenses/dependencies/jsonschema-license.txt new file mode 100644 index 000000000000..af9cfbdb134f --- /dev/null +++ b/docs/licenses/dependencies/jsonschema-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2013 Julian Berman + +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/jsonschema-specifications-license.txt b/docs/licenses/dependencies/jsonschema-specifications-license.txt new file mode 100644 index 000000000000..a9f853e43069 --- /dev/null +++ b/docs/licenses/dependencies/jsonschema-specifications-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2022 Julian Berman + +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/junitparser-license.txt b/docs/licenses/dependencies/junitparser-license.txt new file mode 100644 index 000000000000..a4325a4a80d4 --- /dev/null +++ b/docs/licenses/dependencies/junitparser-license.txt @@ -0,0 +1,13 @@ +Copyright 2020 Joel Wang + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/jupyterlab_widgets-license.txt b/docs/licenses/dependencies/jupyterlab_widgets-license.txt new file mode 100644 index 000000000000..deb2c38c8ecf --- /dev/null +++ b/docs/licenses/dependencies/jupyterlab_widgets-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2015 Project Jupyter Contributors +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. diff --git a/docs/licenses/dependencies/kiwisolver-license.txt b/docs/licenses/dependencies/kiwisolver-license.txt new file mode 100644 index 000000000000..206ae79273ed --- /dev/null +++ b/docs/licenses/dependencies/kiwisolver-license.txt @@ -0,0 +1,71 @@ +========================= + The Kiwi licensing terms +========================= +Kiwi is licensed under the terms of the Modified BSD License (also known as +New or Revised BSD), as follows: + +Copyright (c) 2013-2024, Nucleic Development Team + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +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. + +Neither the name of the Nucleic Development Team 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 OWNER 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. + +About Kiwi +---------- +Chris Colbert began the Kiwi project in December 2013 in an effort to +create a blisteringly fast UI constraint solver. Chris is still the +project lead. + +The Nucleic Development Team is the set of all contributors to the Nucleic +project and its subprojects. + +The core team that coordinates development on GitHub can be found here: +http://github.com/nucleic. The current team consists of: + +* Chris Colbert + +Our Copyright Policy +-------------------- +Nucleic uses a shared copyright model. Each contributor maintains copyright +over their contributions to Nucleic. But, it is important to note that these +contributions are typically only changes to the repositories. Thus, the Nucleic +source code, in its entirety is not the copyright of any single person or +institution. Instead, it is the collective copyright of the entire Nucleic +Development Team. If individual contributors want to maintain a record of what +changes/contributions they have specific copyright on, they should indicate +their copyright in the commit message of the change, when they commit the +change to one of the Nucleic repositories. + +With this in mind, the following banner should be used in any source code file +to indicate the copyright and license terms: + +#------------------------------------------------------------------------------ +# Copyright (c) 2013-2024, Nucleic Development Team. +# +# Distributed under the terms of the Modified BSD License. +# +# The full license is in the file LICENSE, distributed with this software. +#------------------------------------------------------------------------------ diff --git a/docs/licenses/dependencies/labeler-license.txt b/docs/licenses/dependencies/labeler-license.txt new file mode 100644 index 000000000000..cfbc8bb6dda5 --- /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/latexcodec-license.txt b/docs/licenses/dependencies/latexcodec-license.txt new file mode 100644 index 000000000000..e9511f21fb9a --- /dev/null +++ b/docs/licenses/dependencies/latexcodec-license.txt @@ -0,0 +1,7 @@ +latexcodec is a lexer and codec to work with LaTeX code in Python +Copyright (c) 2011-2020 by Matthias C. M. Troffaes +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/loop-rate-limiters-license.txt b/docs/licenses/dependencies/loop-rate-limiters-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/loop-rate-limiters-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/markdown-it-py-license.txt b/docs/licenses/dependencies/markdown-it-py-license.txt new file mode 100644 index 000000000000..582ddf59e082 --- /dev/null +++ b/docs/licenses/dependencies/markdown-it-py-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2020 ExecutableBookProject + +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/markdown-license.txt b/docs/licenses/dependencies/markdown-license.txt new file mode 100644 index 000000000000..ff993de4a45a --- /dev/null +++ b/docs/licenses/dependencies/markdown-license.txt @@ -0,0 +1,15 @@ +BSD 3-Clause License + +Copyright 2007, 2008 The Python Markdown Project (v. 1.7 and later) +Copyright 2004, 2005, 2006 Yuri Takhteyev (v. 0.2-1.6b) +Copyright 2004 Manfred Stienstra (the original version) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +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. + +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. diff --git a/docs/licenses/dependencies/markupsafe-license.txt b/docs/licenses/dependencies/markupsafe-license.txt new file mode 100644 index 000000000000..9d227a0cc43c --- /dev/null +++ b/docs/licenses/dependencies/markupsafe-license.txt @@ -0,0 +1,28 @@ +Copyright 2010 Pallets + +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. diff --git a/docs/licenses/dependencies/matplotlib-inline-license.txt b/docs/licenses/dependencies/matplotlib-inline-license.txt new file mode 100644 index 000000000000..b8350378e810 --- /dev/null +++ b/docs/licenses/dependencies/matplotlib-inline-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2019-2022, IPython Development Team. +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. diff --git a/docs/licenses/dependencies/mdit-py-plugins-license.txt b/docs/licenses/dependencies/mdit-py-plugins-license.txt new file mode 100644 index 000000000000..582ddf59e082 --- /dev/null +++ b/docs/licenses/dependencies/mdit-py-plugins-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2020 ExecutableBookProject + +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/mdurl-license.txt b/docs/licenses/dependencies/mdurl-license.txt new file mode 100644 index 000000000000..2a920c59d8ab --- /dev/null +++ b/docs/licenses/dependencies/mdurl-license.txt @@ -0,0 +1,46 @@ +Copyright (c) 2015 Vitaly Puzrin, Alex Kocharin. +Copyright (c) 2021 Taneli Hukkinen + +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. + +-------------------------------------------------------------------------------- + +.parse() is based on Joyent's node.js `url` code: + +Copyright Joyent, Inc. and other Node contributors. All rights reserved. +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/mpmath-license.txt b/docs/licenses/dependencies/mpmath-license.txt new file mode 100644 index 000000000000..6aa2fc91d7a1 --- /dev/null +++ b/docs/licenses/dependencies/mpmath-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2005-2025 Fredrik Johansson and mpmath contributors + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + a. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + b. 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. + c. 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 REGENTS 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/networkx-license.txt b/docs/licenses/dependencies/networkx-license.txt new file mode 100644 index 000000000000..0bf9a8f3f492 --- /dev/null +++ b/docs/licenses/dependencies/networkx-license.txt @@ -0,0 +1,37 @@ +NetworkX is distributed with the 3-clause BSD license. + +:: + + Copyright (c) 2004-2025, NetworkX Developers + Aric Hagberg + Dan Schult + Pieter Swart + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are + met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * 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. + + * Neither the name of the NetworkX Developers 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 + OWNER 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/newton-license.txt b/docs/licenses/dependencies/newton-license.txt new file mode 100644 index 000000000000..d9a10c0d8e86 --- /dev/null +++ b/docs/licenses/dependencies/newton-license.txt @@ -0,0 +1,176 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/docs/licenses/dependencies/nlopt-license.txt b/docs/licenses/dependencies/nlopt-license.txt new file mode 100644 index 000000000000..884683dfcf26 --- /dev/null +++ b/docs/licenses/dependencies/nlopt-license.txt @@ -0,0 +1,39 @@ +NLopt combines several free/open-source nonlinear optimization +libraries by various authors. See the COPYING, COPYRIGHT, and README +files in the subdirectories for the original copyright and licensing +information of these packages. + +The compiled NLopt library, i.e. the combined work of all of the +included optimization routines, is licensed under the conjunction of +all of these licensing terms. By default, the most restrictive terms +are for the code in the "luksan" directory, which is licensed under +the GNU Lesser General Public License (GNU LGPL), version 2.1 or +later (see luksan/COPYRIGHT). That means that, by default, the compiled +NLopt library is governed by the terms of the LGPL. + +--------------------------------------------------------------------------- + +However, NLopt also offers the option to be built without the code in +the "luksan" directory. In this case, NLopt, including any modifications +to the abovementioned packages, are licensed under the standard "MIT License:" + +Copyright (c) 2007-2024 Massachusetts Institute of Technology + +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/octomap-license.txt b/docs/licenses/dependencies/octomap-license.txt new file mode 100644 index 000000000000..a3c73b020480 --- /dev/null +++ b/docs/licenses/dependencies/octomap-license.txt @@ -0,0 +1,31 @@ + +OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + +License for the "octomap" library: New BSD License. + +Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * 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. + * Neither the name of the University of Freiburg 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 OWNER 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/omegaconf-license.txt b/docs/licenses/dependencies/omegaconf-license.txt new file mode 100644 index 000000000000..eb208dada03b --- /dev/null +++ b/docs/licenses/dependencies/omegaconf-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2018, Omry Yadan +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. diff --git a/docs/licenses/dependencies/opencv-python-license.txt b/docs/licenses/dependencies/opencv-python-license.txt new file mode 100644 index 000000000000..09ad12ac89f5 --- /dev/null +++ b/docs/licenses/dependencies/opencv-python-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) Olli-Pekka Heinisuo + +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/packaging-license.txt b/docs/licenses/dependencies/packaging-license.txt new file mode 100644 index 000000000000..d9a10c0d8e86 --- /dev/null +++ b/docs/licenses/dependencies/packaging-license.txt @@ -0,0 +1,176 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/docs/licenses/dependencies/pandas-license.txt b/docs/licenses/dependencies/pandas-license.txt new file mode 100644 index 000000000000..c343da2ebe87 --- /dev/null +++ b/docs/licenses/dependencies/pandas-license.txt @@ -0,0 +1,31 @@ +BSD 3-Clause License + +Copyright (c) 2008-2011, AQR Capital Management, LLC, Lambda Foundry, Inc. and PyData Development Team +All rights reserved. + +Copyright (c) 2011-2025, Open source contributors. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/docs/licenses/dependencies/pathtools-license.txt b/docs/licenses/dependencies/pathtools-license.txt new file mode 100644 index 000000000000..5e6adc9dadd8 --- /dev/null +++ b/docs/licenses/dependencies/pathtools-license.txt @@ -0,0 +1,21 @@ +Copyright (C) 2010 by Yesudeep Mangalapilly + +MIT License +----------- +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/pexpect-license.txt b/docs/licenses/dependencies/pexpect-license.txt new file mode 100644 index 000000000000..6ee60f42b44e --- /dev/null +++ b/docs/licenses/dependencies/pexpect-license.txt @@ -0,0 +1,19 @@ +ISC LICENSE + + This license is approved by the OSI and FSF as GPL-compatible. + http://opensource.org/licenses/isc-license.txt + + Copyright (c) 2013-2014, Pexpect development team + Copyright (c) 2012, Noah Spurrier + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. diff --git a/docs/licenses/dependencies/pin-license.txt b/docs/licenses/dependencies/pin-license.txt new file mode 100644 index 000000000000..dfacb6731484 --- /dev/null +++ b/docs/licenses/dependencies/pin-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/pin-pink-license.txt b/docs/licenses/dependencies/pin-pink-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/pin-pink-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/pinocchio-license.txt b/docs/licenses/dependencies/pinocchio-license.txt new file mode 100644 index 000000000000..dfacb6731484 --- /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/pluggy-license.txt b/docs/licenses/dependencies/pluggy-license.txt new file mode 100644 index 000000000000..85f4dd63d2da --- /dev/null +++ b/docs/licenses/dependencies/pluggy-license.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2015 holger krekel (rather uses bitbucket/hpk42) + +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/proglog-license.txt b/docs/licenses/dependencies/proglog-license.txt new file mode 100644 index 000000000000..a68d0a5fdd95 --- /dev/null +++ b/docs/licenses/dependencies/proglog-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2017 Edinburgh Genome Foundry, University of Edinburgh + +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/psutil-license.txt b/docs/licenses/dependencies/psutil-license.txt new file mode 100644 index 000000000000..cff5eb74e1ba --- /dev/null +++ b/docs/licenses/dependencies/psutil-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2009, Jay Loden, Dave Daeschler, Giampaolo Rodola +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * 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. + + * Neither the name of the psutil authors 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 OWNER 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/ptyprocess-license.txt b/docs/licenses/dependencies/ptyprocess-license.txt new file mode 100644 index 000000000000..6ee60f42b44e --- /dev/null +++ b/docs/licenses/dependencies/ptyprocess-license.txt @@ -0,0 +1,19 @@ +ISC LICENSE + + This license is approved by the OSI and FSF as GPL-compatible. + http://opensource.org/licenses/isc-license.txt + + Copyright (c) 2013-2014, Pexpect development team + Copyright (c) 2012, Noah Spurrier + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. diff --git a/docs/licenses/dependencies/pure_eval-license.txt b/docs/licenses/dependencies/pure_eval-license.txt new file mode 100644 index 000000000000..473e36e246ed --- /dev/null +++ b/docs/licenses/dependencies/pure_eval-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019 Alex Hall + +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/pybtex-docutils-license.txt b/docs/licenses/dependencies/pybtex-docutils-license.txt new file mode 100644 index 000000000000..810a5a8536c1 --- /dev/null +++ b/docs/licenses/dependencies/pybtex-docutils-license.txt @@ -0,0 +1,7 @@ +pybtex-docutils is a docutils backend for pybtex +Copyright (c) 2013-2023 by Matthias C. M. Troffaes +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/pydata-sphinx-theme-license.txt b/docs/licenses/dependencies/pydata-sphinx-theme-license.txt new file mode 100644 index 000000000000..464d11719d21 --- /dev/null +++ b/docs/licenses/dependencies/pydata-sphinx-theme-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2018, pandas +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/docs/licenses/dependencies/pygame-license.txt b/docs/licenses/dependencies/pygame-license.txt new file mode 100644 index 000000000000..56de5de6e152 --- /dev/null +++ b/docs/licenses/dependencies/pygame-license.txt @@ -0,0 +1,502 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! diff --git a/docs/licenses/dependencies/pyparsing-license.txt b/docs/licenses/dependencies/pyparsing-license.txt new file mode 100644 index 000000000000..1bf98523e331 --- /dev/null +++ b/docs/licenses/dependencies/pyparsing-license.txt @@ -0,0 +1,18 @@ +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/pyperclip-license.txt b/docs/licenses/dependencies/pyperclip-license.txt new file mode 100644 index 000000000000..799b74c5bf39 --- /dev/null +++ b/docs/licenses/dependencies/pyperclip-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2014, Al Sweigart +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* Neither the name of the {organization} 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. diff --git a/docs/licenses/dependencies/pytest-license.txt b/docs/licenses/dependencies/pytest-license.txt new file mode 100644 index 000000000000..c3f1657fce94 --- /dev/null +++ b/docs/licenses/dependencies/pytest-license.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2004 Holger Krekel and others + +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/pytest-mock-license.txt b/docs/licenses/dependencies/pytest-mock-license.txt new file mode 100644 index 000000000000..6e1ee5d5b8ae --- /dev/null +++ b/docs/licenses/dependencies/pytest-mock-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) [2016] [Bruno Oliveira] + +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/python-dateutil-license.txt b/docs/licenses/dependencies/python-dateutil-license.txt new file mode 100644 index 000000000000..6053d35cfc60 --- /dev/null +++ b/docs/licenses/dependencies/python-dateutil-license.txt @@ -0,0 +1,54 @@ +Copyright 2017- Paul Ganssle +Copyright 2017- dateutil contributors (see AUTHORS file) + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +The above license applies to all contributions after 2017-12-01, as well as +all contributions that have been re-licensed (see AUTHORS file for the list of +contributors who have re-licensed their code). +-------------------------------------------------------------------------------- +dateutil - Extensions to the standard Python datetime module. + +Copyright (c) 2003-2011 - Gustavo Niemeyer +Copyright (c) 2012-2014 - Tomi Pievilรคinen +Copyright (c) 2014-2016 - Yaron de Leeuw +Copyright (c) 2015- - Paul Ganssle +Copyright (c) 2015- - dateutil contributors (see AUTHORS file) + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * 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. + * 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 OWNER 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. + +The above BSD License Applies to all code, even that also covered by Apache 2.0. diff --git a/docs/licenses/dependencies/python-dotenv-license.txt b/docs/licenses/dependencies/python-dotenv-license.txt new file mode 100644 index 000000000000..3a97119010ac --- /dev/null +++ b/docs/licenses/dependencies/python-dotenv-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2014, Saurabh Kumar (python-dotenv), 2013, Ted Tieken (django-dotenv-rw), 2013, Jacob Kaplan-Moss (django-dotenv) + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +- Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + +- 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. + +- Neither the name of django-dotenv 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 OWNER 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/pytransform3d-license.txt b/docs/licenses/dependencies/pytransform3d-license.txt new file mode 100644 index 000000000000..061bfc4a75b8 --- /dev/null +++ b/docs/licenses/dependencies/pytransform3d-license.txt @@ -0,0 +1,30 @@ +BSD 3-Clause License + +Copyright 2014-2017 Alexander Fabisch (Robotics Research Group, University of Bremen), +2017-2025 Alexander Fabisch (DFKI GmbH, Robotics Innovation Center), +and pytransform3d contributors + +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. diff --git a/docs/licenses/dependencies/pytz-license.txt b/docs/licenses/dependencies/pytz-license.txt new file mode 100644 index 000000000000..5f1c11289f6a --- /dev/null +++ b/docs/licenses/dependencies/pytz-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2003-2019 Stuart Bishop + +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/pyyaml-license.txt b/docs/licenses/dependencies/pyyaml-license.txt new file mode 100644 index 000000000000..2f1b8e15e562 --- /dev/null +++ b/docs/licenses/dependencies/pyyaml-license.txt @@ -0,0 +1,20 @@ +Copyright (c) 2017-2021 Ingy dรถt Net +Copyright (c) 2006-2016 Kirill Simonov + +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/qhull-license.txt b/docs/licenses/dependencies/qhull-license.txt new file mode 100644 index 000000000000..9c802c06494d --- /dev/null +++ b/docs/licenses/dependencies/qhull-license.txt @@ -0,0 +1,39 @@ + Qhull, Copyright (c) 1993-2020 + + C.B. Barber + Arlington, MA + + and + + The National Science and Technology Research Center for + Computation and Visualization of Geometric Structures + (The Geometry Center) + University of Minnesota + + email: qhull@qhull.org + +This software includes Qhull from C.B. Barber and The Geometry Center. +Files derived from Qhull 1.0 are copyrighted by the Geometry Center. The +remaining files are copyrighted by C.B. Barber. Qhull is free software +and may be obtained via http from www.qhull.org. It may be freely copied, +modified, and redistributed under the following conditions: + +1. All copyright notices must remain intact in all files. + +2. A copy of this text file must be distributed along with any copies + of Qhull that you redistribute; this includes copies that you have + modified, or copies of programs or other software products that + include Qhull. + +3. If you modify Qhull, you must include a notice giving the + name of the person performing the modification, the date of + modification, and the reason for such modification. + +4. When distributing modified versions of Qhull, or other software + products that include Qhull, you must provide notice that the original + source code may be obtained as noted above. + +5. There is no warranty or other guarantee of fitness for Qhull, it is + provided solely "as is". Bug reports or fixes may be sent to + qhull_bug@qhull.org; the authors may or may not act on them as + they desire. diff --git a/docs/licenses/dependencies/qpsolvers-license.txt b/docs/licenses/dependencies/qpsolvers-license.txt new file mode 100644 index 000000000000..0a041280bd00 --- /dev/null +++ b/docs/licenses/dependencies/qpsolvers-license.txt @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/docs/licenses/dependencies/quadprog-license.txt b/docs/licenses/dependencies/quadprog-license.txt new file mode 100644 index 000000000000..23cb790338e1 --- /dev/null +++ b/docs/licenses/dependencies/quadprog-license.txt @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + {description} + Copyright (C) {year} {fullname} + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + {signature of Ty Coon}, 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/docs/licenses/dependencies/referencing-license.txt b/docs/licenses/dependencies/referencing-license.txt new file mode 100644 index 000000000000..a9f853e43069 --- /dev/null +++ b/docs/licenses/dependencies/referencing-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2022 Julian Berman + +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/regex-license.txt b/docs/licenses/dependencies/regex-license.txt new file mode 100644 index 000000000000..99c19cf84414 --- /dev/null +++ b/docs/licenses/dependencies/regex-license.txt @@ -0,0 +1,208 @@ +This work was derived from the 're' module of CPython 2.6 and CPython 3.1, +copyright (c) 1998-2001 by Secret Labs AB and licensed under CNRI's Python 1.6 +license. + +All additions and alterations are licensed under the Apache 2.0 License. + + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2020 Matthew Barnett + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/requests-license.txt b/docs/licenses/dependencies/requests-license.txt new file mode 100644 index 000000000000..dd5b3a58aa18 --- /dev/null +++ b/docs/licenses/dependencies/requests-license.txt @@ -0,0 +1,174 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. diff --git a/docs/licenses/dependencies/rich-license.txt b/docs/licenses/dependencies/rich-license.txt new file mode 100644 index 000000000000..4415505566f2 --- /dev/null +++ b/docs/licenses/dependencies/rich-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2020 Will McGugan + +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/ruff-license.txt b/docs/licenses/dependencies/ruff-license.txt new file mode 100644 index 000000000000..655a0c76fc55 --- /dev/null +++ b/docs/licenses/dependencies/ruff-license.txt @@ -0,0 +1,430 @@ +MIT License + +Copyright (c) 2022 Charles Marsh + +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. + +end of terms and conditions + +The externally maintained libraries from which parts of the Software is derived +are: + +- autoflake, licensed as follows: + """ + Copyright (C) 2012-2018 Steven Myint + + 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. + """ + +- autotyping, licensed as follows: + """ + MIT License + + Copyright (c) 2023 Jelle Zijlstra + + 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. + """ + +- Flake8, licensed as follows: + """ + == Flake8 License (MIT) == + + Copyright (C) 2011-2013 Tarek Ziade + Copyright (C) 2012-2016 Ian Cordasco + + 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. + """ + +- flake8-eradicate, licensed as follows: + """ + MIT License + + Copyright (c) 2018 Nikita Sobolev + + 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. + """ + +- flake8-pyi, licensed as follows: + """ + The MIT License (MIT) + + Copyright (c) 2016 ลukasz Langa + + 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. + """ + +- flake8-simplify, licensed as follows: + """ + MIT License + + Copyright (c) 2020 Martin Thoma + + 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. + """ + +- isort, licensed as follows: + """ + The MIT License (MIT) + + Copyright (c) 2013 Timothy Edmund Crosley + + 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. + """ + +- pygrep-hooks, licensed as follows: + """ + Copyright (c) 2018 Anthony Sottile + + 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. + """ + +- pycodestyle, licensed as follows: + """ + Copyright ยฉ 2006-2009 Johann C. Rocholl + Copyright ยฉ 2009-2014 Florent Xicluna + Copyright ยฉ 2014-2020 Ian Lee + + Licensed under the terms of the Expat License + + 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. + """ + +- pydocstyle, licensed as follows: + """ + Copyright (c) 2012 GreenSteam, + + Copyright (c) 2014-2020 Amir Rachum, + + Copyright (c) 2020 Sambhav Kothari, + + 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. + """ + +- Pyflakes, licensed as follows: + """ + Copyright 2005-2011 Divmod, Inc. + Copyright 2013-2014 Florent Xicluna + + 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. + """ + +- Pyright, licensed as follows: + """ + MIT License + + Pyright - A static type checker for the Python language + Copyright (c) Microsoft Corporation. All rights reserved. + + 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 + """ + +- pyupgrade, licensed as follows: + """ + Copyright (c) 2017 Anthony Sottile + + 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. + """ + +- rome/tools, licensed under the MIT license: + """ + MIT License + + Copyright (c) Rome Tools, Inc. and its affiliates. + + 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. + """ + +- RustPython, licensed as follows: + """ + MIT License + + Copyright (c) 2020 RustPython Team + + 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. + """ + +- rust-analyzer/text-size, licensed under the MIT license: + """ + 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/ruff-pre-commit-license.txt b/docs/licenses/dependencies/ruff-pre-commit-license.txt new file mode 100644 index 000000000000..c16d8bb1db26 --- /dev/null +++ b/docs/licenses/dependencies/ruff-pre-commit-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2024 Astral Software Inc. + +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/safetensors-license.txt b/docs/licenses/dependencies/safetensors-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/safetensors-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/scikit-learn-license.txt b/docs/licenses/dependencies/scikit-learn-license.txt new file mode 100644 index 000000000000..e1cd01d58457 --- /dev/null +++ b/docs/licenses/dependencies/scikit-learn-license.txt @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2007-2024 The scikit-learn developers. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/docs/licenses/dependencies/sentry-sdk-license.txt b/docs/licenses/dependencies/sentry-sdk-license.txt new file mode 100644 index 000000000000..016323bd8da8 --- /dev/null +++ b/docs/licenses/dependencies/sentry-sdk-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 Functional Software, Inc. dba Sentry + +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/setproctitle-license.txt b/docs/licenses/dependencies/setproctitle-license.txt new file mode 100644 index 000000000000..1f632fa116aa --- /dev/null +++ b/docs/licenses/dependencies/setproctitle-license.txt @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2009, Daniele Varrazzo + +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. diff --git a/docs/licenses/dependencies/shortuuid-license.txt b/docs/licenses/dependencies/shortuuid-license.txt new file mode 100644 index 000000000000..c14f01beb96a --- /dev/null +++ b/docs/licenses/dependencies/shortuuid-license.txt @@ -0,0 +1,29 @@ +Copyright (c) 2011, Stavros Korokithakis +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +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. + +Neither the name of Stochastic Technologies 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 +OWNER 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/six-license.txt b/docs/licenses/dependencies/six-license.txt new file mode 100644 index 000000000000..1cc22a5aa767 --- /dev/null +++ b/docs/licenses/dependencies/six-license.txt @@ -0,0 +1,18 @@ +Copyright (c) 2010-2024 Benjamin Peterson + +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/smmap-license.txt b/docs/licenses/dependencies/smmap-license.txt new file mode 100644 index 000000000000..8ef91f979049 --- /dev/null +++ b/docs/licenses/dependencies/smmap-license.txt @@ -0,0 +1,29 @@ +Copyright (C) 2010, 2011 Sebastian Thiel and contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +* Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. + +* 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. + +* Neither the name of the async project 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 +OWNER 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/sniffio-license.txt b/docs/licenses/dependencies/sniffio-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/sniffio-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/snowballstemmer-license.txt b/docs/licenses/dependencies/snowballstemmer-license.txt new file mode 100644 index 000000000000..f36607f9e93f --- /dev/null +++ b/docs/licenses/dependencies/snowballstemmer-license.txt @@ -0,0 +1,29 @@ +Copyright (c) 2001, Dr Martin Porter +Copyright (c) 2004,2005, Richard Boulton +Copyright (c) 2013, Yoshiki Shibukawa +Copyright (c) 2006,2007,2009,2010,2011,2014-2019, Olly Betts +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 Snowball project 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 OWNER 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/soupsieve-license.txt b/docs/licenses/dependencies/soupsieve-license.txt new file mode 100644 index 000000000000..a34ec87ec69a --- /dev/null +++ b/docs/licenses/dependencies/soupsieve-license.txt @@ -0,0 +1,9 @@ +MIT License + +Copyright (c) 2018 - 2025 Isaac Muse isaacmuse@gmail.com + +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/sphinxcontrib-applehelp-license.txt b/docs/licenses/dependencies/sphinxcontrib-applehelp-license.txt new file mode 100644 index 000000000000..8391fd1a2335 --- /dev/null +++ b/docs/licenses/dependencies/sphinxcontrib-applehelp-license.txt @@ -0,0 +1,8 @@ +License for sphinxcontrib-applehelp +Copyright (c) 2007-2019 by the Sphinx team (see https://github.com/sphinx-doc/sphinx/blob/master/AUTHORS). All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +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 OWNER 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/sphinxcontrib-devhelp-license.txt b/docs/licenses/dependencies/sphinxcontrib-devhelp-license.txt new file mode 100644 index 000000000000..e15238ffc3b5 --- /dev/null +++ b/docs/licenses/dependencies/sphinxcontrib-devhelp-license.txt @@ -0,0 +1,8 @@ +License for sphinxcontrib-devhelp +Copyright (c) 2007-2019 by the Sphinx team (see https://github.com/sphinx-doc/sphinx/blob/master/AUTHORS). All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +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 OWNER 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/sphinxcontrib-htmlhelp-license.txt b/docs/licenses/dependencies/sphinxcontrib-htmlhelp-license.txt new file mode 100644 index 000000000000..0339165c9d48 --- /dev/null +++ b/docs/licenses/dependencies/sphinxcontrib-htmlhelp-license.txt @@ -0,0 +1,8 @@ +License for sphinxcontrib-htmlhelp +Copyright (c) 2007-2019 by the Sphinx team (see https://github.com/sphinx-doc/sphinx/blob/master/AUTHORS). All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +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 OWNER 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/sphinxcontrib-jsmath-license.txt b/docs/licenses/dependencies/sphinxcontrib-jsmath-license.txt new file mode 100644 index 000000000000..e28495c913ce --- /dev/null +++ b/docs/licenses/dependencies/sphinxcontrib-jsmath-license.txt @@ -0,0 +1,8 @@ +License for sphinxcontrib-jsmath +Copyright (c) 2007-2019 by the Sphinx team (see https://github.com/sphinx-doc/sphinx/blob/master/AUTHORS). All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +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 OWNER 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/sphinxcontrib-qthelp-license.txt b/docs/licenses/dependencies/sphinxcontrib-qthelp-license.txt new file mode 100644 index 000000000000..dd8f49eb65b3 --- /dev/null +++ b/docs/licenses/dependencies/sphinxcontrib-qthelp-license.txt @@ -0,0 +1,8 @@ +License for sphinxcontrib-qthelp +Copyright (c) 2007-2019 by the Sphinx team (see https://github.com/sphinx-doc/sphinx/blob/master/AUTHORS). All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +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 OWNER 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/sphinxcontrib-serializinghtml-license.txt b/docs/licenses/dependencies/sphinxcontrib-serializinghtml-license.txt new file mode 100644 index 000000000000..b396b73ecd72 --- /dev/null +++ b/docs/licenses/dependencies/sphinxcontrib-serializinghtml-license.txt @@ -0,0 +1,8 @@ +License for sphinxcontrib-serializinghtml +Copyright (c) 2007-2019 by the Sphinx team (see https://github.com/sphinx-doc/sphinx/blob/master/AUTHORS). All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +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 OWNER 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/stack-data-license.txt b/docs/licenses/dependencies/stack-data-license.txt new file mode 100644 index 000000000000..473e36e246ed --- /dev/null +++ b/docs/licenses/dependencies/stack-data-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019 Alex Hall + +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/sympy-license.txt b/docs/licenses/dependencies/sympy-license.txt new file mode 100644 index 000000000000..bff6f8ff065e --- /dev/null +++ b/docs/licenses/dependencies/sympy-license.txt @@ -0,0 +1,153 @@ +Copyright (c) 2006-2023 SymPy Development Team + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + a. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + b. 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. + c. Neither the name of SymPy 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 REGENTS 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. + +-------------------------------------------------------------------------------- + +Patches that were taken from the Diofant project (https://github.com/diofant/diofant) +are licensed as: + +Copyright (c) 2006-2018 SymPy Development Team, + 2013-2023 Sergey B Kirpichev + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + a. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + b. 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. + c. Neither the name of Diofant or SymPy 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 REGENTS 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. + +-------------------------------------------------------------------------------- + +Submodules taken from the multipledispatch project (https://github.com/mrocklin/multipledispatch) +are licensed as: + +Copyright (c) 2014 Matthew Rocklin + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + a. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + b. 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. + c. Neither the name of multipledispatch 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 REGENTS 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. + +-------------------------------------------------------------------------------- + +The files under the directory sympy/parsing/autolev/tests/pydy-example-repo +are directly copied from PyDy project and are licensed as: + +Copyright (c) 2009-2023, PyDy Authors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +* 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. +* Neither the name of this project 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 PYDY AUTHORS 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. + +-------------------------------------------------------------------------------- + +The files under the directory sympy/parsing/latex +are directly copied from latex2sympy project and are licensed as: + +Copyright 2016, latex2sympy + +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/tensorboardx-license.txt b/docs/licenses/dependencies/tensorboardx-license.txt new file mode 100644 index 000000000000..bed02cde0be5 --- /dev/null +++ b/docs/licenses/dependencies/tensorboardx-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2025 Tzu-Wei Huang + +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/termcolor-license.txt b/docs/licenses/dependencies/termcolor-license.txt new file mode 100644 index 000000000000..d0b79705c354 --- /dev/null +++ b/docs/licenses/dependencies/termcolor-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2008-2011 Volvox Development Team + +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/threadpoolctl-license.txt b/docs/licenses/dependencies/threadpoolctl-license.txt new file mode 100644 index 000000000000..1d7302bf83b0 --- /dev/null +++ b/docs/licenses/dependencies/threadpoolctl-license.txt @@ -0,0 +1,24 @@ +Copyright (c) 2019, threadpoolctl contributors + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * 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. + * Neither the name of 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 OWNER 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/tinyxml-license.txt b/docs/licenses/dependencies/tinyxml-license.txt new file mode 100644 index 000000000000..85a6a36f0ddf --- /dev/null +++ b/docs/licenses/dependencies/tinyxml-license.txt @@ -0,0 +1,18 @@ +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. diff --git a/docs/licenses/dependencies/tokenizers-license.txt b/docs/licenses/dependencies/tokenizers-license.txt new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/docs/licenses/dependencies/tokenizers-license.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/docs/licenses/dependencies/tqdm-license.txt b/docs/licenses/dependencies/tqdm-license.txt new file mode 100644 index 000000000000..1899a730fb42 --- /dev/null +++ b/docs/licenses/dependencies/tqdm-license.txt @@ -0,0 +1,49 @@ +`tqdm` is a product of collaborative work. +Unless otherwise stated, all authors (see commit logs) retain copyright +for their respective work, and release the work under the MIT licence +(text below). + +Exceptions or notable authors are listed below +in reverse chronological order: + +* files: * + MPL-2.0 2015-2024 (c) Casper da Costa-Luis + [casperdcl](https://github.com/casperdcl). +* files: tqdm/_tqdm.py + MIT 2016 (c) [PR #96] on behalf of Google Inc. +* files: tqdm/_tqdm.py README.rst .gitignore + MIT 2013 (c) Noam Yorav-Raphael, original author. + +[PR #96]: #96 + + +Mozilla Public Licence (MPL) v. 2.0 - Exhibit A +----------------------------------------------- + +This Source Code Form is subject to the terms of the +Mozilla Public License, v. 2.0. +If a copy of the MPL was not distributed with this project, +You can obtain one at https://mozilla.org/MPL/2.0/. + + +MIT License (MIT) +----------------- + +Copyright (c) 2013 noamraph + +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/traitlets-license.txt b/docs/licenses/dependencies/traitlets-license.txt new file mode 100644 index 000000000000..76910b096a63 --- /dev/null +++ b/docs/licenses/dependencies/traitlets-license.txt @@ -0,0 +1,30 @@ +BSD 3-Clause License + +- Copyright (c) 2001-, IPython Development Team + +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. diff --git a/docs/licenses/dependencies/triton-license.txt b/docs/licenses/dependencies/triton-license.txt new file mode 100644 index 000000000000..1d0238e86b1b --- /dev/null +++ b/docs/licenses/dependencies/triton-license.txt @@ -0,0 +1,23 @@ +/* +* Copyright 2018-2020 Philippe Tillet +* Copyright 2020-2022 OpenAI +* +* 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/typing-extensions-license.txt b/docs/licenses/dependencies/typing-extensions-license.txt new file mode 100644 index 000000000000..f26bcf4d2de6 --- /dev/null +++ b/docs/licenses/dependencies/typing-extensions-license.txt @@ -0,0 +1,279 @@ +A. HISTORY OF THE SOFTWARE +========================== + +Python was created in the early 1990s by Guido van Rossum at Stichting +Mathematisch Centrum (CWI, see https://www.cwi.nl) in the Netherlands +as a successor of a language called ABC. Guido remains Python's +principal author, although it includes many contributions from others. + +In 1995, Guido continued his work on Python at the Corporation for +National Research Initiatives (CNRI, see https://www.cnri.reston.va.us) +in Reston, Virginia where he released several versions of the +software. + +In May 2000, Guido and the Python core development team moved to +BeOpen.com to form the BeOpen PythonLabs team. In October of the same +year, the PythonLabs team moved to Digital Creations, which became +Zope Corporation. In 2001, the Python Software Foundation (PSF, see +https://www.python.org/psf/) was formed, a non-profit organization +created specifically to own Python-related Intellectual Property. +Zope Corporation was a sponsoring member of the PSF. + +All Python releases are Open Source (see https://opensource.org for +the Open Source Definition). Historically, most, but not all, Python +releases have also been GPL-compatible; the table below summarizes +the various releases. + + Release Derived Year Owner GPL- + from compatible? (1) + + 0.9.0 thru 1.2 1991-1995 CWI yes + 1.3 thru 1.5.2 1.2 1995-1999 CNRI yes + 1.6 1.5.2 2000 CNRI no + 2.0 1.6 2000 BeOpen.com no + 1.6.1 1.6 2001 CNRI yes (2) + 2.1 2.0+1.6.1 2001 PSF no + 2.0.1 2.0+1.6.1 2001 PSF yes + 2.1.1 2.1+2.0.1 2001 PSF yes + 2.1.2 2.1.1 2002 PSF yes + 2.1.3 2.1.2 2002 PSF yes + 2.2 and above 2.1.1 2001-now PSF yes + +Footnotes: + +(1) GPL-compatible doesn't mean that we're distributing Python under + the GPL. All Python licenses, unlike the GPL, let you distribute + a modified version without making your changes open source. The + GPL-compatible licenses make it possible to combine Python with + other software that is released under the GPL; the others don't. + +(2) According to Richard Stallman, 1.6.1 is not GPL-compatible, + because its license has a choice of law clause. According to + CNRI, however, Stallman's lawyer has told CNRI's lawyer that 1.6.1 + is "not incompatible" with the GPL. + +Thanks to the many outside volunteers who have worked under Guido's +direction to make these releases possible. + + +B. TERMS AND CONDITIONS FOR ACCESSING OR OTHERWISE USING PYTHON +=============================================================== + +Python software and documentation are licensed under the +Python Software Foundation License Version 2. + +Starting with Python 3.8.6, examples, recipes, and other code in +the documentation are dual licensed under the PSF License Version 2 +and the Zero-Clause BSD license. + +Some software incorporated into Python is under different licenses. +The licenses are listed with code falling under that license. + + +PYTHON SOFTWARE FOUNDATION LICENSE VERSION 2 +-------------------------------------------- + +1. This LICENSE AGREEMENT is between the Python Software Foundation +("PSF"), and the Individual or Organization ("Licensee") accessing and +otherwise using this software ("Python") in source or binary form and +its associated documentation. + +2. Subject to the terms and conditions of this License Agreement, PSF hereby +grants Licensee a nonexclusive, royalty-free, world-wide license to reproduce, +analyze, test, perform and/or display publicly, prepare derivative works, +distribute, and otherwise use Python alone or in any derivative version, +provided, however, that PSF's License Agreement and PSF's notice of copyright, +i.e., "Copyright (c) 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008, 2009, 2010, +2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, 2020, 2021, 2022, 2023 Python Software Foundation; +All Rights Reserved" are retained in Python alone or in any derivative version +prepared by Licensee. + +3. In the event Licensee prepares a derivative work that is based on +or incorporates Python or any part thereof, and wants to make +the derivative work available to others as provided herein, then +Licensee hereby agrees to include in any such work a brief summary of +the changes made to Python. + +4. PSF is making Python available to Licensee on an "AS IS" +basis. PSF MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR +IMPLIED. BY WAY OF EXAMPLE, BUT NOT LIMITATION, PSF MAKES NO AND +DISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS +FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF PYTHON WILL NOT +INFRINGE ANY THIRD PARTY RIGHTS. + +5. PSF SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF PYTHON +FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS +A RESULT OF MODIFYING, DISTRIBUTING, OR OTHERWISE USING PYTHON, +OR ANY DERIVATIVE THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF. + +6. This License Agreement will automatically terminate upon a material +breach of its terms and conditions. + +7. Nothing in this License Agreement shall be deemed to create any +relationship of agency, partnership, or joint venture between PSF and +Licensee. This License Agreement does not grant permission to use PSF +trademarks or trade name in a trademark sense to endorse or promote +products or services of Licensee, or any third party. + +8. By copying, installing or otherwise using Python, Licensee +agrees to be bound by the terms and conditions of this License +Agreement. + + +BEOPEN.COM LICENSE AGREEMENT FOR PYTHON 2.0 +------------------------------------------- + +BEOPEN PYTHON OPEN SOURCE LICENSE AGREEMENT VERSION 1 + +1. This LICENSE AGREEMENT is between BeOpen.com ("BeOpen"), having an +office at 160 Saratoga Avenue, Santa Clara, CA 95051, and the +Individual or Organization ("Licensee") accessing and otherwise using +this software in source or binary form and its associated +documentation ("the Software"). + +2. Subject to the terms and conditions of this BeOpen Python License +Agreement, BeOpen hereby grants Licensee a non-exclusive, +royalty-free, world-wide license to reproduce, analyze, test, perform +and/or display publicly, prepare derivative works, distribute, and +otherwise use the Software alone or in any derivative version, +provided, however, that the BeOpen Python License is retained in the +Software, alone or in any derivative version prepared by Licensee. + +3. BeOpen is making the Software available to Licensee on an "AS IS" +basis. BEOPEN MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR +IMPLIED. BY WAY OF EXAMPLE, BUT NOT LIMITATION, BEOPEN MAKES NO AND +DISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS +FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF THE SOFTWARE WILL NOT +INFRINGE ANY THIRD PARTY RIGHTS. + +4. BEOPEN SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF THE +SOFTWARE FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS +AS A RESULT OF USING, MODIFYING OR DISTRIBUTING THE SOFTWARE, OR ANY +DERIVATIVE THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF. + +5. This License Agreement will automatically terminate upon a material +breach of its terms and conditions. + +6. This License Agreement shall be governed by and interpreted in all +respects by the law of the State of California, excluding conflict of +law provisions. Nothing in this License Agreement shall be deemed to +create any relationship of agency, partnership, or joint venture +between BeOpen and Licensee. This License Agreement does not grant +permission to use BeOpen trademarks or trade names in a trademark +sense to endorse or promote products or services of Licensee, or any +third party. As an exception, the "BeOpen Python" logos available at +http://www.pythonlabs.com/logos.html may be used according to the +permissions granted on that web page. + +7. By copying, installing or otherwise using the software, Licensee +agrees to be bound by the terms and conditions of this License +Agreement. + + +CNRI LICENSE AGREEMENT FOR PYTHON 1.6.1 +--------------------------------------- + +1. This LICENSE AGREEMENT is between the Corporation for National +Research Initiatives, having an office at 1895 Preston White Drive, +Reston, VA 20191 ("CNRI"), and the Individual or Organization +("Licensee") accessing and otherwise using Python 1.6.1 software in +source or binary form and its associated documentation. + +2. Subject to the terms and conditions of this License Agreement, CNRI +hereby grants Licensee a nonexclusive, royalty-free, world-wide +license to reproduce, analyze, test, perform and/or display publicly, +prepare derivative works, distribute, and otherwise use Python 1.6.1 +alone or in any derivative version, provided, however, that CNRI's +License Agreement and CNRI's notice of copyright, i.e., "Copyright (c) +1995-2001 Corporation for National Research Initiatives; All Rights +Reserved" are retained in Python 1.6.1 alone or in any derivative +version prepared by Licensee. Alternately, in lieu of CNRI's License +Agreement, Licensee may substitute the following text (omitting the +quotes): "Python 1.6.1 is made available subject to the terms and +conditions in CNRI's License Agreement. This Agreement together with +Python 1.6.1 may be located on the internet using the following +unique, persistent identifier (known as a handle): 1895.22/1013. This +Agreement may also be obtained from a proxy server on the internet +using the following URL: http://hdl.handle.net/1895.22/1013". + +3. In the event Licensee prepares a derivative work that is based on +or incorporates Python 1.6.1 or any part thereof, and wants to make +the derivative work available to others as provided herein, then +Licensee hereby agrees to include in any such work a brief summary of +the changes made to Python 1.6.1. + +4. CNRI is making Python 1.6.1 available to Licensee on an "AS IS" +basis. CNRI MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR +IMPLIED. BY WAY OF EXAMPLE, BUT NOT LIMITATION, CNRI MAKES NO AND +DISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS +FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF PYTHON 1.6.1 WILL NOT +INFRINGE ANY THIRD PARTY RIGHTS. + +5. CNRI SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF PYTHON +1.6.1 FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS +A RESULT OF MODIFYING, DISTRIBUTING, OR OTHERWISE USING PYTHON 1.6.1, +OR ANY DERIVATIVE THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF. + +6. This License Agreement will automatically terminate upon a material +breach of its terms and conditions. + +7. This License Agreement shall be governed by the federal +intellectual property law of the United States, including without +limitation the federal copyright law, and, to the extent such +U.S. federal law does not apply, by the law of the Commonwealth of +Virginia, excluding Virginia's conflict of law provisions. +Notwithstanding the foregoing, with regard to derivative works based +on Python 1.6.1 that incorporate non-separable material that was +previously distributed under the GNU General Public License (GPL), the +law of the Commonwealth of Virginia shall govern this License +Agreement only as to issues arising under or with respect to +Paragraphs 4, 5, and 7 of this License Agreement. Nothing in this +License Agreement shall be deemed to create any relationship of +agency, partnership, or joint venture between CNRI and Licensee. This +License Agreement does not grant permission to use CNRI trademarks or +trade name in a trademark sense to endorse or promote products or +services of Licensee, or any third party. + +8. By clicking on the "ACCEPT" button where indicated, or by copying, +installing or otherwise using Python 1.6.1, Licensee agrees to be +bound by the terms and conditions of this License Agreement. + + ACCEPT + + +CWI LICENSE AGREEMENT FOR PYTHON 0.9.0 THROUGH 1.2 +-------------------------------------------------- + +Copyright (c) 1991 - 1995, Stichting Mathematisch Centrum Amsterdam, +The Netherlands. All rights reserved. + +Permission to use, copy, modify, and distribute this software and its +documentation for any purpose and without fee is hereby granted, +provided that the above copyright notice appear in all copies and that +both that copyright notice and this permission notice appear in +supporting documentation, and that the name of Stichting Mathematisch +Centrum or CWI not be used in advertising or publicity pertaining to +distribution of the software without specific, written prior +permission. + +STICHTING MATHEMATISCH CENTRUM DISCLAIMS ALL WARRANTIES WITH REGARD TO +THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS, IN NO EVENT SHALL STICHTING MATHEMATISCH CENTRUM BE LIABLE +FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT +OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +ZERO-CLAUSE BSD LICENSE FOR CODE IN THE PYTHON DOCUMENTATION +---------------------------------------------------------------------- + +Permission to use, copy, modify, and/or distribute this software for any +purpose with or without fee is hereby granted. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH +REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY +AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, +INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM +LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR +OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR +PERFORMANCE OF THIS SOFTWARE. diff --git a/docs/licenses/dependencies/typing-inspection-license.txt b/docs/licenses/dependencies/typing-inspection-license.txt new file mode 100644 index 000000000000..e825ad51621c --- /dev/null +++ b/docs/licenses/dependencies/typing-inspection-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) Pydantic Services Inc. 2025 to present + +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/tzdata-license.txt b/docs/licenses/dependencies/tzdata-license.txt new file mode 100644 index 000000000000..c2f84aeb06f7 --- /dev/null +++ b/docs/licenses/dependencies/tzdata-license.txt @@ -0,0 +1,15 @@ +Apache Software License 2.0 + +Copyright (c) 2020, Paul Ganssle (Google) + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/docs/licenses/dependencies/urdfdom-license.txt b/docs/licenses/dependencies/urdfdom-license.txt new file mode 100644 index 000000000000..4c328b81145a --- /dev/null +++ b/docs/licenses/dependencies/urdfdom-license.txt @@ -0,0 +1,33 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * 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. +* * Neither the name of the Willow Garage 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 OWNER 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/urllib3-license.txt b/docs/licenses/dependencies/urllib3-license.txt new file mode 100644 index 000000000000..e6183d0276b2 --- /dev/null +++ b/docs/licenses/dependencies/urllib3-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2008-2020 Andrey Petrov 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/uv-license.txt b/docs/licenses/dependencies/uv-license.txt new file mode 100644 index 000000000000..014835144877 --- /dev/null +++ b/docs/licenses/dependencies/uv-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2025 Astral Software Inc. + +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/warp-license.txt b/docs/licenses/dependencies/warp-license.txt index 9fd8e1dc408d..d9a10c0d8e86 100644 --- a/docs/licenses/dependencies/warp-license.txt +++ b/docs/licenses/dependencies/warp-license.txt @@ -1,36 +1,176 @@ -# NVIDIA Source Code License for Warp + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -## 1. Definitions + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -โ€œLicensorโ€ means any person or entity that distributes its Work. -โ€œSoftwareโ€ means the original work of authorship made available under this License. -โ€œWorkโ€ means the Software and any additions to or derivative works of the Software that are made available under this License. -The terms โ€œreproduce,โ€ โ€œreproduction,โ€ โ€œderivative works,โ€ and โ€œdistributionโ€ have the meaning as provided under U.S. copyright law; provided, however, that for the purposes of this License, derivative works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work. -Works, including the Software, are โ€œmade availableโ€ under this License by including in or with the Work either (a) a copyright notice referencing the applicability of this License to the Work, or (b) a copy of this License. + 1. Definitions. -## 2. License Grant + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. -2.1 Copyright Grant. Subject to the terms and conditions of this License, each Licensor grants to you a perpetual, worldwide, non-exclusive, royalty-free, copyright license to reproduce, prepare derivative works of, publicly display, publicly perform, sublicense and distribute its Work and any resulting derivative works in any form. + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. -## 3. Limitations + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. -3.1 Redistribution. You may reproduce or distribute the Work only if (a) you do so under this License, (b) you include a complete copy of this License with your distribution, and (c) you retain without modification any copyright, patent, trademark, or attribution notices that are present in the Work. + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. -3.2 Derivative Works. You may specify that additional or different terms apply to the use, reproduction, and distribution of your derivative works of the Work (โ€œYour Termsโ€) only if (a) Your Terms provide that the use limitation in Section 3.3 applies to your derivative works, and (b) you identify the specific derivative works that are subject to Your Terms. Notwithstanding Your Terms, this License (including the redistribution requirements in Section 3.1) will continue to apply to the Work itself. + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. -3.3 Use Limitation. The Work and any derivative works thereof only may be used or intended for use non-commercially. Notwithstanding the foregoing, NVIDIA and its affiliates may use the Work and any derivative works commercially. As used herein, โ€œnon-commerciallyโ€ means for research or evaluation purposes only. + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. -3.4 Patent Claims. If you bring or threaten to bring a patent claim against any Licensor (including any claim, cross-claim or counterclaim in a lawsuit) to enforce any patents that you allege are infringed by any Work, then your rights under this License from such Licensor (including the grant in Section 2.1) will terminate immediately. + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). -3.5 Trademarks. This License does not grant any rights to use any Licensorโ€™s or its affiliatesโ€™ names, logos, or trademarks, except as necessary to reproduce the notices described in this License. + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. -3.6 Termination. If you violate any term of this License, then your rights under this License (including the grant in Section 2.1) will terminate immediately. + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." -## 4. Disclaimer of Warranty. + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. -THE WORK IS PROVIDED โ€œAS ISโ€ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WARRANTIES OR CONDITIONS OF -MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE OR NON-INFRINGEMENT. YOU BEAR THE RISK OF UNDERTAKING ANY ACTIVITIES UNDER THIS LICENSE. + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. -## 5. Limitation of Liability. + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. -EXCEPT AS PROHIBITED BY APPLICABLE LAW, IN NO EVENT AND UNDER NO LEGAL THEORY, WHETHER IN TORT (INCLUDING NEGLIGENCE), CONTRACT, OR OTHERWISE SHALL ANY LICENSOR BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR RELATED TO THIS LICENSE, THE USE OR INABILITY TO USE THE WORK (INCLUDING BUT NOT LIMITED TO LOSS OF GOODWILL, BUSINESS INTERRUPTION, LOST PROFITS OR DATA, COMPUTER FAILURE OR MALFUNCTION, OR ANY OTHER COMM ERCIAL DAMAGES OR LOSSES), EVEN IF THE LICENSOR HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/docs/licenses/dependencies/werkzeug-license.txt b/docs/licenses/dependencies/werkzeug-license.txt new file mode 100644 index 000000000000..c37cae49ec77 --- /dev/null +++ b/docs/licenses/dependencies/werkzeug-license.txt @@ -0,0 +1,28 @@ +Copyright 2007 Pallets + +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. diff --git a/docs/licenses/dependencies/widgetsnbextension-license.txt b/docs/licenses/dependencies/widgetsnbextension-license.txt new file mode 100644 index 000000000000..2ec51d75f362 --- /dev/null +++ b/docs/licenses/dependencies/widgetsnbextension-license.txt @@ -0,0 +1,27 @@ +BSD-3-Clause license +Copyright (c) 2015-2022, conda-forge contributors +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 REGENTS 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/zipp-license.txt b/docs/licenses/dependencies/zipp-license.txt new file mode 100644 index 000000000000..b5b7b529ffc2 --- /dev/null +++ b/docs/licenses/dependencies/zipp-license.txt @@ -0,0 +1 @@ +license = "MIT" as indicated in pyproject.toml diff --git a/docs/licenses/dependencies/zlib-license.txt b/docs/licenses/dependencies/zlib-license.txt new file mode 100644 index 000000000000..038d398538e9 --- /dev/null +++ b/docs/licenses/dependencies/zlib-license.txt @@ -0,0 +1,25 @@ +/* zlib.h -- interface of the 'zlib' general purpose compression library + version 1.3.1, January 22nd, 2024 + + Copyright (C) 1995-2024 Jean-loup Gailly and Mark Adler + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + + Jean-loup Gailly Mark Adler + jloup@gzip.org madler@alumni.caltech.edu + +*/ diff --git a/docs/make.bat b/docs/make.bat index cdaf22f257cb..676a3abc67d6 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/actuator-group/dc_motor_clipping.jpg b/docs/source/_static/actuator-group/dc_motor_clipping.jpg new file mode 100644 index 000000000000..83a865f538e8 Binary files /dev/null and b/docs/source/_static/actuator-group/dc_motor_clipping.jpg differ diff --git a/docs/source/_static/demos/bin_packing.jpg b/docs/source/_static/demos/bin_packing.jpg new file mode 100644 index 000000000000..52d242d6cc4e Binary files /dev/null and b/docs/source/_static/demos/bin_packing.jpg differ diff --git a/docs/source/_static/demos/bipeds.jpg b/docs/source/_static/demos/bipeds.jpg new file mode 100644 index 000000000000..d486dcb05fbb Binary files /dev/null and b/docs/source/_static/demos/bipeds.jpg differ diff --git a/docs/source/_static/demos/haply_teleop_franka.jpg b/docs/source/_static/demos/haply_teleop_franka.jpg new file mode 100644 index 000000000000..14461095c719 Binary files /dev/null and b/docs/source/_static/demos/haply_teleop_franka.jpg differ diff --git a/docs/source/_static/demos/multi-mesh-raycast.jpg b/docs/source/_static/demos/multi-mesh-raycast.jpg new file mode 100644 index 000000000000..b9d20d7150a1 Binary files /dev/null and b/docs/source/_static/demos/multi-mesh-raycast.jpg differ diff --git a/docs/source/_static/demos/pick_and_place.jpg b/docs/source/_static/demos/pick_and_place.jpg new file mode 100644 index 000000000000..76e2faa10cb7 Binary files /dev/null and b/docs/source/_static/demos/pick_and_place.jpg differ diff --git a/docs/source/_static/demos/quadcopter.jpg b/docs/source/_static/demos/quadcopter.jpg new file mode 100644 index 000000000000..541b8de7c9e1 Binary files /dev/null and b/docs/source/_static/demos/quadcopter.jpg differ diff --git a/docs/source/_static/how-to/howto_rendering_example_balanced.jpg b/docs/source/_static/how-to/howto_rendering_example_balanced.jpg new file mode 100644 index 000000000000..5ec36af1c0e6 Binary files /dev/null and b/docs/source/_static/how-to/howto_rendering_example_balanced.jpg differ diff --git a/docs/source/_static/how-to/howto_rendering_example_performance.jpg b/docs/source/_static/how-to/howto_rendering_example_performance.jpg new file mode 100644 index 000000000000..f9f5b826942b Binary files /dev/null and b/docs/source/_static/how-to/howto_rendering_example_performance.jpg differ diff --git a/docs/source/_static/how-to/howto_rendering_example_quality.jpg b/docs/source/_static/how-to/howto_rendering_example_quality.jpg new file mode 100644 index 000000000000..95d5c2845a06 Binary files /dev/null and b/docs/source/_static/how-to/howto_rendering_example_quality.jpg differ diff --git a/docs/source/_static/migration/ovd_pvd_comparison.jpg b/docs/source/_static/migration/ovd_pvd_comparison.jpg new file mode 100644 index 000000000000..f2d8fb31462f Binary files /dev/null and b/docs/source/_static/migration/ovd_pvd_comparison.jpg differ diff --git a/docs/source/_static/overview/sensors/contact_visualization.jpg b/docs/source/_static/overview/sensors/contact_visualization.jpg new file mode 100644 index 000000000000..79d61261ff6f Binary files /dev/null and b/docs/source/_static/overview/sensors/contact_visualization.jpg differ diff --git a/docs/source/_static/overview/sensors/tacsl_demo.jpg b/docs/source/_static/overview/sensors/tacsl_demo.jpg new file mode 100644 index 000000000000..a723d5ea8cb6 Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_demo.jpg differ diff --git a/docs/source/_static/overview/sensors/tacsl_diagram.jpg b/docs/source/_static/overview/sensors/tacsl_diagram.jpg new file mode 100644 index 000000000000..323c9a77e8d8 Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_diagram.jpg differ diff --git a/docs/source/_static/overview/sensors/tacsl_force_field_example.jpg b/docs/source/_static/overview/sensors/tacsl_force_field_example.jpg new file mode 100644 index 000000000000..eeca0295a2f0 Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_force_field_example.jpg differ diff --git a/docs/source/_static/overview/sensors/tacsl_taxim_example.jpg b/docs/source/_static/overview/sensors/tacsl_taxim_example.jpg new file mode 100644 index 000000000000..37e8dab491b6 Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_taxim_example.jpg differ diff --git a/docs/source/_static/policy_deployment/00_hover/hover_stable_wave.png b/docs/source/_static/policy_deployment/00_hover/hover_stable_wave.png new file mode 100644 index 000000000000..9604312be8b8 Binary files /dev/null and b/docs/source/_static/policy_deployment/00_hover/hover_stable_wave.png differ diff --git a/docs/source/_static/policy_deployment/00_hover/hover_training_robots.png b/docs/source/_static/policy_deployment/00_hover/hover_training_robots.png new file mode 100644 index 000000000000..6d8b254fc2ed Binary files /dev/null and b/docs/source/_static/policy_deployment/00_hover/hover_training_robots.png differ diff --git a/docs/source/_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_anymal_d_v0_IO_descriptors.yaml b/docs/source/_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_anymal_d_v0_IO_descriptors.yaml new file mode 100644 index 000000000000..f44840c0f908 --- /dev/null +++ b/docs/source/_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_anymal_d_v0_IO_descriptors.yaml @@ -0,0 +1,349 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +actions: +- action_type: JointAction + clip: null + dtype: torch.float32 + extras: + description: Joint action term that applies the processed actions to the articulation's + joints as position commands. + full_path: isaaclab.envs.mdp.actions.joint_actions.JointPositionAction + 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 + mdp_type: Action + name: joint_position_action + offset: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.4000000059604645 + - -0.4000000059604645 + - 0.4000000059604645 + - -0.4000000059604645 + - -0.800000011920929 + - 0.800000011920929 + - -0.800000011920929 + - 0.800000011920929 + scale: 0.5 + shape: + - 12 +articulations: + robot: + default_joint_armature: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + default_joint_damping: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + default_joint_friction: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + default_joint_pos: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.4000000059604645 + - -0.4000000059604645 + - 0.4000000059604645 + - -0.4000000059604645 + - -0.800000011920929 + - 0.800000011920929 + - -0.800000011920929 + - 0.800000011920929 + default_joint_pos_limits: + - - -0.7853984236717224 + - 0.6108654141426086 + - - -0.7853984236717224 + - 0.6108654141426086 + - - -0.6108654141426086 + - 0.7853984236717224 + - - -0.6108654141426086 + - 0.7853984236717224 + - - -9.42477798461914 + - 9.42477798461914 + - - -9.42477798461914 + - 9.42477798461914 + - - -9.42477798461914 + - 9.42477798461914 + - - -9.42477798461914 + - 9.42477798461914 + - - -9.42477798461914 + - 9.42477798461914 + - - -9.42477798461914 + - 9.42477798461914 + - - -9.42477798461914 + - 9.42477798461914 + - - -9.42477798461914 + - 9.42477798461914 + default_joint_stiffness: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + default_joint_vel: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + 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 +observations: + policy: + - dtype: torch.float32 + extras: + axes: + - X + - Y + - Z + description: Root linear velocity in the asset's root frame. + modifiers: null + units: m/s + full_path: isaaclab.envs.mdp.observations.base_lin_vel + mdp_type: Observation + name: base_lin_vel + observation_type: RootState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + axes: + - X + - Y + - Z + description: Root angular velocity in the asset's root frame. + modifiers: null + units: rad/s + full_path: isaaclab.envs.mdp.observations.base_ang_vel + mdp_type: Observation + name: base_ang_vel + observation_type: RootState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + axes: + - X + - Y + - Z + description: Gravity projection on the asset's root frame. + modifiers: null + units: m/s^2 + full_path: isaaclab.envs.mdp.observations.projected_gravity + mdp_type: Observation + name: projected_gravity + observation_type: RootState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + description: The generated command from command term in the command manager + with the given name. + modifiers: null + full_path: isaaclab.envs.mdp.observations.generated_commands + mdp_type: Observation + name: generated_commands + observation_type: Command + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + description: '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.' + modifiers: null + units: rad + full_path: isaaclab.envs.mdp.observations.joint_pos_rel + 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 + joint_pos_offsets: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.4000000059604645 + - -0.4000000059604645 + - 0.4000000059604645 + - -0.4000000059604645 + - -0.800000011920929 + - 0.800000011920929 + - -0.800000011920929 + - 0.800000011920929 + mdp_type: Observation + name: joint_pos_rel + observation_type: JointState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 12 + - dtype: torch.float32 + extras: + description: '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.' + modifiers: null + units: rad/s + full_path: isaaclab.envs.mdp.observations.joint_vel_rel + 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 + joint_vel_offsets: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + mdp_type: Observation + name: joint_vel_rel + observation_type: JointState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 12 + - dtype: torch.float32 + extras: + description: 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. + modifiers: null + full_path: isaaclab.envs.mdp.observations.last_action + mdp_type: Observation + name: last_action + observation_type: Action + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 12 +scene: + decimation: 4 + dt: 0.02 + physics_dt: 0.005 diff --git a/docs/source/_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_g1_v0_IO_descriptors.yaml b/docs/source/_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_g1_v0_IO_descriptors.yaml new file mode 100644 index 000000000000..d932800eaf6e --- /dev/null +++ b/docs/source/_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_g1_v0_IO_descriptors.yaml @@ -0,0 +1,724 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +actions: +- action_type: JointAction + clip: null + dtype: torch.float32 + extras: + description: Joint action term that applies the processed actions to the articulation's + joints as position commands. + full_path: isaaclab.envs.mdp.actions.joint_actions.JointPositionAction + 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 + mdp_type: Action + name: joint_position_action + offset: + - -0.20000000298023224 + - -0.20000000298023224 + - 0.0 + - 0.0 + - 0.0 + - 0.3499999940395355 + - 0.3499999940395355 + - 0.0 + - 0.0 + - 0.1599999964237213 + - -0.1599999964237213 + - 0.41999998688697815 + - 0.41999998688697815 + - 0.0 + - 0.0 + - -0.23000000417232513 + - -0.23000000417232513 + - 0.8700000047683716 + - 0.8700000047683716 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - -1.0 + - 0.5199999809265137 + - -0.5199999809265137 + scale: 0.5 + shape: + - 37 +articulations: + robot: + default_joint_armature: + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.009999999776482582 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + - 0.0010000000474974513 + default_joint_damping: + - 5.0 + - 5.0 + - 5.0 + - 5.0 + - 5.0 + - 10.0 + - 10.0 + - 5.0 + - 5.0 + - 10.0 + - 10.0 + - 5.0 + - 5.0 + - 10.0 + - 10.0 + - 2.0 + - 2.0 + - 10.0 + - 10.0 + - 2.0 + - 2.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 10.0 + default_joint_friction: + - 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 + - 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 + default_joint_pos: + - -0.20000000298023224 + - -0.20000000298023224 + - 0.0 + - 0.0 + - 0.0 + - 0.3499999940395355 + - 0.3499999940395355 + - 0.0 + - 0.0 + - 0.1599999964237213 + - -0.1599999964237213 + - 0.41999998688697815 + - 0.41999998688697815 + - 0.0 + - 0.0 + - -0.23000000417232513 + - -0.23000000417232513 + - 0.8700000047683716 + - 0.8700000047683716 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - -1.0 + - 0.5199999809265137 + - -0.5199999809265137 + default_joint_pos_limits: + - - -2.3499996662139893 + - 3.049999952316284 + - - -2.3499996662139893 + - 3.049999952316284 + - - -2.618000030517578 + - 2.618000030517578 + - - -0.25999996066093445 + - 2.5299997329711914 + - - -2.5299997329711914 + - 0.25999996066093445 + - - -2.967099666595459 + - 2.7924997806549072 + - - -2.967099666595459 + - 2.7924997806549072 + - - -2.749999761581421 + - 2.749999761581421 + - - -2.749999761581421 + - 2.749999761581421 + - - -1.5881999731063843 + - 2.251499652862549 + - - -2.251499652862549 + - 1.5881999731063843 + - - -0.3348899781703949 + - 2.5448997020721436 + - - -0.3348899781703949 + - 2.5448997020721436 + - - -2.618000030517578 + - 2.618000030517578 + - - -2.618000030517578 + - 2.618000030517578 + - - -0.6799999475479126 + - 0.7299999594688416 + - - -0.6799999475479126 + - 0.7299999594688416 + - - -0.22679997980594635 + - 3.420799732208252 + - - -0.22679997980594635 + - 3.420799732208252 + - - -0.26179996132850647 + - 0.26179996132850647 + - - -0.26179996132850647 + - 0.26179996132850647 + - - -2.094299793243408 + - 2.094299793243408 + - - -2.094299793243408 + - 2.094299793243408 + - - -1.8399999141693115 + - 0.30000001192092896 + - - -1.8399999141693115 + - 0.30000001192092896 + - - -0.5235979557037354 + - 0.5235979557037354 + - - -0.30000001192092896 + - 1.8399999141693115 + - - -0.30000001192092896 + - 1.8399999141693115 + - - -0.5235979557037354 + - 0.5235979557037354 + - - -1.8399999141693115 + - 0.0 + - - -1.8399999141693115 + - 0.0 + - - -0.9999999403953552 + - 1.2000000476837158 + - - 0.0 + - 1.8399999141693115 + - - 0.0 + - 1.8399999141693115 + - - -1.2000000476837158 + - 0.9999999403953552 + - - 0.0 + - 1.8399999141693115 + - - -1.8399999141693115 + - 0.0 + default_joint_stiffness: + - 200.0 + - 200.0 + - 200.0 + - 150.0 + - 150.0 + - 40.0 + - 40.0 + - 150.0 + - 150.0 + - 40.0 + - 40.0 + - 200.0 + - 200.0 + - 40.0 + - 40.0 + - 20.0 + - 20.0 + - 40.0 + - 40.0 + - 20.0 + - 20.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + - 40.0 + default_joint_vel: + - 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 + - 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 + 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 +observations: + policy: + - dtype: torch.float32 + extras: + axes: + - X + - Y + - Z + description: Root linear velocity in the asset's root frame. + modifiers: null + units: m/s + full_path: isaaclab.envs.mdp.observations.base_lin_vel + mdp_type: Observation + name: base_lin_vel + observation_type: RootState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + axes: + - X + - Y + - Z + description: Root angular velocity in the asset's root frame. + modifiers: null + units: rad/s + full_path: isaaclab.envs.mdp.observations.base_ang_vel + mdp_type: Observation + name: base_ang_vel + observation_type: RootState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + axes: + - X + - Y + - Z + description: Gravity projection on the asset's root frame. + modifiers: null + units: m/s^2 + full_path: isaaclab.envs.mdp.observations.projected_gravity + mdp_type: Observation + name: projected_gravity + observation_type: RootState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + description: The generated command from command term in the command manager + with the given name. + modifiers: null + full_path: isaaclab.envs.mdp.observations.generated_commands + mdp_type: Observation + name: generated_commands + observation_type: Command + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 3 + - dtype: torch.float32 + extras: + description: '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.' + modifiers: null + units: rad + full_path: isaaclab.envs.mdp.observations.joint_pos_rel + 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 + joint_pos_offsets: + - -0.20000000298023224 + - -0.20000000298023224 + - 0.0 + - 0.0 + - 0.0 + - 0.3499999940395355 + - 0.3499999940395355 + - 0.0 + - 0.0 + - 0.1599999964237213 + - -0.1599999964237213 + - 0.41999998688697815 + - 0.41999998688697815 + - 0.0 + - 0.0 + - -0.23000000417232513 + - -0.23000000417232513 + - 0.8700000047683716 + - 0.8700000047683716 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - -1.0 + - 0.5199999809265137 + - -0.5199999809265137 + mdp_type: Observation + name: joint_pos_rel + observation_type: JointState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 37 + - dtype: torch.float32 + extras: + description: '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.' + modifiers: null + units: rad/s + full_path: isaaclab.envs.mdp.observations.joint_vel_rel + 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 + joint_vel_offsets: + - 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 + - 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 + mdp_type: Observation + name: joint_vel_rel + observation_type: JointState + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 37 + - dtype: torch.float32 + extras: + description: 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. + modifiers: null + full_path: isaaclab.envs.mdp.observations.last_action + mdp_type: Observation + name: last_action + observation_type: Action + overloads: + clip: null + flatten_history_dim: true + history_length: 0 + scale: null + shape: + - 37 +scene: + decimation: 4 + dt: 0.02 + physics_dt: 0.005 diff --git a/docs/source/_static/policy_deployment/02_gear_assembly/gear_assembly_sim_real.webm b/docs/source/_static/policy_deployment/02_gear_assembly/gear_assembly_sim_real.webm new file mode 100644 index 000000000000..9a232dc0ceb2 Binary files /dev/null and b/docs/source/_static/policy_deployment/02_gear_assembly/gear_assembly_sim_real.webm differ diff --git a/docs/source/_static/policy_deployment/02_gear_assembly/sim_real_gear_assembly_train.jpg b/docs/source/_static/policy_deployment/02_gear_assembly/sim_real_gear_assembly_train.jpg new file mode 100644 index 000000000000..6bc1fc111043 Binary files /dev/null and b/docs/source/_static/policy_deployment/02_gear_assembly/sim_real_gear_assembly_train.jpg differ diff --git a/docs/source/_static/refs.bib b/docs/source/_static/refs.bib index c87c90c07696..8f696b6b36e1 100644 --- a/docs/source/_static/refs.bib +++ b/docs/source/_static/refs.bib @@ -100,7 +100,7 @@ @ARTICLE{frankhauser2018probabilistic @article{makoviychuk2021isaac, title={Isaac gym: High performance gpu-based physics simulation for robot learning}, - author={Makoviychuk, Viktor and Wawrzyniak, Lukasz and Guo, Yunrong and Lu, Michelle and Storey, Kier and Macklin, Miles and Hoeller, David and Rudin, Nikita and Allshire, Arthur and Handa, Ankur and others}, + author={Makoviychuk, Viktor and Wawrzyniak, Lukasz and Guo, Yunrong and Lu, Michelle and Storey, Kier and Macklin, Miles and Hoeller, David and Rudin, Nikita and Allshire, Arthur and Handa, Ankur and State, Gavriel}, journal={arXiv preprint arXiv:2108.10470}, year={2021} } @@ -108,27 +108,35 @@ @article{makoviychuk2021isaac @article{handa2022dextreme, title={DeXtreme: Transfer of Agile In-hand Manipulation from Simulation to Reality}, - author={Handa, Ankur and Allshire, Arthur and Makoviychuk, Viktor and Petrenko, Aleksei and Singh, Ritvik and Liu, Jingzhou and Makoviichuk, Denys and Van Wyk, Karl and Zhurkevich, Alexander and Sundaralingam, Balakumar and others}, + author={Handa, Ankur and Allshire, Arthur and Makoviychuk, Viktor and Petrenko, Aleksei and Singh, Ritvik and Liu, Jingzhou and Makoviichuk, Denys and Van Wyk, Karl and Zhurkevich, Alexander and Sundaralingam, Balakumar and Narang, Yashraj and Lafleche, Jean-Francois and Fox, Dieter and State, Gavriel}, journal={arXiv preprint arXiv:2210.13702}, year={2022} } @article{narang2022factory, title={Factory: Fast contact for robotic assembly}, - author={Narang, Yashraj and Storey, Kier and Akinola, Iretiayo and Macklin, Miles and Reist, Philipp and Wawrzyniak, Lukasz and Guo, Yunrong and Moravanszky, Adam and State, Gavriel and Lu, Michelle and others}, + author={Narang, Yashraj and Storey, Kier and Akinola, Iretiayo and Macklin, Miles and Reist, Philipp and Wawrzyniak, Lukasz and Guo, Yunrong and Moravanszky, Adam and State, Gavriel and Lu, Michelle and Handa, Ankur and Fox, Dieter}, journal={arXiv preprint arXiv:2205.03532}, year={2022} } @inproceedings{allshire2022transferring, title={Transferring dexterous manipulation from gpu simulation to a remote real-world trifinger}, - author={Allshire, Arthur and MittaI, Mayank and Lodaya, Varun and Makoviychuk, Viktor and Makoviichuk, Denys and Widmaier, Felix and W{\"u}thrich, Manuel and Bauer, Stefan and Handa, Ankur and Garg, Animesh}, + author={Allshire, Arthur and Mittal, Mayank and Lodaya, Varun and Makoviychuk, Viktor and Makoviichuk, Denys and Widmaier, Felix and W{\"u}thrich, Manuel and Bauer, Stefan and Handa, Ankur and Garg, Animesh}, booktitle={2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, pages={11802--11809}, year={2022}, organization={IEEE} } +@article{mittal2025isaaclab, + title={Isaac Lab: A GPU-Accelerated Simulation Framework for Multi-Modal Robot Learning}, + author={Mayank Mittal and Pascal Roth and James Tigue and Antoine Richard and Octi Zhang and Peter Du and Antonio Serrano-Muรฑoz and Xinjie Yao and Renรฉ Zurbrรผgg and Nikita Rudin and Lukasz Wawrzyniak and Milad Rakhsha and Alain Denzler and Eric Heiden and Ales Borovicka and Ossama Ahmed and Iretiayo Akinola and Abrar Anwar and Mark T. Carlson and Ji Yuan Feng and Animesh Garg and Renato Gasoto and Lionel Gulich and Yijie Guo and M. Gussert and Alex Hansen and Mihir Kulkarni and Chenran Li and Wei Liu and Viktor Makoviychuk and Grzegorz Malczyk and Hammad Mazhar and Masoud Moghani and Adithyavairavan Murali and Michael Noseworthy and Alexander Poddubny and Nathan Ratliff and Welf Rehberg and Clemens Schwarke and Ritvik Singh and James Latham Smith and Bingjie Tang and Ruchik Thaker and Matthew Trepte and Karl Van Wyk and Fangzhou Yu and Alex Millane and Vikram Ramasamy and Remo Steiner and Sangeeta Subramanian and Clemens Volk and CY Chen and Neel Jawale and Ashwin Varghese Kuruttukulam and Michael A. Lin and Ajay Mandlekar and Karsten Patzwaldt and John Welsh and Huihua Zhao and Fatima Anes and Jean-Francois Lafleche and Nicolas Moรซnne-Loccoz and Soowan Park and Rob Stepinski and Dirk Van Gelder and Chris Amevor and Jan Carius and Jumyung Chang and Anka He Chen and Pablo de Heras Ciechomski and Gilles Daviet and Mohammad Mohajerani and Julia von Muralt and Viktor Reutskyy and Michael Sauter and Simon Schirm and Eric L. Shi and Pierre Terdiman and Kenny Vilella and Tobias Widmer and Gordon Yeoman and Tiffany Chen and Sergey Grizan and Cathy Li and Lotus Li and Connor Smith and Rafael Wiltz and Kostas Alexis and Yan Chang and David Chu and Linxi "Jim" Fan and Farbod Farshidian and Ankur Handa and Spencer Huang and Marco Hutter and Yashraj Narang and Soha Pouya and Shiwei Sheng and Yuke Zhu and Miles Macklin and Adam Moravanszky and Philipp Reist and Yunrong Guo and David Hoeller and Gavriel State}, + journal={arXiv preprint arXiv:2511.04831}, + year={2025}, + url={https://arxiv.org/abs/2511.04831} +} + @article{mittal2023orbit, author={Mittal, Mayank and Yu, Calvin and Yu, Qinxi and Liu, Jingzhou and Rudin, Nikita and Hoeller, David and Yuan, Jia Lin and Singh, Ritvik and Guo, Yunrong and Mazhar, Hammad and Mandlekar, Ajay and Babich, Buck and State, Gavriel and Hutter, Marco and Garg, Animesh}, journal={IEEE Robotics and Automation Letters}, @@ -154,3 +162,46 @@ @inproceedings{he2016deep pages={770--778}, year={2016} } + +@InProceedings{schwarke2023curiosity, + title = {Curiosity-Driven Learning of Joint Locomotion and Manipulation Tasks}, + author = {Schwarke, Clemens and Klemm, Victor and Boon, Matthijs van der and Bjelonic, Marko and Hutter, Marco}, + booktitle = {Proceedings of The 7th Conference on Robot Learning}, + pages = {2594--2610}, + year = {2023}, + volume = {229}, + series = {Proceedings of Machine Learning Research}, + publisher = {PMLR}, + url = {https://proceedings.mlr.press/v229/schwarke23a.html}, +} + +@InProceedings{mittal2024symmetry, + author={Mittal, Mayank and Rudin, Nikita and Klemm, Victor and Allshire, Arthur and Hutter, Marco}, + booktitle={2024 IEEE International Conference on Robotics and Automation (ICRA)}, + title={Symmetry Considerations for Learning Task Symmetric Robot Policies}, + year={2024}, + pages={7433-7439}, + doi={10.1109/ICRA57147.2024.10611493} +} + +@article{kulkarni2025aerialgym, + author={Kulkarni, Mihir and Rehberg, Welf and Alexis, Kostas}, + journal={IEEE Robotics and Automation Letters}, + title={Aerial Gym Simulator: A Framework for Highly Parallelized Simulation of Aerial Robots}, + year={2025}, + volume={10}, + number={4}, + pages={4093-4100}, + doi={10.1109/LRA.2025.3548507} +} + +@article{si2022taxim, + title={Taxim: An example-based simulation model for gelsight tactile sensors}, + author={Si, Zilin and Yuan, Wenzhen}, + journal={IEEE Robotics and Automation Letters}, + volume={7}, + number={2}, + pages={2361--2368}, + year={2022}, + publisher={IEEE} +} diff --git a/docs/source/_static/setup/cloudxr_ar_panel.jpg b/docs/source/_static/setup/cloudxr_ar_panel.jpg new file mode 100644 index 000000000000..e92df07f4e22 Binary files /dev/null and b/docs/source/_static/setup/cloudxr_ar_panel.jpg differ diff --git a/docs/source/_static/setup/cloudxr_avp_connect_ui.jpg b/docs/source/_static/setup/cloudxr_avp_connect_ui.jpg new file mode 100644 index 000000000000..facd34bf8565 Binary files /dev/null and b/docs/source/_static/setup/cloudxr_avp_connect_ui.jpg differ diff --git a/docs/source/_static/setup/cloudxr_avp_ik_error.jpg b/docs/source/_static/setup/cloudxr_avp_ik_error.jpg new file mode 100644 index 000000000000..3f0d430182e7 Binary files /dev/null and b/docs/source/_static/setup/cloudxr_avp_ik_error.jpg differ diff --git a/docs/source/_static/setup/cloudxr_avp_teleop_ui.jpg b/docs/source/_static/setup/cloudxr_avp_teleop_ui.jpg new file mode 100644 index 000000000000..a072ec42df46 Binary files /dev/null and b/docs/source/_static/setup/cloudxr_avp_teleop_ui.jpg differ diff --git a/docs/source/_static/setup/cloudxr_viewport.jpg b/docs/source/_static/setup/cloudxr_viewport.jpg new file mode 100644 index 000000000000..8c412940fbbc Binary files /dev/null and b/docs/source/_static/setup/cloudxr_viewport.jpg differ diff --git a/docs/source/_static/setup/teleop_avp_voice_control.jpg b/docs/source/_static/setup/teleop_avp_voice_control.jpg deleted file mode 100644 index e12a1b93c4ce..000000000000 Binary files a/docs/source/_static/setup/teleop_avp_voice_control.jpg and /dev/null differ diff --git a/docs/source/_static/setup/teleop_avp_voice_item_name.jpg b/docs/source/_static/setup/teleop_avp_voice_item_name.jpg deleted file mode 100644 index d2af55033527..000000000000 Binary files a/docs/source/_static/setup/teleop_avp_voice_item_name.jpg and /dev/null differ diff --git a/docs/source/_static/setup/walkthrough_1_1_result.jpg b/docs/source/_static/setup/walkthrough_1_1_result.jpg new file mode 100644 index 000000000000..0aa64f48d831 Binary files /dev/null and b/docs/source/_static/setup/walkthrough_1_1_result.jpg differ diff --git a/docs/source/_static/setup/walkthrough_1_2_arrows.jpg b/docs/source/_static/setup/walkthrough_1_2_arrows.jpg new file mode 100644 index 000000000000..81b98ac7b523 Binary files /dev/null and b/docs/source/_static/setup/walkthrough_1_2_arrows.jpg differ diff --git a/docs/source/_static/setup/walkthrough_project_setup.svg b/docs/source/_static/setup/walkthrough_project_setup.svg new file mode 100644 index 000000000000..f5accffbc680 --- /dev/null +++ b/docs/source/_static/setup/walkthrough_project_setup.svg @@ -0,0 +1 @@ +isaac_lab_tutorialscriptssourceLICENSEREADME.mdisaac_lab_tutorialconfigpyproject.tomlsetup.pydocsisaac_lab_tutorialextension.tomltasksdirectisaac_lab_tutorialagents__init__.pyisaac_lab_tutorial_env_cfg.pyisaac_lab_tutorial_env.pyProjectExtensionModulesTask diff --git a/docs/source/_static/setup/walkthrough_sim_stage_scene.svg b/docs/source/_static/setup/walkthrough_sim_stage_scene.svg new file mode 100644 index 000000000000..17cd53aaf371 --- /dev/null +++ b/docs/source/_static/setup/walkthrough_sim_stage_scene.svg @@ -0,0 +1 @@ +WorldStageSceneSimApp diff --git a/docs/source/_static/setup/walkthrough_stage_context.svg b/docs/source/_static/setup/walkthrough_stage_context.svg new file mode 100644 index 000000000000..b98ccb2cedef --- /dev/null +++ b/docs/source/_static/setup/walkthrough_stage_context.svg @@ -0,0 +1 @@ +OOโ€™/World/Table/Sphere/World/Table/PyramidWorldTablePyramidSphereVisualsVisualsVisuals diff --git a/docs/source/_static/setup/walkthrough_training_vectors.svg b/docs/source/_static/setup/walkthrough_training_vectors.svg new file mode 100644 index 000000000000..a285d2cb90f8 --- /dev/null +++ b/docs/source/_static/setup/walkthrough_training_vectors.svg @@ -0,0 +1 @@ +W๐‘ฅเทœ๐‘ฆเทœ๐‘ฆเทœโ€ฒ๐‘ฅเทœโ€ฒBcommandforwardsrobot.data.root_pos_wrobot.data.root_com_vel_w[:,:3]๐‘ฅเทœyawforwardscommand(forwardscommand)๐‘งฦธ diff --git a/docs/source/_static/tasks/automate/00004.jpg b/docs/source/_static/tasks/automate/00004.jpg new file mode 100644 index 000000000000..bf2b3bedd874 Binary files /dev/null and b/docs/source/_static/tasks/automate/00004.jpg differ diff --git a/docs/source/_static/tasks/automate/01053_disassembly.jpg b/docs/source/_static/tasks/automate/01053_disassembly.jpg new file mode 100644 index 000000000000..a4ff086578ca Binary files /dev/null and b/docs/source/_static/tasks/automate/01053_disassembly.jpg differ diff --git a/docs/source/_static/tasks/drone_arl/arl_robot_1_track_position_state_based.jpg b/docs/source/_static/tasks/drone_arl/arl_robot_1_track_position_state_based.jpg new file mode 100644 index 000000000000..61c8ecb2d5e5 Binary files /dev/null and b/docs/source/_static/tasks/drone_arl/arl_robot_1_track_position_state_based.jpg differ diff --git a/docs/source/_static/tasks/locomotion/agility_digit_flat.jpg b/docs/source/_static/tasks/locomotion/agility_digit_flat.jpg new file mode 100644 index 000000000000..f98c15e014ae Binary files /dev/null and b/docs/source/_static/tasks/locomotion/agility_digit_flat.jpg differ diff --git a/docs/source/_static/tasks/locomotion/agility_digit_loco_manip.jpg b/docs/source/_static/tasks/locomotion/agility_digit_loco_manip.jpg new file mode 100644 index 000000000000..5d6abe598657 Binary files /dev/null and b/docs/source/_static/tasks/locomotion/agility_digit_loco_manip.jpg differ diff --git a/docs/source/_static/tasks/locomotion/agility_digit_rough.jpg b/docs/source/_static/tasks/locomotion/agility_digit_rough.jpg new file mode 100644 index 000000000000..85b9d8fe24ba Binary files /dev/null and b/docs/source/_static/tasks/locomotion/agility_digit_rough.jpg differ 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 000000000000..73a421714c23 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 000000000000..15f41ba7fb42 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 000000000000..86d2180c0580 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 000000000000..ce9d73c8dcdf 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 000000000000..45d712c2c740 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/galbot_stack_cube.jpg b/docs/source/_static/tasks/manipulation/galbot_stack_cube.jpg new file mode 100644 index 000000000000..72b7321a5d6e Binary files /dev/null and b/docs/source/_static/tasks/manipulation/galbot_stack_cube.jpg differ diff --git a/docs/source/_static/tasks/manipulation/gr-1_pick_place.jpg b/docs/source/_static/tasks/manipulation/gr-1_pick_place.jpg new file mode 100644 index 000000000000..6cbcea112238 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/gr-1_pick_place.jpg differ diff --git a/docs/source/_static/tasks/manipulation/gr-1_pick_place_annotation.jpg b/docs/source/_static/tasks/manipulation/gr-1_pick_place_annotation.jpg new file mode 100644 index 000000000000..04dd386ca9ff Binary files /dev/null and b/docs/source/_static/tasks/manipulation/gr-1_pick_place_annotation.jpg differ diff --git a/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg b/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg new file mode 100644 index 000000000000..1f99cb727419 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.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 000000000000..9d19b0e42366 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 000000000000..384e7632e44b Binary files /dev/null and b/docs/source/_static/tasks/manipulation/kuka_allegro_reorient.jpg differ diff --git a/docs/source/_static/tasks/manipulation/openarm_bi_reach.jpg b/docs/source/_static/tasks/manipulation/openarm_bi_reach.jpg new file mode 100644 index 000000000000..12e6620027d6 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/openarm_bi_reach.jpg differ diff --git a/docs/source/_static/tasks/manipulation/openarm_uni_lift.jpg b/docs/source/_static/tasks/manipulation/openarm_uni_lift.jpg new file mode 100644 index 000000000000..b7bb7c3cecf9 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/openarm_uni_lift.jpg differ diff --git a/docs/source/_static/tasks/manipulation/openarm_uni_open_drawer.jpg b/docs/source/_static/tasks/manipulation/openarm_uni_open_drawer.jpg new file mode 100644 index 000000000000..0bec48a40194 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/openarm_uni_open_drawer.jpg differ diff --git a/docs/source/_static/tasks/manipulation/openarm_uni_reach.jpg b/docs/source/_static/tasks/manipulation/openarm_uni_reach.jpg new file mode 100644 index 000000000000..46128f7607be Binary files /dev/null and b/docs/source/_static/tasks/manipulation/openarm_uni_reach.jpg differ diff --git a/docs/source/_static/tasks/manipulation/ur10_stack_surface_gripper.jpg b/docs/source/_static/tasks/manipulation/ur10_stack_surface_gripper.jpg new file mode 100644 index 000000000000..c92b4a32f8c9 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/ur10_stack_surface_gripper.jpg differ diff --git a/docs/source/_static/tasks/manipulation/ur10e_reach.jpg b/docs/source/_static/tasks/manipulation/ur10e_reach.jpg new file mode 100644 index 000000000000..740b33fde668 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/ur10e_reach.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 000000000000..240b784673bd 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 000000000000..48d6c29e7d7d Binary files /dev/null and b/docs/source/_static/teleop/teleop_diagram.jpg differ diff --git a/docs/source/_static/tutorials/tutorial_add_new_robot_result.jpg b/docs/source/_static/tutorials/tutorial_add_new_robot_result.jpg new file mode 100644 index 000000000000..9713aa0eba0e Binary files /dev/null and b/docs/source/_static/tutorials/tutorial_add_new_robot_result.jpg differ diff --git a/docs/source/_static/tutorials/tutorial_run_surface_gripper.jpg b/docs/source/_static/tutorials/tutorial_run_surface_gripper.jpg new file mode 100644 index 000000000000..d86ab4ed4c4b Binary files /dev/null and b/docs/source/_static/tutorials/tutorial_run_surface_gripper.jpg differ diff --git a/docs/source/_static/visualizers/newton_viz.jpg b/docs/source/_static/visualizers/newton_viz.jpg new file mode 100644 index 000000000000..9a37ad09fc37 Binary files /dev/null and b/docs/source/_static/visualizers/newton_viz.jpg differ diff --git a/docs/source/_static/visualizers/ov_viz.jpg b/docs/source/_static/visualizers/ov_viz.jpg new file mode 100644 index 000000000000..e172ae42ee9f Binary files /dev/null and b/docs/source/_static/visualizers/ov_viz.jpg differ diff --git a/docs/source/_static/visualizers/rerun_viz.jpg b/docs/source/_static/visualizers/rerun_viz.jpg new file mode 100644 index 000000000000..2263cfbccfed Binary files /dev/null and b/docs/source/_static/visualizers/rerun_viz.jpg differ diff --git a/docs/source/api/index.rst b/docs/source/api/index.rst index aef91e80df50..92eec5ed6f3f 100644 --- a/docs/source/api/index.rst +++ b/docs/source/api/index.rst @@ -36,6 +36,8 @@ The following modules are available in the ``isaaclab`` extension: lab/isaaclab.sim.converters lab/isaaclab.sim.schemas lab/isaaclab.sim.spawners + lab/isaaclab.sim.views + lab/isaaclab.sim.utils isaaclab_rl extension @@ -64,6 +66,20 @@ The following modules are available in the ``isaaclab_mimic`` extension: datagen envs +isaaclab_contrib extension +----------------------------- + +The following modules are available in the ``isaaclab_contrib`` extension: + +.. currentmodule:: isaaclab_contrib + +.. autosummary:: + :toctree: lab_contrib + + actuators + assets + mdp + sensors isaaclab_tasks extension ------------------------ diff --git a/docs/source/api/lab/isaaclab.app.rst b/docs/source/api/lab/isaaclab.app.rst index 46eff80ab951..b170fa8fa8ff 100644 --- a/docs/source/api/lab/isaaclab.app.rst +++ b/docs/source/api/lab/isaaclab.app.rst @@ -23,10 +23,9 @@ The following details the behavior of the class based on the environment variabl * **Livestreaming**: If the environment variable ``LIVESTREAM={1,2}`` , then `livestream`_ is enabled. Any of the livestream modes being true forces the app to run in headless mode. - * ``LIVESTREAM=1`` [DEPRECATED] enables streaming via the Isaac `Native Livestream`_ extension. This allows users to - connect through the Omniverse Streaming Client. This method is deprecated from Isaac Sim 4.5. Please use the WebRTC - livestreaming instead. - * ``LIVESTREAM=2`` enables streaming via the `WebRTC Livestream`_ extension. This allows users to + * ``LIVESTREAM=1`` enables streaming via the `WebRTC Livestream`_ extension over **public networks**. This allows users to + connect through the WebRTC Client using the WebRTC protocol. + * ``LIVESTREAM=2`` enables streaming via the `WebRTC Livestream`_ extension over **private and local networks**. This allows users to connect through the WebRTC Client using the WebRTC protocol. .. note:: @@ -55,16 +54,16 @@ To set the environment variables, one can use the following command in the termi .. code:: bash - export REMOTE_DEPLOYMENT=3 + export LIVESTREAM=2 export ENABLE_CAMERAS=1 # run the python script - ./isaaclab.sh -p scripts/demo/play_quadrupeds.py + ./isaaclab.sh -p scripts/demos/quadrupeds.py Alternatively, one can set the environment variables to the python script directly: .. code:: bash - REMOTE_DEPLOYMENT=3 ENABLE_CAMERAS=1 ./isaaclab.sh -p scripts/demo/play_quadrupeds.py + LIVESTREAM=2 ENABLE_CAMERAS=1 ./isaaclab.sh -p scripts/demos/quadrupeds.py Overriding the environment variables @@ -113,5 +112,4 @@ Simulation App Launcher .. _livestream: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/manual_livestream_clients.html -.. _`Native Livestream`: https://docs.isaacsim.omniverse.nvidia.com/latest/installation/manual_livestream_clients.html#omniverse-streaming-client-deprecated .. _`WebRTC Livestream`: https://docs.isaacsim.omniverse.nvidia.com/latest/installation/manual_livestream_clients.html#isaac-sim-short-webrtc-streaming-client diff --git a/docs/source/api/lab/isaaclab.assets.rst b/docs/source/api/lab/isaaclab.assets.rst index 338d729ddb6f..c91066966e80 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.controllers.rst b/docs/source/api/lab/isaaclab.controllers.rst index 07adbfcc0cda..1ef31448ab86 100644 --- a/docs/source/api/lab/isaaclab.controllers.rst +++ b/docs/source/api/lab/isaaclab.controllers.rst @@ -11,6 +11,9 @@ DifferentialIKControllerCfg OperationalSpaceController OperationalSpaceControllerCfg + pink_ik.PinkIKController + pink_ik.PinkIKControllerCfg + pink_ik.NullSpacePostureTask Differential Inverse Kinematics ------------------------------- @@ -39,3 +42,25 @@ Operational Space controllers :inherited-members: :show-inheritance: :exclude-members: __init__, class_type + + +Pink IK Controller +------------------ + +.. automodule:: isaaclab.controllers.pink_ik + +.. autoclass:: PinkIKController + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: PinkIKControllerCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type + +Available Pink IK Tasks +^^^^^^^^^^^^^^^^^^^^^^^ + +.. autoclass:: NullSpacePostureTask diff --git a/docs/source/api/lab/isaaclab.devices.rst b/docs/source/api/lab/isaaclab.devices.rst index 7e02c76e5786..5f04c4733cf6 100644 --- a/docs/source/api/lab/isaaclab.devices.rst +++ b/docs/source/api/lab/isaaclab.devices.rst @@ -8,12 +8,26 @@ .. autosummary:: DeviceBase + RetargeterBase Se2Gamepad Se3Gamepad Se2Keyboard Se3Keyboard + Se2SpaceMouse Se3SpaceMouse - Se3SpaceMouse + HaplyDevice + OpenXRDevice + ManusVive + isaaclab.devices.openxr.retargeters.GripperRetargeter + isaaclab.devices.openxr.retargeters.Se3AbsRetargeter + isaaclab.devices.openxr.retargeters.Se3RelRetargeter + isaaclab.devices.openxr.retargeters.GR1T2Retargeter + + .. rubric:: Modules + + .. autosummary:: + + isaaclab.devices.openxr.retargeters Device Base ----------- @@ -21,6 +35,12 @@ Device Base .. autoclass:: DeviceBase :members: +Retargeter Base +--------------- + +.. autoclass:: RetargeterBase + :members: + Game Pad -------- @@ -28,11 +48,13 @@ Game Pad :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: Se3Gamepad :members: :inherited-members: :show-inheritance: + :noindex: Keyboard -------- @@ -41,11 +63,13 @@ Keyboard :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: Se3Keyboard :members: :inherited-members: :show-inheritance: + :noindex: Space Mouse ----------- @@ -54,8 +78,64 @@ Space Mouse :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: Se3SpaceMouse :members: :inherited-members: :show-inheritance: + :noindex: + +Haply +----- + +.. autoclass:: HaplyDevice + :members: + :inherited-members: + :show-inheritance: + :noindex: + +OpenXR +------ + +.. autoclass:: OpenXRDevice + :members: + :inherited-members: + :show-inheritance: + :noindex: + +Manus + Vive +------------ + +.. autoclass:: ManusVive + :members: + :inherited-members: + :show-inheritance: + :noindex: + +Retargeters +----------- + +.. autoclass:: isaaclab.devices.openxr.retargeters.GripperRetargeter + :members: + :inherited-members: + :show-inheritance: + :noindex: + +.. autoclass:: isaaclab.devices.openxr.retargeters.Se3AbsRetargeter + :members: + :inherited-members: + :show-inheritance: + :noindex: + +.. autoclass:: isaaclab.devices.openxr.retargeters.Se3RelRetargeter + :members: + :inherited-members: + :show-inheritance: + :noindex: + +.. autoclass:: isaaclab.devices.openxr.retargeters.GR1T2Retargeter + :members: + :inherited-members: + :show-inheritance: + :noindex: diff --git a/docs/source/api/lab/isaaclab.envs.rst b/docs/source/api/lab/isaaclab.envs.rst index e003fa4f175c..51b6e1866888 100644 --- a/docs/source/api/lab/isaaclab.envs.rst +++ b/docs/source/api/lab/isaaclab.envs.rst @@ -24,6 +24,8 @@ DirectMARLEnvCfg ManagerBasedRLMimicEnv MimicEnvCfg + SubTaskConfig + SubTaskConstraintConfig ViewerCfg Manager Based Environment @@ -92,6 +94,18 @@ Mimic Environment :show-inheritance: :exclude-members: __init__, class_type +.. autoclass:: SubTaskConfig + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type + +.. autoclass:: SubTaskConstraintConfig + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type + Common ------ diff --git a/docs/source/api/lab/isaaclab.sensors.rst b/docs/source/api/lab/isaaclab.sensors.rst index 17ce71e38278..52dfd9bba5d1 100644 --- a/docs/source/api/lab/isaaclab.sensors.rst +++ b/docs/source/api/lab/isaaclab.sensors.rst @@ -31,6 +31,11 @@ RayCasterCfg RayCasterCamera RayCasterCameraCfg + MultiMeshRayCaster + MultiMeshRayCasterData + MultiMeshRayCasterCfg + MultiMeshRayCasterCamera + MultiMeshRayCasterCameraCfg Imu ImuCfg @@ -61,7 +66,7 @@ USD Camera :members: :inherited-members: :show-inheritance: - :exclude-members: __init__, class_type + :exclude-members: __init__, class_type, OffsetCfg Tile-Rendered USD Camera ------------------------ @@ -151,7 +156,40 @@ Ray-Cast Camera :members: :inherited-members: :show-inheritance: - :exclude-members: __init__, class_type + :exclude-members: __init__, class_type, OffsetCfg + +Multi-Mesh Ray-Cast Sensor +-------------------------- + +.. autoclass:: MultiMeshRayCaster + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: MultiMeshRayCasterData + :members: + :inherited-members: + :exclude-members: __init__ + +.. autoclass:: MultiMeshRayCasterCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type, OffsetCfg + +Multi-Mesh Ray-Cast Camera +-------------------------- + +.. autoclass:: MultiMeshRayCasterCamera + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: MultiMeshRayCasterCameraCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type, OffsetCfg, RaycastTargetCfg Inertia Measurement Unit ------------------------ diff --git a/docs/source/api/lab/isaaclab.sim.converters.rst b/docs/source/api/lab/isaaclab.sim.converters.rst index da8dd49c427e..6fd5155c4e53 100644 --- a/docs/source/api/lab/isaaclab.sim.converters.rst +++ b/docs/source/api/lab/isaaclab.sim.converters.rst @@ -13,6 +13,8 @@ MeshConverterCfg UrdfConverter UrdfConverterCfg + MjcfConverter + MjcfConverterCfg Asset Converter Base -------------------- @@ -52,3 +54,17 @@ URDF Converter :inherited-members: :show-inheritance: :exclude-members: __init__ + +MJCF Converter +-------------- + +.. autoclass:: MjcfConverter + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: MjcfConverterCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__ diff --git a/docs/source/api/lab/isaaclab.sim.rst b/docs/source/api/lab/isaaclab.sim.rst index 86aff36c175a..b6b5c372bc17 100644 --- a/docs/source/api/lab/isaaclab.sim.rst +++ b/docs/source/api/lab/isaaclab.sim.rst @@ -56,10 +56,3 @@ Simulation Context Builder -------------------------- .. automethod:: simulation_context.build_simulation_context - -Utilities ---------- - -.. automodule:: isaaclab.sim.utils - :members: - :show-inheritance: diff --git a/docs/source/api/lab/isaaclab.sim.utils.rst b/docs/source/api/lab/isaaclab.sim.utils.rst new file mode 100644 index 000000000000..f27e574efb9a --- /dev/null +++ b/docs/source/api/lab/isaaclab.sim.utils.rst @@ -0,0 +1,57 @@ +๏ปฟisaaclab.sim.utils +================== + +.. automodule:: isaaclab.sim.utils + + .. rubric:: Submodules + + .. autosummary:: + + stage + queries + prims + transforms + semantics + legacy + +Stage +----- + +.. automodule:: isaaclab.sim.utils.stage + :members: + :show-inheritance: + +Queries +------- + +.. automodule:: isaaclab.sim.utils.queries + :members: + :show-inheritance: + +Prims +----- + +.. automodule:: isaaclab.sim.utils.prims + :members: + :show-inheritance: + +Transforms +---------- + +.. automodule:: isaaclab.sim.utils.transforms + :members: + :show-inheritance: + +Semantics +--------- + +.. automodule:: isaaclab.sim.utils.semantics + :members: + :show-inheritance: + +Legacy +------ + +.. automodule:: isaaclab.sim.utils.legacy + :members: + :show-inheritance: diff --git a/docs/source/api/lab/isaaclab.sim.views.rst b/docs/source/api/lab/isaaclab.sim.views.rst new file mode 100644 index 000000000000..3a5f9bdecfe9 --- /dev/null +++ b/docs/source/api/lab/isaaclab.sim.views.rst @@ -0,0 +1,17 @@ +๏ปฟisaaclab.sim.views +================== + +.. automodule:: isaaclab.sim.views + + .. rubric:: Classes + + .. autosummary:: + + XformPrimView + +XForm Prim View +--------------- + +.. autoclass:: XformPrimView + :members: + :show-inheritance: diff --git a/docs/source/api/lab/isaaclab.utils.rst b/docs/source/api/lab/isaaclab.utils.rst index 9ef89739f79b..5b352152e0b5 100644 --- a/docs/source/api/lab/isaaclab.utils.rst +++ b/docs/source/api/lab/isaaclab.utils.rst @@ -11,14 +11,20 @@ array assets buffers + datasets dict interpolation + logger math + mesh modifiers noise + seed + sensors string timer types + version warp .. Rubric:: Functions @@ -65,6 +71,14 @@ Buffer operations :inherited-members: :show-inheritance: +Datasets operations +~~~~~~~~~~~~~~~~~~~ + +.. automodule:: isaaclab.utils.datasets + :members: + :show-inheritance: + :exclude-members: __init__, func + Dictionary operations ~~~~~~~~~~~~~~~~~~~~~ @@ -81,6 +95,13 @@ Interpolation operations :inherited-members: :show-inheritance: +Logger operations +~~~~~~~~~~~~~~~~~ + +.. automodule:: isaaclab.utils.logger + :members: + :show-inheritance: + Math operations ~~~~~~~~~~~~~~~ @@ -89,6 +110,14 @@ Math operations :inherited-members: :show-inheritance: +Mesh operations +~~~~~~~~~~~~~~~ + +.. automodule:: isaaclab.utils.mesh + :members: + :imported-members: + :show-inheritance: + Modifier operations ~~~~~~~~~~~~~~~~~~~ @@ -110,6 +139,20 @@ Noise operations :show-inheritance: :exclude-members: __init__, func +Seed operations +~~~~~~~~~~~~~~~ + +.. automodule:: isaaclab.utils.seed + :members: + :show-inheritance: + +Sensor operations +~~~~~~~~~~~~~~~~~ + +.. automodule:: isaaclab.utils.sensors + :members: + :show-inheritance: + String operations ~~~~~~~~~~~~~~~~~ @@ -131,6 +174,13 @@ Type operations :members: :show-inheritance: +Version operations +~~~~~~~~~~~~~~~~~~ + +.. automodule:: isaaclab.utils.version + :members: + :show-inheritance: + Warp operations ~~~~~~~~~~~~~~~ diff --git a/docs/source/api/lab_contrib/isaaclab_contrib.actuators.rst b/docs/source/api/lab_contrib/isaaclab_contrib.actuators.rst new file mode 100644 index 000000000000..1171c31e5eaf --- /dev/null +++ b/docs/source/api/lab_contrib/isaaclab_contrib.actuators.rst @@ -0,0 +1,25 @@ +isaaclab_contrib.actuators +============================= + +.. automodule:: isaaclab_contrib.actuators + + .. rubric:: Classes + + .. autosummary:: + + Thruster + ThrusterCfg + +Thruster Actuator +----------------- + +.. autoclass:: Thruster + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: ThrusterCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type diff --git a/docs/source/api/lab_contrib/isaaclab_contrib.assets.rst b/docs/source/api/lab_contrib/isaaclab_contrib.assets.rst new file mode 100644 index 000000000000..24375f918c8f --- /dev/null +++ b/docs/source/api/lab_contrib/isaaclab_contrib.assets.rst @@ -0,0 +1,31 @@ +isaaclab_contrib.assets +========================== + +.. automodule:: isaaclab_contrib.assets + + .. rubric:: Classes + + .. autosummary:: + + Multirotor + MultirotorCfg + MultirotorData + +Multirotor Asset +---------------- + +.. autoclass:: Multirotor + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: MultirotorCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type + +.. autoclass:: MultirotorData + :members: + :inherited-members: + :show-inheritance: diff --git a/docs/source/api/lab_contrib/isaaclab_contrib.mdp.rst b/docs/source/api/lab_contrib/isaaclab_contrib.mdp.rst new file mode 100644 index 000000000000..32421ef9b1f8 --- /dev/null +++ b/docs/source/api/lab_contrib/isaaclab_contrib.mdp.rst @@ -0,0 +1,33 @@ +isaaclab_contrib.mdp +======================= + +.. automodule:: isaaclab_contrib.mdp + +Actions +------- + +.. automodule:: isaaclab_contrib.mdp.actions + + .. rubric:: Classes + + .. autosummary:: + + ThrustActionCfg + ThrustAction + +Thrust Action Configuration +--------------------------- + +.. autoclass:: isaaclab_contrib.mdp.actions.thrust_actions_cfg.ThrustActionCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type + +Thrust Action Implementation +---------------------------- + +.. autoclass:: isaaclab_contrib.mdp.actions.thrust_actions.ThrustAction + :members: + :inherited-members: + :show-inheritance: diff --git a/docs/source/api/lab_contrib/isaaclab_contrib.sensors.rst b/docs/source/api/lab_contrib/isaaclab_contrib.sensors.rst new file mode 100644 index 000000000000..efcdeffe239b --- /dev/null +++ b/docs/source/api/lab_contrib/isaaclab_contrib.sensors.rst @@ -0,0 +1,39 @@ +isaaclab_contrib.sensors +======================== + +.. automodule:: isaaclab_contrib.sensors + + .. rubric:: Classes + + .. autosummary:: + + VisuoTactileSensor + VisuoTactileSensorCfg + VisuoTactileSensorData + GelSightRenderCfg + + +Visuo-Tactile Sensor +-------------------- + +.. autoclass:: VisuoTactileSensor + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: VisuoTactileSensorData + :members: + :inherited-members: + :exclude-members: __init__ + +.. autoclass:: VisuoTactileSensorCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type + +.. autoclass:: GelSightRenderCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__ diff --git a/docs/source/api/lab_mimic/isaaclab_mimic.envs.rst b/docs/source/api/lab_mimic/isaaclab_mimic.envs.rst index 7b4146f5b608..ea8dc82d1615 100644 --- a/docs/source/api/lab_mimic/isaaclab_mimic.envs.rst +++ b/docs/source/api/lab_mimic/isaaclab_mimic.envs.rst @@ -7,20 +7,138 @@ .. autosummary:: - FrankaCubeStackIKRelMimicEnv - FrankaCubeStackIKRelMimicEnvCfg + isaaclab_mimic.envs.franka_stack_ik_rel_mimic_env.FrankaCubeStackIKRelMimicEnv + isaaclab_mimic.envs.franka_stack_ik_rel_mimic_env_cfg.FrankaCubeStackIKRelMimicEnvCfg + isaaclab_mimic.envs.franka_stack_ik_abs_mimic_env.FrankaCubeStackIKAbsMimicEnv + isaaclab_mimic.envs.franka_stack_ik_abs_mimic_env_cfg.FrankaCubeStackIKAbsMimicEnvCfg + isaaclab_mimic.envs.galbot_stack_rmp_rel_mimic_env.RmpFlowGalbotCubeStackRelMimicEnv + isaaclab_mimic.envs.galbot_stack_rmp_rel_mimic_env_cfg.RmpFlowGalbotLeftArmGripperCubeStackRelMimicEnvCfg + isaaclab_mimic.envs.galbot_stack_rmp_rel_mimic_env_cfg.RmpFlowGalbotRightArmSuctionCubeStackRelMimicEnvCfg + isaaclab_mimic.envs.galbot_stack_rmp_abs_mimic_env.RmpFlowGalbotCubeStackAbsMimicEnv + isaaclab_mimic.envs.galbot_stack_rmp_abs_mimic_env_cfg.RmpFlowGalbotLeftArmGripperCubeStackAbsMimicEnvCfg + isaaclab_mimic.envs.galbot_stack_rmp_abs_mimic_env_cfg.RmpFlowGalbotRightArmSuctionCubeStackAbsMimicEnvCfg + isaaclab_mimic.envs.pick_place_mimic_env.PickPlaceRelMimicEnv + isaaclab_mimic.envs.pick_place_mimic_env.PickPlaceAbsMimicEnv + isaaclab_mimic.envs.agibot_place_upright_mug_mimic_env_cfg.RmpFlowAgibotPlaceUprightMugMimicEnvCfg + isaaclab_mimic.envs.agibot_place_toy2box_mimic_env_cfg.RmpFlowAgibotPlaceToy2BoxMimicEnvCfg + +Franka Environments +------------------- Franka Cube Stack IK Rel Mimic Env ----------------------------------- +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -.. autoclass:: FrankaCubeStackIKRelMimicEnv +.. autoclass:: isaaclab_mimic.envs.franka_stack_ik_rel_mimic_env.FrankaCubeStackIKRelMimicEnv :members: :inherited-members: + :show-inheritance: Franka Cube Stack IK Rel Mimic Env Cfg --------------------------------------- +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.franka_stack_ik_rel_mimic_env_cfg.FrankaCubeStackIKRelMimicEnvCfg + :members: + :inherited-members: + :show-inheritance: + +Franka Cube Stack IK Abs Mimic Env +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.franka_stack_ik_abs_mimic_env.FrankaCubeStackIKAbsMimicEnv + :members: + :inherited-members: + :show-inheritance: + +Franka Cube Stack IK Abs Mimic Env Cfg +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.franka_stack_ik_abs_mimic_env_cfg.FrankaCubeStackIKAbsMimicEnvCfg + :members: + :inherited-members: + :show-inheritance: + +Galbot Environments +------------------- + +Galbot Cube Stack Rel Mimic Env (RmpFlow) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.galbot_stack_rmp_rel_mimic_env.RmpFlowGalbotCubeStackRelMimicEnv + :members: + :inherited-members: + :show-inheritance: + +Galbot Left Arm Gripper Cube Stack Rel Mimic Env Cfg +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.galbot_stack_rmp_rel_mimic_env_cfg.RmpFlowGalbotLeftArmGripperCubeStackRelMimicEnvCfg + :members: + :inherited-members: + :show-inheritance: + +Galbot Right Arm Suction Cube Stack Rel Mimic Env Cfg +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.galbot_stack_rmp_rel_mimic_env_cfg.RmpFlowGalbotRightArmSuctionCubeStackRelMimicEnvCfg + :members: + :inherited-members: + :show-inheritance: + +Galbot Cube Stack Abs Mimic Env (RmpFlow) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.galbot_stack_rmp_abs_mimic_env.RmpFlowGalbotCubeStackAbsMimicEnv + :members: + :inherited-members: + :show-inheritance: + +Galbot Left Arm Gripper Cube Stack Abs Mimic Env Cfg +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.galbot_stack_rmp_abs_mimic_env_cfg.RmpFlowGalbotLeftArmGripperCubeStackAbsMimicEnvCfg + :members: + :inherited-members: + :show-inheritance: + +Galbot Right Arm Suction Cube Stack Abs Mimic Env Cfg +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.galbot_stack_rmp_abs_mimic_env_cfg.RmpFlowGalbotRightArmSuctionCubeStackAbsMimicEnvCfg + :members: + :inherited-members: + :show-inheritance: + +Agibot Environments +------------------- + +Pick Place Rel Mimic Env +~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.pick_place_mimic_env.PickPlaceRelMimicEnv + :members: + :inherited-members: + :show-inheritance: + +Pick Place Abs Mimic Env +~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.pick_place_mimic_env.PickPlaceAbsMimicEnv + :members: + :inherited-members: + :show-inheritance: + +Agibot Place Upright Mug Mimic Env Cfg +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: isaaclab_mimic.envs.agibot_place_upright_mug_mimic_env_cfg.RmpFlowAgibotPlaceUprightMugMimicEnvCfg + :members: + :inherited-members: + :show-inheritance: + +Agibot Place Toy2Box Mimic Env Cfg +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -.. autoclass:: FrankaCubeStackIKRelMimicEnvCfg +.. autoclass:: isaaclab_mimic.envs.agibot_place_toy2box_mimic_env_cfg.RmpFlowAgibotPlaceToy2BoxMimicEnvCfg :members: :inherited-members: :show-inheritance: diff --git a/docs/source/api/lab_rl/isaaclab_rl.rst b/docs/source/api/lab_rl/isaaclab_rl.rst index 77a4ca1bd96e..32b4b4c62468 100644 --- a/docs/source/api/lab_rl/isaaclab_rl.rst +++ b/docs/source/api/lab_rl/isaaclab_rl.rst @@ -1,4 +1,6 @@ -๏ปฟisaaclab_rl +๏ปฟ.. _api-isaaclab-rl: + +isaaclab_rl =========== .. automodule:: isaaclab_rl diff --git a/docs/source/api/lab_tasks/isaaclab_tasks.utils.rst b/docs/source/api/lab_tasks/isaaclab_tasks.utils.rst index b653c14e48ef..3ffd8f075bc5 100644 --- a/docs/source/api/lab_tasks/isaaclab_tasks.utils.rst +++ b/docs/source/api/lab_tasks/isaaclab_tasks.utils.rst @@ -4,5 +4,3 @@ .. automodule:: isaaclab_tasks.utils :members: :imported-members: - - .. rubric:: Submodules diff --git a/docs/source/deployment/cloudxr_teleoperation_cluster.rst b/docs/source/deployment/cloudxr_teleoperation_cluster.rst new file mode 100644 index 000000000000..2b374bcbe548 --- /dev/null +++ b/docs/source/deployment/cloudxr_teleoperation_cluster.rst @@ -0,0 +1,207 @@ +.. _cloudxr-teleoperation-cluster: + +Deploying CloudXR Teleoperation on Kubernetes +============================================= + +.. currentmodule:: isaaclab + +This section explains how to deploy CloudXR Teleoperation for Isaac Lab on a Kubernetes (K8s) cluster. + +.. _k8s-system-requirements: + +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 +--------------------- + +* ``kubectl`` on your host computer + + * If you use MicroK8s, you already have ``microk8s kubectl`` + * Otherwise follow the `official kubectl installation guide `_ + +* ``helm`` on your host computer + + * If you use MicroK8s, you already have ``microk8s helm`` + * Otherwise follow the `official Helm installation guide `_ + +* Access to NGC public registry from your Kubernetes cluster, in particular these container images: + + * ``https://catalog.ngc.nvidia.com/orgs/nvidia/containers/isaac-lab`` + * ``https://catalog.ngc.nvidia.com/orgs/nvidia/containers/cloudxr-runtime`` + +* NVIDIA GPU Operator or equivalent installed in your Kubernetes cluster to expose NVIDIA GPUs +* NVIDIA Container Toolkit installed on the nodes of your Kubernetes cluster + +Preparation +----------- + +On your host computer, you should have already configured ``kubectl`` to access your Kubernetes cluster. To validate, run the following command and verify it returns your nodes correctly: + +.. code:: bash + + kubectl get node + +If you are installing this to your own Kubernetes cluster instead of using the setup described in the :ref:`k8s-appendix`, your role in the K8s cluster should have at least the following RBAC permissions: + +.. code:: yaml + + rules: + - apiGroups: [""] + resources: ["configmaps"] + verbs: ["get", "list", "watch", "create", "update", "patch", "delete"] + - apiGroups: ["apps"] + resources: ["deployments", "replicasets"] + verbs: ["get", "list", "watch", "create", "update", "patch", "delete"] + - apiGroups: [""] + resources: ["pods"] + verbs: ["get", "list", "watch", "create", "update", "patch", "delete"] + - apiGroups: [""] + resources: ["services"] + verbs: ["get", "list", "watch", "create", "update", "patch", "delete"] + +.. _k8s-installation: + +Installation +------------ + +.. note:: + + The following steps are verified on a MicroK8s cluster with GPU Operator installed (see configurations in the :ref:`k8s-appendix`). You can configure your own K8s cluster accordingly if you encounter issues. + +#. Download the Helm chart from NGC (get your NGC API key based on the `public guide `_): + + .. code:: bash + + helm fetch https://helm.ngc.nvidia.com/nvidia/charts/isaac-lab-teleop-2.3.0.tgz \ + --username='$oauthtoken' \ + --password= + +#. Install and run the CloudXR Teleoperation for Isaac Lab pod in the default namespace, consuming all host GPUs: + + .. code:: bash + + helm upgrade --install hello-isaac-teleop isaac-lab-teleop-2.3.0.tgz \ + --set fullnameOverride=hello-isaac-teleop \ + --set hostNetwork="true" + + .. note:: + + You can remove the need for host network by creating an external LoadBalancer VIP (e.g., with MetalLB), and setting the environment variable ``NV_CXR_ENDPOINT_IP`` when deploying the Helm chart: + + .. code:: yaml + + # local_values.yml file example: + fullnameOverride: hello-isaac-teleop + streamer: + extraEnvs: + - name: NV_CXR_ENDPOINT_IP + value: "" + - name: ACCEPT_EULA + value: "Y" + + .. code:: bash + + # command + helm upgrade --install --values local_values.yml \ + hello-isaac-teleop isaac-lab-teleop-2.3.0.tgz + +#. Verify the deployment is completed: + + .. code:: bash + + kubectl wait --for=condition=available --timeout=300s \ + deployment/hello-isaac-teleop + + After the pod is running, it might take approximately 5-8 minutes to complete loading assets and start streaming. + +Uninstallation +-------------- + +You can uninstall by simply running: + +.. code:: bash + + helm uninstall hello-isaac-teleop + +.. _k8s-appendix: + +Appendix: Setting Up a Local K8s Cluster with MicroK8s +------------------------------------------------------ + +Your local workstation should have the NVIDIA Container Toolkit and its dependencies installed. Otherwise, the following setup will not work. + +Cleaning Up Existing Installations (Optional) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code:: bash + + # Clean up the system to ensure we start fresh + sudo snap remove microk8s + sudo snap remove helm + sudo apt-get remove docker-ce docker-ce-cli containerd.io + # If you have snap docker installed, remove it as well + sudo snap remove docker + +Installing MicroK8s +~~~~~~~~~~~~~~~~~~~ + +.. code:: bash + + sudo snap install microk8s --classic + +Installing NVIDIA GPU Operator +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code:: bash + + microk8s helm repo add nvidia https://helm.ngc.nvidia.com/nvidia + microk8s helm repo update + microk8s helm install gpu-operator \ + -n gpu-operator \ + --create-namespace nvidia/gpu-operator \ + --set toolkit.env[0].name=CONTAINERD_CONFIG \ + --set toolkit.env[0].value=/var/snap/microk8s/current/args/containerd-template.toml \ + --set toolkit.env[1].name=CONTAINERD_SOCKET \ + --set toolkit.env[1].value=/var/snap/microk8s/common/run/containerd.sock \ + --set toolkit.env[2].name=CONTAINERD_RUNTIME_CLASS \ + --set toolkit.env[2].value=nvidia \ + --set toolkit.env[3].name=CONTAINERD_SET_AS_DEFAULT \ + --set-string toolkit.env[3].value=true + +.. note:: + + If you have configured the GPU operator to use volume mounts for ``DEVICE_LIST_STRATEGY`` on the device plugin and disabled ``ACCEPT_NVIDIA_VISIBLE_DEVICES_ENVVAR_WHEN_UNPRIVILEGED`` on the toolkit, this configuration is currently unsupported, as there is no method to ensure the assigned GPU resource is consistently shared between containers of the same pod. + +Verifying Installation +~~~~~~~~~~~~~~~~~~~~~~ + +Run the following command to verify that all pods are running correctly: + +.. code:: bash + + microk8s kubectl get pods -n gpu-operator + +You should see output similar to: + +.. code:: text + + NAMESPACE NAME READY STATUS RESTARTS AGE + gpu-operator gpu-operator-node-feature-discovery-gc-76dc6664b8-npkdg 1/1 Running 0 77m + gpu-operator gpu-operator-node-feature-discovery-master-7d6b448f6d-76fqj 1/1 Running 0 77m + gpu-operator gpu-operator-node-feature-discovery-worker-8wr4n 1/1 Running 0 77m + gpu-operator gpu-operator-86656466d6-wjqf4 1/1 Running 0 77m + gpu-operator nvidia-container-toolkit-daemonset-qffh6 1/1 Running 0 77m + gpu-operator nvidia-dcgm-exporter-vcxsf 1/1 Running 0 77m + gpu-operator nvidia-cuda-validator-x9qn4 0/1 Completed 0 76m + gpu-operator nvidia-device-plugin-daemonset-t4j4k 1/1 Running 0 77m + gpu-operator gpu-feature-discovery-8dms9 1/1 Running 0 77m + gpu-operator nvidia-operator-validator-gjs9m 1/1 Running 0 77m + +Once all pods are running, you can proceed to the :ref:`k8s-installation` section. diff --git a/docs/source/deployment/cluster.rst b/docs/source/deployment/cluster.rst index 467fda90f46d..ab9e03874e7a 100644 --- a/docs/source/deployment/cluster.rst +++ b/docs/source/deployment/cluster.rst @@ -204,7 +204,7 @@ ANYmal rough terrain locomotion training can be executed with the following comm The above will, in addition, also render videos of the training progress and store them under ``isaaclab/logs`` directory. .. _Singularity: https://docs.sylabs.io/guides/2.6/user-guide/index.html -.. _ETH Zurich Euler: https://scicomp.ethz.ch/wiki/Euler +.. _ETH Zurich Euler: https://www.gdc-docs.ethz.ch/EulerManual/site/overview/ .. _PBS Official Site: https://openpbs.org/ .. _apptainer: https://apptainer.org/ .. _documentation: https://www.apptainer.org/docs/admin/main/installation.html#install-ubuntu-packages diff --git a/docs/source/deployment/docker.rst b/docs/source/deployment/docker.rst index 2532b91439f8..9dad372c9c7d 100644 --- a/docs/source/deployment/docker.rst +++ b/docs/source/deployment/docker.rst @@ -47,9 +47,12 @@ needed to run Isaac Lab inside a Docker container. A subset of these are summari Dockerfiles which end with something else, (i.e. ``Dockerfile.ros2``) build an `image extension <#isaac-lab-image-extensions>`_. * **docker-compose.yaml**: Creates mounts to allow direct editing of Isaac Lab code from the host machine that runs the container. It also creates several named volumes such as ``isaac-cache-kit`` to - store frequently re-used resources compiled by Isaac Sim, such as shaders, and to retain logs, data, and documents. + store frequently reused resources compiled by Isaac Sim, such as shaders, and to retain logs, data, and documents. * **.env.base**: Stores environment variables required for the ``base`` build process and the container itself. ``.env`` files which end with something else (i.e. ``.env.ros2``) define these for `image extension <#isaac-lab-image-extensions>`_. +* **docker-compose.cloudxr-runtime.patch.yaml**: A patch file that is applied to enable CloudXR Runtime support for + streaming to compatible XR devices. It defines services and volumes for CloudXR Runtime and the base. +* **.env.cloudxr-runtime**: Environment variables for the CloudXR Runtime support. * **container.py**: A utility script that interfaces with tools in ``utils`` to configure and build the image, and run and interact with the container. @@ -73,6 +76,7 @@ Running the Container The script ``container.py`` parallels basic ``docker compose`` commands. Each can accept an `image extension argument <#isaac-lab-image-extensions>`_, or else they will default to the ``base`` image extension. These commands are: +* **build**: This builds the image for the given profile. It does not bring up the container. * **start**: This builds the image and brings up the container in detached mode (i.e. in the background). * **enter**: This begins a new bash process in an existing Isaac Lab container, and which can be exited without bringing down the container. @@ -114,6 +118,23 @@ directories to the ``docker/artifacts`` directory. This is useful for copying th ./docker/container.py stop +CloudXR Runtime Support +~~~~~~~~~~~~~~~~~~~~~~~ + +To enable CloudXR Runtime for streaming to compatible XR devices, you need to apply the patch file +``docker-compose.cloudxr-runtime.patch.yaml`` to run CloudXR Runtime container. The patch file defines services and +volumes for CloudXR Runtime and base. The environment variables required for CloudXR Runtime are specified in the +``.env.cloudxr-runtime`` file. To start or stop the CloudXR runtime container with base, use the following command: + +.. code:: bash + + # Start CloudXR Runtime container with base. + ./docker/container.py start --files docker-compose.cloudxr-runtime.patch.yaml --env-file .env.cloudxr-runtime + + # Stop CloudXR Runtime container and base. + ./docker/container.py stop --files docker-compose.cloudxr-runtime.patch.yaml --env-file .env.cloudxr-runtime + + X11 forwarding ~~~~~~~~~~~~~~ @@ -213,20 +234,35 @@ Isaac Lab Image Extensions The produced image depends on the arguments passed to ``container.py start`` and ``container.py stop``. These commands accept an image extension parameter as an additional argument. If no argument is passed, then this parameter defaults to ``base``. Currently, the only valid values are (``base``, ``ros2``). -Only one image extension can be passed at a time. The produced container will be named ``isaac-lab-${profile}``, -where ``${profile}`` is the image extension name. +Only one image extension can be passed at a time. The produced image and container will be named +``isaac-lab-${profile}``, where ``${profile}`` is the image extension name. + +``suffix`` is an optional string argument to ``container.py`` that specifies a docker image and +container name suffix, which can be useful for development purposes. By default ``${suffix}`` is the empty string. +If ``${suffix}`` is a nonempty string, then the produced docker image and container will be named +``isaac-lab-${profile}-${suffix}``, where a hyphen is inserted between ``${profile}`` and ``${suffix}`` in +the name. ``suffix`` should not be used with cluster deployments. .. code:: bash - # start base by default + # start base by default, named isaac-lab-base ./docker/container.py start - # stop base explicitly + # stop base explicitly, named isaac-lab-base ./docker/container.py stop base - # start ros2 container + # start ros2 container named isaac-lab-ros2 ./docker/container.py start ros2 - # stop ros2 container + # stop ros2 container named isaac-lab-ros2 ./docker/container.py stop ros2 + # start base container named isaac-lab-base-custom + ./docker/container.py start base --suffix custom + # stop base container named isaac-lab-base-custom + ./docker/container.py stop base --suffix custom + # start ros2 container named isaac-lab-ros2-custom + ./docker/container.py start ros2 --suffix custom + # stop ros2 container named isaac-lab-ros2-custom + ./docker/container.py stop ros2 --suffix custom + The passed image extension argument will build the image defined in ``Dockerfile.${image_extension}``, with the corresponding `profile`_ in the ``docker-compose.yaml`` and the envars from ``.env.${image_extension}`` in addition to the ``.env.base``, if any. @@ -256,17 +292,23 @@ Running Pre-Built Isaac Lab Container In Isaac Lab 2.0 release, we introduced a minimal pre-built container that contains a very minimal set of Isaac Sim and Omniverse dependencies, along with Isaac Lab 2.0 pre-built into the container. This container allows users to pull the container directly from NGC without requiring a local build of -the docker image. The Isaac Lab 2.0 source code will be available in this container under ``/workspace/IsaacLab``. +the docker image. The Isaac Lab source code will be available in this container under ``/workspace/IsaacLab``. This container is designed for running **headless** only and does not allow for X11 forwarding or running with the GUI. Please only use this container for headless training. For other use cases, we recommend following the above steps to build your own Isaac Lab docker image. +.. note:: + + Currently, we only provide docker images with every major release of Isaac Lab. + For example, we provide the docker image for release 2.0.0 and 2.1.0, but not 2.0.2. + In the future, we will provide docker images for every minor release of Isaac Lab. + To pull the minimal Isaac Lab container, run: .. code:: bash - docker pull nvcr.io/nvidia/isaac-lab:2.0.2 + docker pull nvcr.io/nvidia/isaac-lab:2.3.2 To run the Isaac Lab container with an interactive bash session, run: @@ -282,7 +324,26 @@ 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.0.2 + nvcr.io/nvidia/isaac-lab:2.3.2 + +To enable rendering through X11 forwarding, run: + +.. code:: bash + + xhost + + docker run --name isaac-lab --entrypoint bash -it --gpus all -e "ACCEPT_EULA=Y" --rm --network=host \ + -e "PRIVACY_CONSENT=Y" \ + -e DISPLAY \ + -v $HOME/.Xauthority:/root/.Xauthority \ + -v ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw \ + -v ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw \ + -v ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw \ + -v ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw \ + -v ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw \ + -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.3.2 To run an example within the container, run: diff --git a/docs/source/deployment/index.rst b/docs/source/deployment/index.rst index c8e07ef9e2e8..235a23c9d754 100644 --- a/docs/source/deployment/index.rst +++ b/docs/source/deployment/index.rst @@ -1,3 +1,5 @@ +.. _container-deployment: + Container Deployment ==================== @@ -11,12 +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 - run_docker_example + docker + run_docker_example + cluster + cloudxr_teleoperation_cluster diff --git a/docs/source/experimental-features/bleeding-edge.rst b/docs/source/experimental-features/bleeding-edge.rst new file mode 100644 index 000000000000..5927ba1ae8df --- /dev/null +++ b/docs/source/experimental-features/bleeding-edge.rst @@ -0,0 +1,11 @@ +Welcome to the bleeding edge! +============================= + +Isaac Lab is open source because our intention is to grow a community of open collaboration for robotic simulation. +We believe that robust tools are crucial for the future of robotics. + +Sometimes new features may require extensive changes to the internal structure of Isaac Lab. +Directly integrating such features before they are complete and without feedback from the full community could cause serious issues for users caught unaware. + +To address this, some major features will be released as Experimental Feature Branches. +This way, the community can experiment with and contribute to the feature before it's fully integrated, reducing the likelihood of being derailed by unexpected and new errors. diff --git a/docs/source/experimental-features/newton-physics-integration/index.rst b/docs/source/experimental-features/newton-physics-integration/index.rst new file mode 100644 index 000000000000..48d19caca874 --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/index.rst @@ -0,0 +1,47 @@ +Newton Physics Integration +=========================== + +`Newton `_ is a GPU-accelerated, extensible, and differentiable physics simulation engine designed for robotics, research, +and advanced simulation workflows. Built on top of `NVIDIA Warp `_ and integrating MuJoCo Warp, Newton provides high-performance +simulation, modern Python APIs, and a flexible architecture for both users and developers. + +Newton is an Open Source community-driven project with contributions from NVIDIA, Google Deep Mind, and Disney Research, +managed through the Linux Foundation. + +This `experimental feature branch `_ of Isaac Lab provides an initial integration with the Newton Physics Engine, and is +under active development. Many features are not yet supported, and only a limited set of classic RL and flat terrain locomotion +reinforcement learning examples are included at the moment. + +Both this Isaac Lab integration branch and Newton itself are under heavy development. We intend to support additional +features for other reinforcement learning and imitation learning workflows in the future, but the above tasks should be +a good lens through which to understand how Newton integration works in Isaac Lab. + +We have validated Newton simulation against PhysX by transferring learned policies from Newton to PhysX and vice versa +Furthermore, we have also successfully deployed a Newton-trained locomotion policy to a G1 robot. Please see :ref:`here ` for more information. + +Newton can support `multiple solvers `_ for handling different types of physics simulation, but for the moment, the Isaac +Lab integration focuses primarily on the MuJoCo-Warp solver. + +Future updates of this branch and Newton should include both ongoing improvements in performance as well as integration +with additional solvers. + +Note that this branch does not include support for the PhysX physics engine - only Newton is supported. We are considering +several possible paths to continue to support PhysX within Lab, and feedback from users about their needs around that would be appreciated. + +During the early development phase of both Newton and this Isaac Lab integration, you are likely to encounter breaking +changes as well as limited documentation. We do not expect to be able to provide official support or debugging assistance +until the framework has reached an official release. We appreciate your understanding and patience as we work to deliver a robust and polished framework! + + +.. toctree:: + :maxdepth: 2 + :titlesonly: + + installation + isaaclab_newton-beta-2 + training-environments + visualization + limitations-and-known-bugs + solver-transitioning + sim-to-sim + sim-to-real diff --git a/docs/source/experimental-features/newton-physics-integration/installation.rst b/docs/source/experimental-features/newton-physics-integration/installation.rst new file mode 100644 index 000000000000..be0f82632e66 --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/installation.rst @@ -0,0 +1,78 @@ +Installation +============ + +Installing the Newton physics integration branch requires three things: + +1) The ``feature/newton`` branch of Isaac Lab +2) Ubuntu 22.04 or 24.04 (Windows will be supported soon) +3) [Optional] Isaac sim 5.1 (Isaac Sim is not required if the Omniverse visualizer is not used) + +To begin, verify the version of Isaac Sim by checking the title of the window created when launching the simulation app. Alternatively, you can +find more explicit version information under the ``Help -> About`` menu within the app. +If your version is less than 5.1, you must first `update or reinstall Isaac Sim `_ before +you can proceed further. + +Next, navigate to the root directory of your local copy of the Isaac Lab repository and open a terminal. + +Make sure we are on the ``feature/newton`` branch by running the following command: + +.. code-block:: bash + + git checkout feature/newton + +Below, we provide instructions for installing Isaac Sim through pip. + + +Pip Installation +---------------- + +We recommend using conda for managing your python environments. Conda can be downloaded and installed from `here `_. + +If you previously already have a virtual environment for Isaac Lab, please ensure to start from a fresh environment to avoid any dependency conflicts. +If you have installed earlier versions of mujoco, mujoco-warp, or newton packages through pip, we recommend first +cleaning your pip cache with ``pip cache purge`` to remove any cache of earlier versions that may be conflicting with the latest. + +Create a new conda environment: + +.. code-block:: bash + + conda create -n env_isaaclab python=3.11 + +Activate the environment: + +.. code-block:: bash + + conda activate env_isaaclab + +Install the correct version of torch and torchvision: + +.. code-block:: bash + + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 + +[Optional] Install Isaac Sim 5.1: + +.. code-block:: bash + + pip install "isaacsim[all,extscache]==5.1.0" --extra-index-url https://pypi.nvidia.com + +Install Isaac Lab extensions and dependencies: + +.. code-block:: bash + + ./isaaclab.sh -i + + +Testing the Installation +------------------------ + +To verify that the installation was successful, run the following command from the root directory of your Isaac Lab repository: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/environments/zero_agent.py --task Isaac-Cartpole-Direct-v0 --num_envs 128 + + +Note that since Newton requires a more recent version of Warp than Isaac Sim 5.1, there may be some incompatibility issues +that could result in errors such as ``ModuleNotFoundError: No module named 'warp.sim'``. These are ok to ignore and should not +impact usability. diff --git a/docs/source/experimental-features/newton-physics-integration/isaaclab_newton-beta-2.rst b/docs/source/experimental-features/newton-physics-integration/isaaclab_newton-beta-2.rst new file mode 100644 index 000000000000..2e7b4dd9ec42 --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/isaaclab_newton-beta-2.rst @@ -0,0 +1,52 @@ +Isaac Lab - Newton Beta 2 +========================= + +Isaac Lab - Newton Beta 2 (feature/newton branch) provides Newton physics engine integration for Isaac Lab. We refactored our code so that we can not only support PhysX and Newton, but +any other physics engine, enabling users to bring their own physics engine to Isaac Lab if they desire. To enable this, we introduce base implementations of +our ``simulation interfaces``, :class:`~isaaclab.assets.articulation.Articulation` or :class:`~isaaclab.sensors.ContactSensor` for instance. These provide a +set of abstract methods that all physics engines must implement. In turn this allows all of the default Isaac Lab environments to work with any physics engine. +This also allows us to ensure that Isaac Lab - Newton Beta 2 is backwards compatible with Isaac Lab 2.X. For engine specific calls, users could get the underlying view of +the physics engine and call the engine specific APIs directly. + +However, as we are refactoring the code, we are also looking at ways to limit the overhead of Isaac Lab's. In an effort to minimize the overhead, we are moving +all our low level code away from torch, and instead will rely heavily on warp. This will allow us to write low level code that is more efficient, and also +to take advantage of the cuda-graphing. However, this means that the ``data classes`` such as :class:`~isaaclab.assets.articulation.ArticulationData` or +:class:`~isaaclab.sensors.ContactSensorData` will only return warp arrays. Users will hence have to call ``wp.to_torch`` to convert them to torch tensors if they desire. +Our setters/writers will support both warp arrays and torch tensors, and will use the most optimal strategy to update the warp arrays under the hood. This minimizes the +amount of changes required for users to migrate to Isaac Lab - Newton Beta 2. + +Another new feature of the writers and setters is the ability to provide them with masks and complete data (as opposed to indices and partial data in Isaac Lab 2.X). +Note that this feature will be available along with the ability to provide indices and partial data, and that the default behavior will still be to provide indices and partial data. +However, if using warp, users will have to provide masks and complete data. In general we encourage users to move to adopt this new feature as, if done well, it will +reduce on the fly memory allocations, and should result in better performance. + +On the optimization front, we decided to change quaternion conventions. Originally, Isaac Lab and Isaac Sim both adopted the ``wxyz`` convention. However, we were doing several +conversions to and from ``xyzw`` in our setters/writers as PhysX uses the ``xyzw`` convention. Since both Newton and Warp, also use the ``xyzw`` convention, we decided to change +our default convention to ``xyzw``. This means that all our APIs will now return quaternions in the ``xyzw`` convention. This is likely a breaking change for all the custom +mdps that are not using our :mod:`~isaaclab.utils.math` module. While this change is substantial, it should make things more consistent for when users are using the simulation +views directly, and will remove needless conversions. + +Finally, alongside the new isaaclab_newton extension, we are also introducing new isaaclab_experimental and isaaclab_task_experimental extensions. These extensions will allow +us to quickly bring new features to Isaac Lab main while giving them the time they need to mature before being fully integrated into the core Isaac Lab extensions. In this release, +we are introducing cuda-graphing support for direct rl tasks. This drastically reduces Isaac Lab's overhead making training faster. Try them out and let us know what you think! + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-Direct-Warp-v0 --num_envs 4096 --headless + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Ant-Direct-Warp-v0 --num_envs 4096 --headless + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Humanoid-Direct-Warp-v0 --num_envs 4096 --headless + + +What's Next? +============ + +Isaac Lab 3.0 is the upcoming release of Isaac Lab, which will be compatible with Isaac Sim 6.0, and at the same time will support the new Newton physics engine. +This will allow users to train policies on the Newton physics engine, or PhysX. To accommodate this major code refactoring are required. In this section, we +will go over some of the changes, how that will affect Isaac Lab 2.X users, and how to migrate to Isaac Lab 3.0. The current branch of ``feature/newton`` gives +a glance of what is to come. While the changes to the internal code structure are significant, the changes to the user API are minimal. 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 new file mode 100644 index 000000000000..e5eab3996d8a --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/limitations-and-known-bugs.rst @@ -0,0 +1,55 @@ +Limitations +=========== + +During the early development phase of both Newton and this Isaac Lab integration, +you are likely to encounter breaking changes as well as limited documentation. + +We do not expect to be able to provide support or debugging assistance until the framework has reached an official release. + +Here is a non-exhaustive list of capabilities currently supported in the Newton experimental feature branch grouped by extension: + +* isaaclab: + * 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: + * 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 (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 + * Unitree G1 + * Unitree H1 + * Manipulation reach + * Franka + * UR10 diff --git a/docs/source/experimental-features/newton-physics-integration/sim-to-real.rst b/docs/source/experimental-features/newton-physics-integration/sim-to-real.rst new file mode 100644 index 000000000000..6b7a952a76c4 --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/sim-to-real.rst @@ -0,0 +1,92 @@ +.. _sim2real: + +Sim-to-Real Policy Transfer +=========================== +Deploying policies from simulation to real robots involves important nuances that must be addressed. +This section provides a high-level guide for training policies that can be deployed on a real Unitree G1 robot. +The key challenge is that not all observations available in simulation can be directly measured by real robot sensors. +This means RL-trained policies cannot be directly deployed unless they use only sensor-available observations. For example, while real robot IMU sensors provide angular acceleration (which can be integrated to get angular velocity), they cannot directly measure linear velocity. Therefore, if a policy relies on base linear velocity during training, this information must be removed before real robot deployment. + + +Requirements +~~~~~~~~~~~~ + +We assume that policies from this workflow are first verified through sim-to-sim transfer before real robot deployment. +Please see :ref:`here ` for more information. + + +Overview +-------- + +This section demonstrates a sim-to-real workflow using teacherโ€“student distillation for the Unitree G1 +velocity-tracking task with the Newton backend. + +The teacherโ€“student distillation workflow consists of three stages: + +1. Train a teacher policy with privileged observations that are not available in real-world sensors. +2. Distill a student policy that excludes privileged terms (e.g., root linear velocity) by behavior cloning from the teacher policy. +3. Fine-tune the student policy with RL using only real-sensor observations. + +The teacher and student observation groups are implemented in the velocity task configuration. See the following source for details: + +- Teacher observations: ``PolicyCfg(ObsGroup)`` in `velocity_env_cfg.py `__ +- Student observations: ``StudentPolicyCfg(ObsGroup)`` in `velocity_env_cfg.py `__ + + +1. Train the teacher policy +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Train the teacher policy for the G1 velocity task using the Newton backend. The task ID is ``Isaac-Velocity-Flat-G1-v1`` + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Velocity-Flat-G1-v1 --num_envs=4096 + +The teacher policy includes privileged observations (e.g., root linear velocity) defined in ``PolicyCfg(ObsGroup)``. + + +2. Distill the student policy (remove privileged terms) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +During distillation, the student policy learns to mimic the teacher through behavior cloning by minimizing the mean squared error +between their actions: :math:`loss = MSE(\pi(O_{teacher}), \pi(O_{student}))`. + +The student policy only uses observations available from real sensors (see ``StudentPolicyCfg(ObsGroup)`` +in `velocity_env_cfg.py `__). +Specifically: **Root angular velocity** and **Projected gravity** come from the IMU sensor, **Joint positions and velocities** come from joint encoders, and **Actions** are the joint torques applied by the controller. + +Run the student distillation task ``Velocity-G1-Distillation-v1`` using ``--load_run`` and ``--checkpoint`` to specify the teacher policy you want to distill from. + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Velocity-G1-Distillation-v1 --num_envs=4096 --load_run 2025-08-13_23-53-28 --checkpoint model_1499.pt + +.. note:: + + Use the correct ``--load_run`` and ``--checkpoint`` to ensure you distill from the intended teacher policy. + + +3. Fine-tune the student policy with RL +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Fine-tune the distilled student policy using RL with the ``Velocity-G1-Student-Finetune-v1`` task. +Use ``--load_run`` and ``--checkpoint`` to initialize from the distilled policy. + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Velocity-G1-Student-Finetune-v1 --num_envs=4096 --load_run 2025-08-20_16-06-52_distillation --checkpoint model_1499.pt + +This starts from the distilled student policy and improves it further with RL training. + +.. note:: + + Make sure ``--load_run`` and ``--checkpoint`` point to the correct initial policy (usually the latest checkpoint from the distillation step). + +You can replay the student policy via: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/play.py --task=Velocity-G1-Student-Finetune-v1 --num_envs=32 --visualizer newton + + +This exports the policy as ``.pt`` and ``.onnx`` files in the run's export directory, ready for real robot deployment. 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 new file mode 100644 index 000000000000..8eebbdffac3f --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/sim-to-sim.rst @@ -0,0 +1,171 @@ +.. _sim2sim: + +Sim-to-Sim Policy Transfer +========================== +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 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. + +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 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 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: # Source backend joint order + - joint_1 + - joint_2 + # ... + target_joint_names: # Target backend joint order + - joint_1 + - joint_2 + # ... + +The script automatically computes the necessary mappings for locomotion tasks. + + +PhysX-to-Newton Transfer +~~~~~~~~~~~~~~~~~~~~~~~~ + +To run a PhysX-trained policy with the Newton backend, use this command template: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ + --task= \ + --num_envs=32 \ + --checkpoint \ + --policy_transfer_file \ + --visualizer newton + +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/physx_to_newton_g1.yaml \ + --visualizer newton + +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/physx_to_newton_h1.yaml \ + --visualizer newton + + +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/physx_to_newton_go2.yaml \ + --visualizer newton + + +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/physx_to_newton_anymal_d.yaml \ + --visualizer newton + +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 +~~~~~~~~~~~~~~~~~~~~~ + +- 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/solver-transitioning.rst b/docs/source/experimental-features/newton-physics-integration/solver-transitioning.rst new file mode 100644 index 000000000000..db85df0f991f --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/solver-transitioning.rst @@ -0,0 +1,73 @@ +Solver Transitioning +==================== + +Transitioning to the Newton physics engine introduces new physics solvers that handle simulation using different numerical approaches. +While Newton supports several different solvers, our initial focus for Isaac Lab is on using the MuJoCo-Warp solver from Google DeepMind. + +The way the physics scene itself is defined does not change - we continue to use USD as the primary way to set basic parameters of objects and robots in the scene, +and for current environments, the exact same USD files used for the PhysX-based Isaac Lab are used. +In the future, that may change, as new USD schemas are under development that capture additional physics parameters. + +What does require change is the way that some solver-specific settings are configured. +Tuning these parameters can have a significant impact on both simulation performance and behaviour. + +For now, we will show an example of setting these parameters to help provide a feel for these changes. +Note that the :class:`~isaaclab.sim.NewtonCfg` replaces the :class:`~isaaclab.sim.PhysxCfg` and is used to set everything related to the physical simulation parameters except for the ``dt``: + +.. code-block:: python + + from isaaclab.sim._impl.newton_manager_cfg import NewtonCfg + from isaaclab.sim._impl.solvers_cfg import MJWarpSolverCfg + + solver_cfg = MJWarpSolverCfg( + nefc_per_env=35, + ls_iterations=10, + cone="pyramidal", + ls_parallel=True, + impratio=1, + ) + newton_cfg = NewtonCfg( + solver_cfg=solver_cfg, + num_substeps=1, + debug_mode=False, + ) + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation, newton_cfg=newton_cfg) + + +Here is a very brief explanation of some of the key parameters above: + +* ``nefc_per_env``: This is the size of the buffer constraints we want MuJoCo warp to + pre-allocate for a given environment. A large value will slow down the simulation, + while a too small value may lead to some contacts being missed. + +* ``ls_iterations``: The number of line searches performed by the MuJoCo Warp solver. + Line searches are used to find an optimal step size, and for each solver step, + at most ``ls_iterations`` line searches will be performed. Keeping this number low + is important for performance. This number is also an upper bound when + ``ls_parallel`` is not set. + +* ``cone``: This parameter provides a choice between pyramidal and elliptic + approximations for the friction cone used in contact handling. Please see the + MuJoCo documentation for additional information on contact: + https://mujoco.readthedocs.io/en/stable/computation/index.html#contact + +* ``ls_parallel``: This switches line searches from iterative to parallel execution. + Enabling ``ls_parallel`` provides a performance boost, but at the cost of some + simulation stability. To ensure good simulation behaviour when enabled, a higher + ``ls_iterations`` setting is required. Usually an increase of approximately 50% is + best over the ``ls_iterations`` setting when ``ls_parallel`` is disabled. + +* ``impratio``: This is the frictional-to-normal constraint impedance ratio that + enables finer-grained control of the significance of the tangential forces + compared to the normal forces. Larger values signify more emphasis on harder + frictional constraints to avoid slip. More on how to tune this parameter (and + cone) can be found in the MuJoCo documentation here: + https://mujoco.readthedocs.io/en/stable/XMLreference.html#option-impratio + +* ``num_substeps``: The number of substeps to perform when running the simulation. + Setting this to a number larger than one allows to decimate the simulation + without requiring Isaac Lab to process data between two substeps. This can be + of value when using implicit actuators, for example. + + +A more detailed transition guide covering the full set of available parameters and describing tuning approaches will follow in an upcoming release. diff --git a/docs/source/experimental-features/newton-physics-integration/training-environments.rst b/docs/source/experimental-features/newton-physics-integration/training-environments.rst new file mode 100644 index 000000000000..5e25564a1360 --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/training-environments.rst @@ -0,0 +1,89 @@ +Training Environments +====================== + +To run training, we follow the standard Isaac Lab workflow. If you are new to Isaac Lab, we recommend that you review the `Quickstart Guide here `_. + +The currently supported tasks are as follows: + +* Isaac-Cartpole-Direct-v0 +* Isaac-Cartpole-v0 +* Isaac-Cartpole-RGB-Camera-Direct-v0 +* Isaac-Cartpole-Depth-Camera-Direct-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-Reach-Franka-v0 +* Isaac-Reach-UR10-v0 +* Isaac-Repose-Cube-Allegro-Direct-v0 + +New experimental warp-based enviromnets: + +* Isaac-Cartpole-Direct-Warp-v0 +* Isaac-Ant-Direct-Warp-v0 +* Isaac-Humanoid-Direct-Warp-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. + +.. code-block:: shell + + ./isaaclab.sh -p scripts/environments/zero_agent.py --task TASK_NAME --num_envs NUM_ENVS + +For cartpole with 128 instances it would look like this: + +.. code-block:: shell + + ./isaaclab.sh -p scripts/environments/zero_agent.py --task Isaac-Cartpole-Direct-v0 --num_envs 128 + +To run the same environment with random actions we can use a different script: + +.. code-block:: shell + + ./isaaclab.sh -p scripts/environments/random_agent.py --task Isaac-Cartpole-Direct-v0 --num_envs 128 + +To train the environment we provide hooks to different rl frameworks. See the `Reinforcement Learning Scripts documentation `_ for more information. + +Here are some examples on how to run training on several different RL frameworks. Note that we are explicitly setting the number of environments to +4096 to benefit more from GPU parallelization. + +By default, environments will train in headless mode. If visualization is required, use ``--visualizer`` and specify the desired visualizer. +Available options are ``newton``, ``rerun``, and ``omniverse`` (requires Isaac Sim installation). Note, multiple visualizers can be selected and launched. + +.. code-block:: shell + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-Direct-v0 --num_envs 4096 + +.. code-block:: shell + + ./isaaclab.sh -p scripts/reinforcement_learning/skrl/train.py --task Isaac-Cartpole-Direct-v0 --num_envs 4096 + +.. code-block:: shell + + ./isaaclab.sh -p scripts/reinforcement_learning/rl_games/train.py --task Isaac-Cartpole-Direct-v0 --num_envs 4096 + +Once a policy is trained we can visualize it by using the play scripts. But first, we need to find the checkpoint of the trained policy. Typically, these are stored under: +``logs/NAME_OF_RL_FRAMEWORK/TASK_NAME/DATE``. + +For instance with our rsl_rl example it could look like this: +``logs/rsl_rl/cartpole_direct/2025-08-21_15-45-30/model_299.pt`` + +To then run this policy we can use the following command, note that we reduced the number of environments and added the ``--visualizer newton`` option so that we can see our policy in action! + +.. code-block:: shell + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/play.py --task Isaac-Cartpole-Direct-v0 --num_envs 128 --visualizer newton --checkpoint logs/rsl_rl/cartpole_direct/2025-08-21_15-45-30/model_299.pt + +The same approach applies to all other frameworks. + +Note that not all environments are supported in all frameworks. For example, several of the locomotion environments are only supported in the rsl_rl framework. diff --git a/docs/source/experimental-features/newton-physics-integration/visualization.rst b/docs/source/experimental-features/newton-physics-integration/visualization.rst new file mode 100644 index 000000000000..f54435733934 --- /dev/null +++ b/docs/source/experimental-features/newton-physics-integration/visualization.rst @@ -0,0 +1,310 @@ +Visualization +============= + +.. currentmodule:: isaaclab + +Isaac Lab offers several lightweight visualizers for real-time simulation inspection and debugging. Unlike renderers that process sensor data, visualizers are meant for fast, interactive feedback. + +You can use any visualizer regardless of your chosen physics engine or rendering backend. + + +Overview +-------- + +Isaac Lab supports three visualizer backends, each optimized for different use cases: + +.. list-table:: Visualizer Comparison + :widths: 15 35 50 + :header-rows: 1 + + * - Visualizer + - Best For + - Key Features + * - **Omniverse** + - High-fidelity, Isaac Sim integration + - USD, visual markers, live plots + * - **Newton** + - Fast iteration + - Low overhead, visual markers + * - **Rerun** + - Remote viewing, replay + - Webviewer, time scrubbing, recording export + + +*The following visualizers are shown training the Isaac-Velocity-Flat-Anymal-D-v0 environment.* + +.. figure:: ../../_static/visualizers/ov_viz.jpg + :width: 100% + :alt: Omniverse Visualizer + + Omniverse Visualizer + +.. figure:: ../../_static/visualizers/newton_viz.jpg + :width: 100% + :alt: Newton Visualizer + + Newton Visualizer + +.. figure:: ../../_static/visualizers/rerun_viz.jpg + :width: 100% + :alt: Rerun Visualizer + + Rerun Visualizer + + +Quick Start +----------- + +Launch visualizers from the command line with ``--visualizer``: + +.. code-block:: bash + + # Launch all visualizers + python scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-v0 --visualizer omniverse newton rerun + + # Launch just newton visualizer + python scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-v0 --visualizer newton + + +If ``--headless`` is given, no visualizers will be launched. + +.. note:: + + The ``--headless`` argument may be deprecated in future versions to avoid confusion with the ``--visualizer`` + argument. For now, ``--headless`` takes precedence and disables all visualizers. + + +Configuration +~~~~~~~~~~~~~ + +Launching visualizers with the command line will use default visualizer configurations. Default configs can be found and edited in ``source/isaaclab/isaaclab/visualizers``. + +You can also configure custom visualizers in the code by defining new ``VisualizerCfg`` instances for the ``SimulationCfg``, for example: + +.. code-block:: python + + from isaaclab.sim import SimulationCfg + from isaaclab.visualizers import NewtonVisualizerCfg, OVVisualizerCfg, RerunVisualizerCfg + + sim_cfg = SimulationCfg( + visualizer_cfgs=[ + OVVisualizerCfg( + viewport_name="Visualizer Viewport", + create_viewport=True, + dock_position="SAME", + window_width=1280, + window_height=720, + camera_position=(0.0, 0.0, 20.0), # high top down view + camera_target=(0.0, 0.0, 0.0), + ), + NewtonVisualizerCfg( + camera_position=(5.0, 5.0, 5.0), # closer quarter view + camera_target=(0.0, 0.0, 0.0), + show_joints=True, + ), + RerunVisualizerCfg( + keep_historical_data=True, + keep_scalar_history=True, + record_to_rrd="my_training.rrd", + ), + ] + ) + + +Visualizer Backends +------------------- + +Omniverse Visualizer +~~~~~~~~~~~~~~~~~~~~ + +**Main Features:** + +- Native USD stage integration +- Visualization markers for debugging (arrows, frames, points, etc.) +- Live plots for monitoring training metrics +- Full Isaac Sim rendering capabilities and tooling + +**Core Configuration:** + +.. code-block:: python + + from isaaclab.visualizers import OVVisualizerCfg + + visualizer_cfg = OVVisualizerCfg( + # Viewport settings + viewport_name="Visualizer Viewport", # Viewport window name + create_viewport=True, # Create new viewport vs. use existing + dock_position="SAME", # Docking: 'LEFT', 'RIGHT', 'BOTTOM', 'SAME' + window_width=1280, # Viewport width in pixels + window_height=720, # Viewport height in pixels + + # Camera settings + camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + camera_target=(0.0, 0.0, 0.0), # Camera look-at target + + # Feature toggles + enable_markers=True, # Enable visualization markers + enable_live_plots=True, # Enable live plots (auto-expands frames) + ) + + +Newton Visualizer +~~~~~~~~~~~~~~~~~~~~~~~~~ + +**Main Features:** + +- Lightweight OpenGL rendering with low overhead +- Visualization markers (joints, contacts, springs, COM) +- Training and rendering pause controls +- Adjustable update frequency for performance tuning +- Some customizable rendering options (shadows, sky, wireframe) + + +**Interactive Controls:** + +.. list-table:: + :widths: 30 70 + :header-rows: 1 + + * - Key/Input + - Action + * - **W, A, S, D** or **Arrow Keys** + - Forward / Left / Back / Right + * - **Q, E** + - Down / Up + * - **Left Click + Drag** + - Look around + * - **Mouse Scroll** + - Zoom in/out + * - **Space** + - Pause/resume rendering (physics continues) + * - **H** + - Toggle UI sidebar + * - **ESC** + - Exit viewer + +**Core Configuration:** + +.. code-block:: python + + from isaaclab.visualizers import NewtonVisualizerCfg + + visualizer_cfg = NewtonVisualizerCfg( + # Window settings + window_width=1920, # Window width in pixels + window_height=1080, # Window height in pixels + + # Camera settings + camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + camera_target=(0.0, 0.0, 0.0), # Camera look-at target + + # Performance tuning + update_frequency=1, # Update every N frames (1=every frame) + + # Physics debug visualization + show_joints=False, # Show joint visualizations + show_contacts=False, # Show contact points and normals + show_springs=False, # Show spring constraints + show_com=False, # Show center of mass markers + + # Rendering options + enable_shadows=True, # Enable shadow rendering + enable_sky=True, # Enable sky rendering + enable_wireframe=False, # Enable wireframe mode + + # Color customization + background_color=(0.53, 0.81, 0.92), # Sky/background color (RGB [0,1]) + ground_color=(0.18, 0.20, 0.25), # Ground plane color (RGB [0,1]) + light_color=(1.0, 1.0, 1.0), # Directional light color (RGB [0,1]) + ) + + +Rerun Visualizer +~~~~~~~~~~~~~~~~ + +**Main Features:** + +- Web viewer interface accessible from local or remote browser +- Metadata logging and filtering +- Recording to .rrd files for offline replay (.rrd files can be opened with ctrl+O from the web viewer) +- Timeline scrubbing and playback controls of recordings + +**Core Configuration:** + +.. code-block:: python + + from isaaclab.visualizers import RerunVisualizerCfg + + visualizer_cfg = RerunVisualizerCfg( + # Server settings + app_id="isaaclab-simulation", # Application identifier for viewer + web_port=9090, # Port for local web viewer (launched in browser) + + # Camera settings + camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + camera_target=(0.0, 0.0, 0.0), # Camera look-at target + + # History settings + keep_historical_data=False, # Keep transforms for time scrubbing + keep_scalar_history=False, # Keep scalar/plot history + + # Recording + record_to_rrd="recording.rrd", # Path to save .rrd file (None = no recording) + ) + + +Performance Note +---------------- + +To reduce overhead when visualizing large-scale environments, consider: + +- Using Newton instead of Omniverse or Rerun +- Reducing window sizes +- Higher update frequencies +- Pausing visualizers while they are not being used + + +Limitations +----------- + +**Rerun Visualizer Performance** + +The Rerun web-based visualizer may experience performance issues or crashes when visualizing large-scale +environments. For large-scale simulations, the Newton visualizer is recommended. Alternatively, to reduce load, +the num of environments can be overwritten and decreased using ``--num_envs``: + +.. code-block:: bash + + python scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-v0 --visualizer rerun --num_envs 512 + + +.. note:: + + A future feature will support visualizing only a subset of environments, which will improve visualization performance + and reduce resource usage while maintaining full-scale training in the background. + + +**Rerun Visualizer FPS Control** + +The FPS control in the Rerun visualizer UI may not affect the visualization frame rate in all configurations. + + +**Newton Visualizer Contact and Center of Mass Markers** + +Contact and center of mass markers are not yet supported in the Newton visualizer. This will be addressed in a future release. + + +**Newton Visualizer CUDA/OpenGL Interoperability Warnings** + +On some system configurations, the Newton visualizer may display warnings about CUDA/OpenGL interoperability: + +.. code-block:: text + + Warning: Could not get MSAA config, falling back to non-AA. + Warp CUDA error 999: unknown error (in function wp_cuda_graphics_register_gl_buffer) + Warp UserWarning: Could not register GL buffer since CUDA/OpenGL interoperability + is not available. Falling back to copy operations between the Warp array and the + OpenGL buffer. + +The visualizer will still function correctly but may experience reduced performance due to falling back to +CPU copy operations instead of direct GPU memory sharing. diff --git a/docs/source/features/multi_gpu.rst b/docs/source/features/multi_gpu.rst index 6d60c1a2a8f4..2537e5eff25b 100644 --- a/docs/source/features/multi_gpu.rst +++ b/docs/source/features/multi_gpu.rst @@ -4,7 +4,7 @@ Multi-GPU and Multi-Node Training .. currentmodule:: isaaclab Isaac Lab supports multi-GPU and multi-node reinforcement learning. Currently, this feature is only -available for RL-Games and skrl libraries workflows. We are working on extending this feature to +available for RL-Games, RSL-RL and skrl libraries workflows. We are working on extending this feature to other workflows. .. attention:: @@ -16,19 +16,57 @@ other workflows. Multi-GPU Training ------------------ -For complex reinforcement learning environments, it may be desirable to scale up training across multiple GPUs. -This is possible in Isaac Lab through the use of the -`PyTorch distributed `_ framework or the -`JAX distributed `_ module respectively. +Isaac Lab supports the following multi-GPU training frameworks: -In PyTorch, the :meth:`torch.distributed` API is used to launch multiple processes of training, where the number of -processes must be equal to or less than the number of GPUs available. Each process runs on -a dedicated GPU and launches its own instance of Isaac Sim and the Isaac Lab environment. -Each process collects its own rollouts during the training process and has its own copy of the policy -network. During training, gradients are aggregated across the processes and broadcasted back to the process -at the end of the epoch. +* `Torchrun `_ through `PyTorch distributed `_ +* `JAX distributed `_ -In JAX, since the ML framework doesn't automatically start multiple processes from a single program invocation, +Pytorch Torchrun Implementation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +We are using `Pytorch Torchrun `_ to manage multi-GPU +training. Torchrun manages the distributed training by: + +* **Process Management**: Launching one process per GPU, where each process is assigned to a specific GPU. +* **Script Execution**: Running the same training script (e.g., RL Games trainer) on each process. +* **Environment Instances**: Each process creates its own instance of the Isaac Lab environment. +* **Gradient Synchronization**: Aggregating gradients across all processes and broadcasting the synchronized + gradients back to each process after each training step. + +.. tip:: + Check out this `3 minute youtube video from PyTorch `_ + to understand how Torchrun works. + +The key components in this setup are: + +* **Torchrun**: Handles process spawning, communication, and gradient synchronization. +* **RL Library**: The reinforcement learning library that runs the actual training algorithm. +* **Isaac Lab**: Provides the simulation environment that each process instantiates independently. + +Under the hood, Torchrun uses the `DistributedDataParallel `_ +module to manage the distributed training. When training with multiple GPUs using Torchrun, the following happens: + +* Each GPU runs an independent process +* Each process executes the full training script +* Each process maintains its own: + + * Isaac Lab environment instance (with *n* parallel environments) + * Policy network copy + * Experience buffer for rollout collection + +* All processes synchronize only for gradient updates + +For a deeper dive into how Torchrun works, checkout +`PyTorch Docs: DistributedDataParallel - Internal Design `_. + +Jax Implementation +^^^^^^^^^^^^^^^^^^ + +.. tip:: + JAX is only supported with the skrl library. + +With JAX, we are using `skrl.utils.distributed.jax `_ +Since the ML framework doesn't automatically start multiple processes from a single program invocation, the skrl library provides a module to start them. .. image:: ../_static/multi-gpu-rl/a3c-light.svg @@ -45,6 +83,9 @@ the skrl library provides a module to start them. | +Running Multi-GPU Training +^^^^^^^^^^^^^^^^^^^^^^^^^^ + To train with multiple GPUs, use the following command, where ``--nproc_per_node`` represents the number of available GPUs: .. tab-set:: @@ -57,6 +98,13 @@ To train with multiple GPUs, use the following command, where ``--nproc_per_node python -m torch.distributed.run --nnodes=1 --nproc_per_node=2 scripts/reinforcement_learning/rl_games/train.py --task=Isaac-Cartpole-v0 --headless --distributed + .. tab-item:: rsl_rl + :sync: rsl_rl + + .. code-block:: shell + + python -m torch.distributed.run --nnodes=1 --nproc_per_node=2 scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Cartpole-v0 --headless --distributed + .. tab-item:: skrl :sync: skrl @@ -93,7 +141,14 @@ For the master node, use the following command, where ``--nproc_per_node`` repre .. code-block:: shell - python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=0 --rdzv_id=123 --rdzv_backend=c10d --rdzv_endpoint=localhost:5555 scripts/reinforcement_learning/rl_games/train.py --task=Isaac-Cartpole-v0 --headless --distributed + python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=0 --master_addr= --master_port=5555 scripts/reinforcement_learning/rl_games/train.py --task=Isaac-Cartpole-v0 --headless --distributed + + .. tab-item:: rsl_rl + :sync: rsl_rl + + .. code-block:: shell + + python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=0 --master_addr= --master_port=5555 scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Cartpole-v0 --headless --distributed .. tab-item:: skrl :sync: skrl @@ -105,7 +160,7 @@ For the master node, use the following command, where ``--nproc_per_node`` repre .. code-block:: shell - python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=0 --rdzv_id=123 --rdzv_backend=c10d --rdzv_endpoint=localhost:5555 scripts/reinforcement_learning/skrl/train.py --task=Isaac-Cartpole-v0 --headless --distributed + python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=0 --master_addr= --master_port=5555 scripts/reinforcement_learning/skrl/train.py --task=Isaac-Cartpole-v0 --headless --distributed .. tab-item:: JAX :sync: jax @@ -126,7 +181,14 @@ For non-master nodes, use the following command, replacing ``--node_rank`` with .. code-block:: shell - python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=1 --rdzv_id=123 --rdzv_backend=c10d --rdzv_endpoint=ip_of_master_machine:5555 scripts/reinforcement_learning/rl_games/train.py --task=Isaac-Cartpole-v0 --headless --distributed + python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=1 --master_addr= --master_port=5555 scripts/reinforcement_learning/rl_games/train.py --task=Isaac-Cartpole-v0 --headless --distributed + + .. tab-item:: rsl_rl + :sync: rsl_rl + + .. code-block:: shell + + python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=1 --master_addr= --master_port=5555 scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Cartpole-v0 --headless --distributed .. tab-item:: skrl :sync: skrl @@ -138,7 +200,7 @@ For non-master nodes, use the following command, replacing ``--node_rank`` with .. code-block:: shell - python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=1 --rdzv_id=123 --rdzv_backend=c10d --rdzv_endpoint=ip_of_master_machine:5555 scripts/reinforcement_learning/skrl/train.py --task=Isaac-Cartpole-v0 --headless --distributed + python -m torch.distributed.run --nproc_per_node=2 --nnodes=2 --node_rank=1 --master_addr= --master_port=5555 scripts/reinforcement_learning/skrl/train.py --task=Isaac-Cartpole-v0 --headless --distributed .. tab-item:: JAX :sync: jax diff --git a/docs/source/features/population_based_training.rst b/docs/source/features/population_based_training.rst new file mode 100644 index 000000000000..d88b8195bc7c --- /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 1f18a804ed05..0edf935e8389 100644 --- a/docs/source/features/ray.rst +++ b/docs/source/features/ray.rst @@ -16,9 +16,14 @@ the general workflow is the same. This functionality is experimental, and has been tested only on Linux. -.. contents:: Table of Contents - :depth: 3 - :local: +.. 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 -------- @@ -46,22 +51,28 @@ specifying the ``--num_workers`` argument for resource-wrapped jobs, or ``--num_ for tuning jobs, which is especially critical for parallel aggregate job processing on local/virtual multi-GPU machines. Tuning jobs assume homogeneous node resource composition for nodes with GPUs. -The two following files contain the core functionality of the Ray integration. +The three following files contain the core functionality of the Ray integration. .. dropdown:: scripts/reinforcement_learning/ray/wrap_resources.py :icon: code .. literalinclude:: ../../../scripts/reinforcement_learning/ray/wrap_resources.py :language: python - :emphasize-lines: 14-66 + :emphasize-lines: 10-63 .. dropdown:: scripts/reinforcement_learning/ray/tuner.py :icon: code .. literalinclude:: ../../../scripts/reinforcement_learning/ray/tuner.py :language: python - :emphasize-lines: 18-53 + :emphasize-lines: 18-59 +.. dropdown:: scripts/reinforcement_learning/ray/task_runner.py + :icon: code + + .. literalinclude:: ../../../scripts/reinforcement_learning/ray/task_runner.py + :language: python + :emphasize-lines: 13-105 The following script can be used to submit aggregate jobs to one or more Ray cluster(s), which can be used for @@ -73,7 +84,7 @@ resource requirements. .. literalinclude:: ../../../scripts/reinforcement_learning/ray/submit_job.py :language: python - :emphasize-lines: 12-53 + :emphasize-lines: 13-61 The following script can be used to extract KubeRay cluster information for aggregate job submission. @@ -91,7 +102,7 @@ The following script can be used to easily create clusters on Google GKE. .. literalinclude:: ../../../scripts/reinforcement_learning/ray/launch.py :language: python - :emphasize-lines: 16-37 + :emphasize-lines: 15-36 Docker-based Local Quickstart ----------------------------- @@ -149,7 +160,26 @@ Submitting resource-wrapped individual jobs instead of automatic tuning runs is .. literalinclude:: ../../../scripts/reinforcement_learning/ray/wrap_resources.py :language: python - :emphasize-lines: 14-66 + :emphasize-lines: 10-63 + +The ``task_runner.py`` dispatches Python tasks to a Ray cluster via a single declarative YAML file. This approach allows users to specify additional pip packages and Python modules for each run. Fine-grained resource allocation is supported, with explicit control over the number of CPUs, GPUs, and memory assigned to each task. The runner also offers advanced scheduling capabilities: tasks can be restricted to specific nodes by hostname or node ID, and supports two launch modes: tasks can be executed independently as resources become available, or grouped into a simultaneous batchโ€”ideal for multi-node training jobsโ€”which ensures that all tasks launch together only when sufficient resources are available across the cluster. + +.. dropdown:: scripts/reinforcement_learning/ray/task_runner.py + :icon: code + + .. literalinclude:: ../../../scripts/reinforcement_learning/ray/task_runner.py + :language: python + :emphasize-lines: 13-105 + +To use this script, run a command similar to the following (replace ``tasks.yaml`` with your actual configuration file): + +.. code-block:: bash + + python3 scripts/reinforcement_learning/ray/submit_job.py --aggregate_jobs task_runner.py --task_cfg tasks.yaml + +For detailed instructions on how to write your ``tasks.yaml`` file, please refer to the comments in ``task_runner.py``. + +**Tip:** Place the ``tasks.yaml`` file in the ``scripts/reinforcement_learning/ray`` directory so that it is included when the ``working_dir`` is uploaded. You can then reference it using a relative path in the command. Transferring files from the running container can be done as follows. @@ -290,7 +320,7 @@ where instructions are included in the following creation file. .. literalinclude:: ../../../scripts/reinforcement_learning/ray/launch.py :language: python - :emphasize-lines: 15-37 + :emphasize-lines: 15-36 For other cloud services, the ``kuberay.yaml.ninja`` will be similar to that of Google's. @@ -347,7 +377,7 @@ Dispatching Steps Shared Between KubeRay and Pure Ray Part II .. literalinclude:: ../../../scripts/reinforcement_learning/ray/submit_job.py :language: python - :emphasize-lines: 12-53 + :emphasize-lines: 13-61 3.) For tuning jobs, specify the tuning job / hyperparameter sweep as a :class:`JobCfg` . The included :class:`JobCfg` only supports the ``rl_games`` workflow due to differences in diff --git a/docs/source/features/reproducibility.rst b/docs/source/features/reproducibility.rst index 9895a375d5d4..631e138376c9 100644 --- a/docs/source/features/reproducibility.rst +++ b/docs/source/features/reproducibility.rst @@ -10,7 +10,7 @@ or soft bodies. For more information, please refer to the `PhysX Determinism doc Based on above, Isaac Lab provides a deterministic simulation that ensures consistent simulation results across different runs. This is achieved by using the same random seed for the simulation environment and the physics engine. At construction of the environment, the random seed -is set to a fixed value using the :meth:`~isaacsim.core.utils.torch.set_seed` method. This method sets the +is set to a fixed value using the :meth:`~isaaclab.utils.seed.configure_seed` method. This method sets the random seed for both the CPU and GPU globally across different libraries, including PyTorch and NumPy. diff --git a/docs/source/how-to/add_own_library.rst b/docs/source/how-to/add_own_library.rst index d74319ebd8ad..8a0347d65979 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 ================================ @@ -5,7 +7,7 @@ Isaac Lab comes pre-integrated with a number of libraries (such as RSL-RL, RL-Ga However, you may want to integrate your own library with Isaac Lab or use a different version of the libraries than the one installed by Isaac Lab. This is possible as long as the library is available as Python package that supports the Python version used by the underlying simulator. For instance, if you are using Isaac Sim 4.0.0 onwards, you need -to ensure that the library is available for Python 3.10. +to ensure that the library is available for Python 3.11. Using a different version of a library -------------------------------------- @@ -39,7 +41,7 @@ command: .. code-block:: bash - ./isaaclab.sh -p -m pip show rsl-rl + ./isaaclab.sh -p -m pip show rsl-rl-lib This should now show the location of the ``rsl-rl`` library as the directory where you cloned the library. For instance, if you cloned the library to ``/home/user/git/rsl_rl``, the output of the above command should be: @@ -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.0.2 + 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 @@ -63,12 +65,12 @@ Integrating a new library Adding a new library to Isaac Lab is similar to using a different version of a library. You can install the library in your Python environment and use it in your experiments. However, if you want to integrate the library with -Isaac Lab, you can will first need to make a wrapper for the library, as explained in +Isaac Lab, you will first need to make a wrapper for the library, as explained in :ref:`how-to-env-wrappers`. The following steps can be followed to integrate a new library with Isaac Lab: -1. Add your library as an extra-dependency in the ``setup.py`` for the extension ``isaaclab_tasks``. +1. Add your library as an extra-dependency in the ``setup.py`` for the extension ``isaaclab_rl``. This will ensure that the library is installed when you install Isaac Lab or it will complain if the library is not installed or available. 2. Install your library in the Python environment used by Isaac Lab. You can do this by following the steps mentioned @@ -86,6 +88,15 @@ works as expected and can guide users on how to use the wrapper. * Add some tests to ensure that the wrapper works as expected and remains compatible with the library. These tests can be added to the ``source/isaaclab_rl/test`` directory. * Add some documentation for the wrapper. You can add the API documentation to the - ``docs/source/api/lab_tasks/isaaclab_rl.rst`` file. + :ref:`API documentation` for the ``isaaclab_rl`` module. + + +Configuring an RL Agent +----------------------- + +Once you have integrated a new library with Isaac Lab, you can configure the example environment to use the new library. +You can check the :ref:`tutorial-configure-rl-training` for an example of how to configure the training process to use a +different library. + .. _rsl-rl: https://github.com/leggedrobotics/rsl_rl diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst new file mode 100644 index 000000000000..e13b76305dcb --- /dev/null +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -0,0 +1,1132 @@ +.. _cloudxr-teleoperation: + +Setting up CloudXR Teleoperation +================================ + +.. currentmodule:: isaaclab + +`NVIDIA CloudXR`_ enables seamless, high-fidelity immersive streaming to extended reality (XR) +devices over any network. + +Isaac Lab developers can use CloudXR with Isaac Lab to build teleoperation workflows that require +immersive XR rendering for increased spatial acuity and/or hand tracking for teleoperation of +dextrous robots. + +In these workflows, Isaac Lab renders and submits stereo views of the robot simulation to CloudXR, +which then encodes and streams the rendered views to a compatible XR device in realtime using a +low-latency, GPU-accelerated pipeline. Control inputs such as hand tracking data are sent from the +XR device back to Isaac Lab through CloudXR, where they can be used to control the robot. + +This guide explains how to use CloudXR and `Apple Vision Pro`_ for immersive streaming and +teleoperation in Isaac Lab. + +.. note:: + + See :ref:`manus-vive-handtracking` for more information on supported hand-tracking peripherals. + +.. note:: + + **Meta Quest 3 and Pico 4 Ultra Support (Early Access)** + + Meta Quest 3 and Pico 4 Ultra are now supported via the `CloudXR Early Access program`_. + Join the program by mentioning isaac use cases. Once approved, you'll receive email to set up NGC, + then download `CloudXR.js with Isaac Teleop samples`_ and follow its guide. + Pico 4 Ultra must use HTTPS mode (see NGC documentation for details). General availability + will be provided in a future version of Isaac Lab. + +.. _`CloudXR Early Access program`: https://developer.nvidia.com/cloudxr-sdk-early-access-program/join +.. _`CloudXR.js with Isaac Teleop samples`: https://catalog.ngc.nvidia.com/orgs/nvidia/resources/cloudxr-js-early-access?version=6.0.0-beta + +Overview +-------- + +Using CloudXR with Isaac Lab involves the following components: + +* **Isaac Lab** is used to simulate the robot environment and apply control data received from the + teleoperator. + +* The **NVIDIA CloudXR Runtime** runs on the Isaac Lab workstation in a Docker container, and streams + the virtual simulation from Isaac Lab to compatible XR devices. + +* The **Isaac XR Teleop Sample Client** is a sample app for Apple Vision Pro which enables + immersive streaming and teleoperation of an Isaac Lab simulation using CloudXR. + +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`, + :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`. + +* :ref:`control-robot-with-xr`, including the :ref:`openxr-device-architecture`, + :ref:`control-robot-with-xr-retargeters`, and how to implement :ref:`control-robot-with-xr-callbacks`. + +As well as :ref:`xr-known-issues`. + + +System Requirements +------------------- + +Prior to using CloudXR with Isaac Lab, please review the following system requirements: + + * 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. + + * Apple Vision Pro + + * 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 + + * Apple Silicon based Mac (for building the Isaac XR Teleop Sample Client App for Apple Vision Pro + with Xcode) + + * macOS Sequoia 15.6 or later + * Xcode 26.0 + + * Wifi 6 capable router + + * A strong wireless connection is essential for a high-quality streaming experience. Refer to the + requirements of `Omniverse Spatial Streaming`_ for more details. + * We recommend using a dedicated router, as concurrent usage will degrade quality + * The Apple Vision Pro and Isaac Lab workstation must be IP-reachable from one another (note: + 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 + + +.. _run-isaac-lab-with-the-cloudxr-runtime: + +Run Isaac Lab with the CloudXR Runtime +-------------------------------------- + +The CloudXR Runtime runs in a Docker container on your Isaac Lab workstation, and is responsible for +streaming the Isaac Lab simulation to a compatible XR device. + +Ensure that `Docker`_, `Docker Compose`_, and the `NVIDIA Container Toolkit`_ are installed on your +Isaac Lab workstation as described in the Isaac Lab :ref:`deployment-docker`. + +Also ensure that your firewall allows connections to the ports used by CloudXR by running: + +.. code:: bash + + sudo ufw allow 47998:48000,48005,48008,48012/udp + sudo ufw allow 48010/tcp + +There are two options to run the CloudXR Runtime Docker container: + +.. dropdown:: Option 1 (Recommended): Use Docker Compose to run the Isaac Lab and CloudXR Runtime + containers together + :open: + + On your Isaac Lab workstation: + + #. From the root of the Isaac Lab repository, start the Isaac Lab and CloudXR Runtime containers + using the Isaac Lab ``container.py`` script + + .. code:: bash + + ./docker/container.py start \ + --files docker-compose.cloudxr-runtime.patch.yaml \ + --env-file .env.cloudxr-runtime + + If prompted, elect to activate X11 forwarding, which is necessary to see the Isaac Sim UI. + + .. note:: + + The ``container.py`` script is a thin wrapper around Docker Compose. The additional + ``--files`` and ``--env-file`` arguments augment the base Docker Compose configuration to + additionally run the CloudXR Runtime + + For more details on ``container.py`` and running Isaac Lab with Docker Compose, see the + :ref:`deployment-docker`. + + #. Enter the Isaac Lab base container with: + + .. code:: bash + + ./docker/container.py enter base + + From within the Isaac Lab base container, you can run Isaac Lab scripts that use XR. + + #. Run an example teleop task with: + + .. code:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-PickPlace-GR1T2-Abs-v0 \ + --teleop_device handtracking \ + --enable_pinocchio + + #. You'll want to leave the container running for the next steps. But once you are finished, you can + stop the containers with: + + .. code:: bash + + ./docker/container.py stop \ + --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. + However, this method requires manually specifying a shared directory for communication between + the Isaac Lab instance and the CloudXR Runtime. + + On your Isaac Lab workstation: + + #. From the root of the Isaac Lab repository, create a local folder for temporary cache files: + + .. code:: bash + + mkdir -p $(pwd)/openxr + + #. Start the CloudXR Runtime, mounting the directory created above to the ``/openxr`` directory in + the container: + + .. code:: bash + + docker run -it --rm --name cloudxr-runtime \ + --user $(id -u):$(id -g) \ + --gpus=all \ + -e "ACCEPT_EULA=Y" \ + --mount type=bind,src=$(pwd)/openxr,dst=/openxr \ + -p 48010:48010 \ + -p 47998:47998/udp \ + -p 47999:47999/udp \ + -p 48000:48000/udp \ + -p 48005:48005/udp \ + -p 48008:48008/udp \ + -p 48012:48012/udp \ + 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: + + .. code:: bash + + export XDG_RUNTIME_DIR=$(pwd)/openxr/run + export XR_RUNTIME_JSON=$(pwd)/openxr/share/openxr/1/openxr_cloudxr.json + + You can now run Isaac Lab scripts that use XR. + + #. Run an example teleop task with: + + .. code:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-PickPlace-GR1T2-Abs-v0 \ + --teleop_device handtracking \ + --enable_pinocchio + +With Isaac Lab and the CloudXR Runtime running: + +#. 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 +active". + +.. figure:: ../_static/setup/cloudxr_viewport.jpg + :align: center + :figwidth: 100% + :alt: Isaac Lab viewport rendering two eyes + +Isaac Lab is now ready to receive connections from a CloudXR client. The next sections will walk +you through building and connecting a CloudXR client. + +.. admonition:: Learn More about Teleoperation and Imitation Learning in Isaac Lab + + To learn more about the Isaac Lab teleoperation scripts, and how to build new teleoperation and + imitation learning workflows in Isaac Lab, see :ref:`teleoperation-imitation-learning`. + + +.. _use-apple-vision-pro: + +Use Apple Vision Pro for Teleoperation +-------------------------------------- + +This section will walk you through building and installing the Isaac XR Teleop Sample Client for +Apple Vision Pro, connecting to Isaac Lab, and teleoperating a simulated robot. + + +.. _build-apple-vision-pro: + +Build and Install the Isaac XR Teleop Sample Client App for Apple Vision Pro +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +On your Mac: + +#. Clone the `Isaac XR Teleop Sample Client`_ GitHub repository: + + .. code-block:: bash + + git clone git@github.com:isaac-sim/isaac-xr-teleop-sample-client-apple.git + +#. Check out the App version that matches your Isaac Lab version: + + +-------------------+---------------------+ + | Isaac Lab Version | Client App Version | + +-------------------+---------------------+ + | 2.3 | v2.3.0 | + +-------------------+---------------------+ + | 2.2 | v2.2.0 | + +-------------------+---------------------+ + | 2.1 | v1.0.0 | + +-------------------+---------------------+ + + .. code-block:: bash + + git checkout + +#. Follow the README in the repository to build and install the app on your Apple Vision Pro. + + +.. _teleoperate-apple-vision-pro: + +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 + :ref:`run-isaac-lab-with-the-cloudxr-runtime`, including starting Isaac Lab with a script that + supports teleoperation. For example: + + .. code-block:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-PickPlace-GR1T2-Abs-v0 \ + --teleop_device handtracking \ + --enable_pinocchio + + .. note:: + Recall that the script above should either be run within the Isaac Lab Docker container + (Option 1, recommended), or with environment variables configured to a directory shared by a + running CloudXR Runtime Docker container (Option 2). + +#. Locate the Panel named **AR**. + +#. Click **Start AR** and ensure that the Viewport shows two eyes being rendered. + +Back on your Apple Vision Pro: + +#. Open the Isaac XR Teleop Sample Client. You should see a UI window: + + .. figure:: ../_static/setup/cloudxr_avp_connect_ui.jpg + :align: center + :figwidth: 50% + :alt: Isaac Sim UI: AR Panel + +#. Enter the IP address of your Isaac Lab workstation. + + .. note:: + The Apple Vision Pro and Isaac Lab machine must be IP-reachable from one another. + + We recommend using a dedicated Wifi 6 router for this process, as 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. + +#. Click **Connect**. + + The first time you attempt to connect, you may need to allow the application access to + permissions such as hand tracking and local network usage, and then connect again. + +#. After a brief period, you should see the Isaac Lab simulation rendered in the Apple Vision Pro, + as well as a set of controls for teleoperation. + + .. figure:: ../_static/setup/cloudxr_avp_teleop_ui.jpg + :align: center + :figwidth: 50% + :alt: Isaac Sim UI: AR Panel + +#. Click **Play** to begin teleoperating the simulated robot. The robot motion should now be + directed by your hand movements. + + You may repeatedly **Play**, **Stop**, and **Reset** the teleoperation session using the UI + controls. + + .. tip:: + For teleoperation tasks that require bimanual manipulation, visionOS accessibility features + can be used to control teleoperation without the use of hand gestures. For example, in order + to enable voice control of the UI: + + #. In **Settings** > **Accessibility** > **Voice Control**, Turn on **Voice Control** + + #. In **Settings** > **Accessibility** > **Voice Control** > **Commands** > **Basic + Navigation** > Turn on **** + + #. Now you can say "Play", "Stop", and "Reset" to control teleoperation while the app is + connected. + +#. Teleoperate the simulated robot by moving your hands. + + .. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/cloudxr_bimanual_teleop.gif + :align: center + :alt: Isaac Lab teleoperation of a bimanual dexterous robot with CloudXR + + .. note:: + + 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. + + .. note:: + When the inverse kinematics solver fails to find a valid solution, an error message will appear + in the XR device display. To recover from this state, click the **Reset** button to return + the robot to its original pose and continue teleoperation. + + .. figure:: ../_static/setup/cloudxr_avp_ik_error.jpg + :align: center + :figwidth: 80% + :alt: IK Error Message Display in XR Device + + + +#. When you are finished with the example, click **Disconnect** to disconnect from Isaac Lab. + +.. admonition:: Learn More about Teleoperation and Imitation Learning in Isaac Lab + + See :ref:`teleoperation-imitation-learning` to learn how to record teleoperated demonstrations + and build teleoperation and imitation learning workflows with Isaac Lab. + + +.. _manus-vive-handtracking: + +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 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 \ + --task Isaac-PickPlace-GR1T2-Abs-v0 \ + --teleop_device manusvive \ + --xr \ + --enable_pinocchio + +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 `_. + +For optimal performance, position the lighthouse above the hands, tilted slightly downward. +One lighthouse is sufficient if both hands are visible. +Ensure the lighthouse remains stable; a stand is recommended to prevent wobbling. + +.. 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 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. + +.. _develop-xr-isaac-lab: + +Develop for XR in Isaac Lab +--------------------------- + +This section will walk you through how to develop XR environments in Isaac Lab for building +teleoperation workflows. + + +.. _run-isaac-lab-with-xr: + +Run Isaac Lab with XR Extensions Enabled +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +In order to enable extensions necessary for XR, and to see the AR Panel in the UI, Isaac Lab must be +loaded with an XR experience file. This can be done automatically by passing the ``--xr`` flag to +any Isaac Lab script that uses :class:`app.AppLauncher`. + +For example: you can enable and use XR in any of the :ref:`tutorials` by invoking them with the +additional ``--xr`` flag. + + +.. _configure-scene-placement: + +Configure XR Scene Placement +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Placement of the robot simulation within the XR device's local coordinate frame can be achieved +using an XR anchor, and is configurable using the ``xr`` field (type :class:`openxr.XrCfg`) in the +environment configuration. + +Specifically: the pose specified by the ``anchor_pos`` and ``anchor_rot`` fields of the +:class:`openxr.XrCfg` will appear at the origin of the XR device's local coordinate frame, which +should be on the floor. + +.. note:: + + On Apple Vision Pro, the local coordinate frame can be reset to a point on the floor beneath the + user by holding the digital crown. + +For example: if a robot should appear at the position of the user, the ``anchor_pos`` and +``anchor_rot`` properties should be set to a pose on the floor directly beneath the robot. + +.. note:: + + The XR anchor configuration is applied in :class:`openxr.OpenXRDevice` by creating a prim at the + position of the anchor, and modifying the ``xr/profile/ar/anchorMode`` and + ``/xrstage/profile/ar/customAnchor`` settings. + + If you are running a script that does not use :class:`openxr.OpenXRDevice`, you will need to do + this explicitly. + + +.. _optimize-xr-performance: + +Optimize XR Performance +~~~~~~~~~~~~~~~~~~~~~~~ + +.. dropdown:: Configure the physics and render time step + :open: + + In order to provide a high-fidelity immersive experience, it is recommended to ensure that the + simulation render time step roughly matches the XR device display time step. + + It is also important to ensure that this time step can be simulated and rendered in real time. + + The Apple Vision Pro display runs at 90Hz, but many Isaac Lab simulations will not achieve 90Hz + performance when rendering stereo views for XR; so for best experience on Apple Vision Pro, we + suggest running with a simulation dt of 90Hz and a render interval of 2, meaning that the + simulation is rendered once for every two simulation steps, or at 45Hz, if performance allows. + + You can still set the simulation dt lower or higher depending on your requirements, but this may + result in the simulation appearing faster or slower when rendered in XR. + + Overriding the time step configuration for an environment can be done by modifying the + :class:`sim.SimulationCfg` in the environment's ``__post_init__`` function. For instance: + + .. code-block:: python + + @configclass + class XrTeleopEnvCfg(ManagerBasedRLEnvCfg): + + def __post_init__(self): + self.sim.dt = 1.0 / 90 + self.sim.render_interval = 2 + + Also note that by default the CloudXR Runtime attempts to dynamically adjust its pacing based on + how long Isaac Lab takes to render. If render times are highly variable, this can lead to the + simulation appearing to speed up or slow down when rendered in XR. If this is an issue, the + CloudXR Runtime can be configured to use a fixed time step by setting the environment variable + ``NV_PACER_FIXED_TIME_STEP_MS`` to an integer quantity when starting the CloudXR Runtime Docker + containere. + + +.. dropdown:: Try running physics on CPU + :open: + + It is currently recommended to try running Isaac Lab teleoperation scripts with the ``--device + cpu`` flag. This will cause Physics calculations to be done on the CPU, which may be reduce + latency when only a single environment is present in the simulation. + + +.. _control-robot-with-xr: + +Control the Robot with XR Device Inputs +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Isaac Lab provides a flexible architecture for using XR tracking data to control +simulated robots. This section explains the components of this architecture and how they work together. + +.. _openxr-device-architecture: + +OpenXR Device +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The :class:`isaaclab.devices.OpenXRDevice` is the core component that enables XR-based teleoperation in Isaac Lab. +This device interfaces with CloudXR to receive tracking data from the XR headset and transform it into robot control +commands. + +At its heart, XR teleoperation requires mapping (or "retargeting") user inputs, such as hand movements and poses, +into robot control signals. Isaac Lab makes this straightforward through its OpenXRDevice and Retargeter architecture. +The OpenXRDevice captures hand tracking data via Isaac Sim's OpenXR API, then passes this data through one or more +Retargeters that convert it into robot actions. + +The OpenXRDevice also integrates with the XR device's user interface when using CloudXR, allowing users to trigger +simulation events directly from their XR environment. + +.. _control-robot-with-xr-retargeters: + +Retargeting Architecture +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Retargeters are specialized components that convert raw tracking data into meaningful control signals +for robots. They implement the :class:`isaaclab.devices.RetargeterBase` interface and are passed to +the OpenXRDevice during initialization. + +Isaac Lab provides three main retargeters for hand tracking: + +.. dropdown:: Se3RelRetargeter (:class:`isaaclab.devices.openxr.retargeters.Se3RelRetargeter`) + + * Generates incremental robot commands from relative hand movements + * Best for precise manipulation tasks + +.. dropdown:: Se3AbsRetargeter (:class:`isaaclab.devices.openxr.retargeters.Se3AbsRetargeter`) + + * Maps hand position directly to robot end-effector position + * Enables 1:1 spatial control + +.. dropdown:: GripperRetargeter (:class:`isaaclab.devices.openxr.retargeters.GripperRetargeter`) + + * Controls gripper state based on thumb-index finger distance + * Used alongside position retargeters for full robot control + +.. dropdown:: GR1T2Retargeter (:class:`isaaclab.devices.openxr.retargeters.GR1T2Retargeter`) + + * Retargets OpenXR hand tracking data to GR1T2 hand end-effector commands + * 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 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Here's an example of setting up hand tracking: + +.. code-block:: python + + from isaaclab.devices import OpenXRDevice, OpenXRDeviceCfg + from isaaclab.devices.openxr.retargeters import Se3AbsRetargeter, GripperRetargeter + + # Create retargeters + position_retargeter = Se3AbsRetargeter( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_position=False # Use pinch position (thumb-index midpoint) instead of wrist + ) + gripper_retargeter = GripperRetargeter(bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT) + + # Create OpenXR device with hand tracking and both retargeters + device = OpenXRDevice( + OpenXRDeviceCfg(xr_cfg=env_cfg.xr), + retargeters=[position_retargeter, gripper_retargeter], + ) + + # Main control loop + while True: + # Get the latest commands from the XR device + commands = device.advance() + if commands is None: + continue + + # Apply the commands to the environment + obs, reward, terminated, truncated, info = env.step(commands) + + 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 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The OpenXRDevice can handle events triggered by user interactions with XR UI elements like buttons and menus. +When a user interacts with these elements, the device triggers registered callback functions: + +.. code-block:: python + + # Register callbacks for teleop control events + device.add_callback("RESET", reset_callback) + device.add_callback("START", start_callback) + device.add_callback("STOP", stop_callback) + +When the user interacts with the XR UI, these callbacks will be triggered to control the simulation +or recording process. You can also add custom messages from the client side using custom keys that will +trigger these callbacks, allowing for programmatic control of the simulation alongside direct user interaction. +The custom keys can be any string value that matches the callback registration. + + +Teleop Environment Configuration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +XR-based teleoperation can be integrated with Isaac Lab's environment configuration system using the +``teleop_devices`` field in your environment configuration: + +.. code-block:: python + + from dataclasses import field + from isaaclab.envs import ManagerBasedEnvCfg + from isaaclab.devices import DevicesCfg, OpenXRDeviceCfg + from isaaclab.devices.openxr import XrCfg + from isaaclab.devices.openxr.retargeters import Se3AbsRetargeterCfg, GripperRetargeterCfg + + @configclass + class MyEnvironmentCfg(ManagerBasedEnvCfg): + """Configuration for a teleoperation-enabled environment.""" + + # Add XR configuration with custom anchor position + xr: XrCfg = XrCfg( + anchor_pos=[0.0, 0.0, 0.0], + anchor_rot=[1.0, 0.0, 0.0, 0.0] + ) + + # Define teleoperation devices + teleop_devices: DevicesCfg = field(default_factory=lambda: DevicesCfg( + # Configuration for hand tracking with absolute position control + handtracking=OpenXRDeviceCfg( + xr_cfg=None, # Will use environment's xr config + retargeters=[ + Se3AbsRetargeterCfg( + bound_hand=0, # HAND_LEFT enum value + zero_out_xy_rotation=True, + use_wrist_position=False, + ), + GripperRetargeterCfg(bound_hand=0), + ] + ), + # Add other device configurations as needed + )) + + +Teleop Device Factory +^^^^^^^^^^^^^^^^^^^^^ + +To create a teleoperation device from your environment configuration, use the ``create_teleop_device`` factory function: + +.. code-block:: python + + from isaaclab.devices import create_teleop_device + from isaaclab.envs import ManagerBasedEnv + + # Create environment from configuration + env_cfg = MyEnvironmentCfg() + env = ManagerBasedEnv(env_cfg) + + # Define callbacks for teleop events + callbacks = { + "RESET": lambda: print("Reset simulation"), + "START": lambda: print("Start teleoperation"), + "STOP": lambda: print("Stop teleoperation"), + } + + # Create teleop device from configuration with callbacks + device_name = "handtracking" # Must match a key in teleop_devices + device = create_teleop_device( + device_name, + env_cfg.teleop_devices, + callbacks=callbacks + ) + + # Use device in control loop + while True: + # Get the latest commands from the device + commands = device.advance() + if commands is None: + continue + + # Apply commands to environment + obs, reward, terminated, truncated, info = env.step(commands) + + +Extending the Retargeting System +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The retargeting system is designed to be extensible. You can create custom retargeters by following these steps: + +1. Create a configuration dataclass for your retargeter: + +.. code-block:: python + + from dataclasses import dataclass + from isaaclab.devices.retargeter_base import RetargeterCfg + + @dataclass + class MyCustomRetargeterCfg(RetargeterCfg): + """Configuration for my custom retargeter.""" + scaling_factor: float = 1.0 + filter_strength: float = 0.5 + # Add any other configuration parameters your retargeter needs + +2. Implement your retargeter class by extending the RetargeterBase: + +.. code-block:: python + + from isaaclab.devices.retargeter_base import RetargeterBase + from isaaclab.devices import OpenXRDevice + import torch + from typing import Any + + class MyCustomRetargeter(RetargeterBase): + """A custom retargeter that processes OpenXR tracking data.""" + + def __init__(self, cfg: MyCustomRetargeterCfg): + """Initialize retargeter with configuration. + + Args: + cfg: Configuration object for retargeter settings. + """ + super().__init__() + self.scaling_factor = cfg.scaling_factor + self.filter_strength = cfg.filter_strength + # Initialize any other required attributes + + def retarget(self, data: dict) -> Any: + """Transform raw tracking data into robot control commands. + + Args: + data: Dictionary containing tracking data from OpenXRDevice. + Keys are TrackingTarget enum values, values are joint pose dictionaries. + + Returns: + Any: The transformed control commands for the robot. + """ + # Access hand tracking data using TrackingTarget enum + right_hand_data = data[DeviceBase.TrackingTarget.HAND_RIGHT] + + # Extract specific joint positions and orientations + wrist_pose = right_hand_data.get("wrist") + thumb_tip_pose = right_hand_data.get("thumb_tip") + index_tip_pose = right_hand_data.get("index_tip") + + # Access head tracking data + head_pose = data[DeviceBase.TrackingTarget.HEAD] + + # Process the tracking data and apply your custom logic + # ... + + # Return control commands in appropriate format + return torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) # Example output + +3. Register your retargeter by setting ``retargeter_type`` on the config class: + +.. code-block:: python + + # Import your retargeter at the top of your module + from my_package.retargeters import MyCustomRetargeter, MyCustomRetargeterCfg + + # Link the config to the implementation for factory construction + MyCustomRetargeterCfg.retargeter_type = MyCustomRetargeter + +4. Now you can use your custom retargeter in teleop device configurations: + +.. code-block:: python + + from isaaclab.devices import OpenXRDeviceCfg, DevicesCfg + from isaaclab.devices.openxr import XrCfg + from my_package.retargeters import MyCustomRetargeterCfg + + # Create XR configuration for proper scene placement + xr_config = XrCfg(anchor_pos=[0.0, 0.0, 0.0], anchor_rot=[1.0, 0.0, 0.0, 0.0]) + + # Define teleop devices with custom retargeter + teleop_devices = DevicesCfg( + handtracking=OpenXRDeviceCfg( + xr_cfg=xr_config, + retargeters=[ + MyCustomRetargeterCfg( + scaling_factor=1.5, + filter_strength=0.7, + ), + ] + ), + ) + +As the OpenXR capabilities expand beyond hand tracking to include head tracking and other features, +additional retargeters can be developed to map this data to various robot control paradigms. + + +Creating Custom Teleop Devices +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +You can create and register your own custom teleoperation devices by following these steps: + +1. Create a configuration dataclass for your device: + +.. code-block:: python + + from dataclasses import dataclass + from isaaclab.devices import DeviceCfg + + @dataclass + class MyCustomDeviceCfg(DeviceCfg): + """Configuration for my custom device.""" + sensitivity: float = 1.0 + invert_controls: bool = False + # Add any other configuration parameters your device needs + +2. Implement your device class by inheriting from DeviceBase: + +.. code-block:: python + + from isaaclab.devices import DeviceBase + import torch + + class MyCustomDevice(DeviceBase): + """A custom teleoperation device.""" + + def __init__(self, cfg: MyCustomDeviceCfg): + """Initialize the device with configuration. + + Args: + cfg: Configuration object for device settings. + """ + super().__init__() + self.sensitivity = cfg.sensitivity + self.invert_controls = cfg.invert_controls + # Initialize any other required attributes + self._device_input = torch.zeros(7) # Example: 6D pose + gripper + + def reset(self): + """Reset the device state.""" + self._device_input.zero_() + # Reset any other state variables + + def add_callback(self, key: str, func): + """Add callback function for a button/event. + + Args: + key: Button or event name. + func: Callback function to be called when event occurs. + """ + # Implement callback registration + pass + + def advance(self) -> torch.Tensor: + """Get the latest commands from the device. + + Returns: + torch.Tensor: Control commands (e.g., delta pose + gripper). + """ + # Update internal state based on device input + # Return command tensor + return self._device_input + +3. Register your device with the teleoperation device factory by adding it to the ``DEVICE_MAP``: + +.. code-block:: python + + # Import your device at the top of your module + from my_package.devices import MyCustomDevice, MyCustomDeviceCfg + + # Add your device to the factory + from isaaclab.devices.teleop_device_factory import DEVICE_MAP + + # Register your device type with its constructor + DEVICE_MAP[MyCustomDeviceCfg] = MyCustomDevice + +4. Now you can use your custom device in environment configurations: + +.. code-block:: python + + from dataclasses import field + from isaaclab.envs import ManagerBasedEnvCfg + from isaaclab.devices import DevicesCfg + from my_package.devices import MyCustomDeviceCfg + + @configclass + class MyEnvironmentCfg(ManagerBasedEnvCfg): + """Environment configuration with custom teleop device.""" + + teleop_devices: DevicesCfg = field(default_factory=lambda: DevicesCfg( + my_custom_device=MyCustomDeviceCfg( + sensitivity=1.5, + invert_controls=True, + ), + )) + + +.. _xr-known-issues: + +Known Issues +------------ + +* ``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 + handler for AR Mode. + +* ``Invalid version string in _ParseVersionString`` + + 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 +--------------------- + +For information on deploying XR Teleop for Isaac Lab on a Kubernetes cluster, see :ref:`cloudxr-teleoperation-cluster`. + +.. + References +.. _`Apple Vision Pro`: https://www.apple.com/apple-vision-pro/ +.. _`Docker Compose`: https://docs.docker.com/compose/install/linux/#install-using-the-repository +.. _`Docker`: https://docs.docker.com/desktop/install/linux-install/ +.. _`NVIDIA CloudXR`: https://developer.nvidia.com/cloudxr-sdk +.. _`NVIDIA Container Toolkit`: https://github.com/NVIDIA/nvidia-container-toolkit +.. _`Isaac XR Teleop Sample Client`: https://github.com/isaac-sim/isaac-xr-teleop-sample-client-apple diff --git a/docs/source/how-to/configure_rendering.rst b/docs/source/how-to/configure_rendering.rst new file mode 100644 index 000000000000..adfa8b5556cc --- /dev/null +++ b/docs/source/how-to/configure_rendering.rst @@ -0,0 +1,151 @@ +Configuring Rendering Settings +============================== + +Isaac Lab offers 3 preset rendering modes: performance, balanced, and quality. +You can select a mode via a command line argument or from within a script, and customize settings as needed. +Adjust and fine-tune rendering to achieve the ideal balance for your workflow. + +Selecting a Rendering Mode +-------------------------- + +Rendering modes can be selected in 2 ways. + +1. using the ``rendering_mode`` input class argument in :class:`~sim.RenderCfg` + + .. code-block:: python + + # for an example of how this can be used, checkout the tutorial script + # scripts/tutorials/00_sim/set_rendering_mode.py + render_cfg = sim_utils.RenderCfg(rendering_mode="performance") + +2. using the ``--rendering_mode`` CLI argument, which takes precedence over the ``rendering_mode`` argument in :class:`~sim.RenderCfg`. + + .. code-block:: bash + + ./isaaclab.sh -p scripts/tutorials/00_sim/set_rendering_mode.py --rendering_mode {performance/balanced/quality} + + +Note, the ``rendering_mode`` defaults to ``balanced``. +However, in the case where the launcher argument ``--enable_cameras`` is not set, then +the default ``rendering_mode`` is not applied and, instead, the default kit rendering settings are used. + + +Example renders from the ``set_rendering_mode.py`` script. +To help assess rendering, the example scene includes some reflections, translucency, direct and ambient lighting, and several material types. + +- Quality Mode + + .. image:: ../_static/how-to/howto_rendering_example_quality.jpg + :width: 100% + :alt: Quality Rendering Mode Example + +- Balanced Mode + + .. image:: ../_static/how-to/howto_rendering_example_balanced.jpg + :width: 100% + :alt: Balanced Rendering Mode Example + +- Performance Mode + + .. image:: ../_static/how-to/howto_rendering_example_performance.jpg + :width: 100% + :alt: Performance Rendering Mode Example + +Overwriting Specific Rendering Settings +--------------------------------------- + +Preset rendering settings can be overwritten via the :class:`~sim.RenderCfg` class. + +There are 2 ways to provide settings that overwrite presets. + +1. :class:`~sim.RenderCfg` supports overwriting specific settings via user-friendly setting names that map to underlying RTX settings. + For example: + + .. code-block:: python + + render_cfg = sim_utils.RenderCfg( + rendering_mode="performance", + # user friendly setting overwrites + enable_translucency=True, # defaults to False in performance mode + enable_reflections=True, # defaults to False in performance mode + dlss_mode="3", # defaults to 1 in performance mode + ) + + List of user-friendly settings. + + .. table:: + :widths: 25 75 + + +----------------------------+--------------------------------------------------------------------------+ + | enable_translucency | Bool. Enables translucency for specular transmissive surfaces such as | + | | glass at the cost of some performance. | + +----------------------------+--------------------------------------------------------------------------+ + | enable_reflections | Bool. Enables reflections at the cost of some performance. | + +----------------------------+--------------------------------------------------------------------------+ + | enable_global_illumination | Bool. Enables Diffused Global Illumination at the cost of some | + | | performance. | + +----------------------------+--------------------------------------------------------------------------+ + | antialiasing_mode | Literal["Off", "FXAA", "DLSS", "TAA", "DLAA"]. | + | | | + | | 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.| + +----------------------------+--------------------------------------------------------------------------+ + | enable_dlssg | Bool. Enables the use of DLSS-G. DLSS Frame Generation boosts performance| + | | by using AI to generate more frames. This feature requires an Ada | + | | Lovelace architecture GPU and can hurt performance due to additional | + | | thread-related activities. | + +----------------------------+--------------------------------------------------------------------------+ + | enable_dl_denoiser | Bool. Enables the use of a DL denoiser, which improves the quality of | + | | renders at the cost of performance. | + +----------------------------+--------------------------------------------------------------------------+ + | dlss_mode | Literal[0, 1, 2, 3]. For DLSS anti-aliasing, selects the performance/ | + | | quality tradeoff mode. Valid values are 0 (Performance), 1 (Balanced), | + | | 2 (Quality), or 3 (Auto). | + +----------------------------+--------------------------------------------------------------------------+ + | enable_direct_lighting | Bool. Enable direct light contributions from lights. | + +----------------------------+--------------------------------------------------------------------------+ + | samples_per_pixel | Int. Defines the Direct Lighting samples per pixel. Higher values | + | | increase the direct lighting quality at the cost of performance. | + +----------------------------+--------------------------------------------------------------------------+ + | enable_shadows | Bool. Enables shadows at the cost of performance. When disabled, lights | + | | will not cast shadows. | + +----------------------------+--------------------------------------------------------------------------+ + | enable_ambient_occlusion | Bool. Enables ambient occlusion at the cost of some performance. | + +----------------------------+--------------------------------------------------------------------------+ + + +2. For more control, :class:`~sim.RenderCfg` allows you to overwrite any RTX setting by using the ``carb_settings`` argument. + + Examples of RTX settings can be found from within the repo, in the render mode preset files located in ``apps/rendering_modes``. + + In addition, the RTX documentation can be found here - https://docs.omniverse.nvidia.com/materials-and-rendering/latest/rtx-renderer.html. + + An example usage of ``carb_settings``. + + .. code-block:: python + + render_cfg = sim_utils.RenderCfg( + rendering_mode="quality", + # carb setting overwrites + carb_settings={ + "rtx.translucency.enabled": False, + "rtx.reflections.enabled": False, + "rtx.domeLight.upperLowerStrategy": 3, + } + ) + + +Current Limitations +------------------- + +For performance reasons, we default to using DLSS for denoising, which generally provides better performance. +This may result in renders of lower quality, which may be especially evident at lower resolutions. +Due to this, we recommend using per-tile or per-camera resolution of at least 100 x 100. +For renders at lower resolutions, we advice setting the ``antialiasing_mode`` attribute in :class:`~sim.RenderCfg` to +``DLAA``, and also potentially enabling ``enable_dl_denoiser``. Both of these settings should help improve render +quality, but also comes at a cost of performance. Additional rendering parameters can also be specified in :class:`~sim.RenderCfg`. diff --git a/docs/source/how-to/curriculums.rst b/docs/source/how-to/curriculums.rst new file mode 100644 index 000000000000..8c2a94e82cbe --- /dev/null +++ b/docs/source/how-to/curriculums.rst @@ -0,0 +1,122 @@ +Curriculum Utilities +==================== + +.. currentmodule:: isaaclab.managers + +This guide walks through the common curriculum helper functions and terms that can be used to create flexible curricula +for RL environments in Isaac Lab. These utilities can be passed to a :class:`~isaaclab.managers.CurriculumTermCfg` +object to enable dynamic modification of reward weights and environment parameters during training. + +.. note:: + + We cover three utilities in this guide: + - The simple function modifies reward :func:`modify_reward_weight` + - The term modify any environment parameters :class:`modify_env_param` + - The term modify term_cfg :class:`modify_term_cfg` + +.. dropdown:: Full source for curriculum utilities + :icon: code + + .. literalinclude:: ../../../source/isaaclab/isaaclab/envs/mdp/curriculums.py + :language: python + + +Modifying Reward Weights +------------------------ + +The function :func:`modify_reward_weight` updates the weight of a reward term after a specified number of simulation +steps. This can be passed directly as the ``func`` in a ``CurriculumTermCfg``. + +.. literalinclude:: ../../../source/isaaclab/isaaclab/envs/mdp/curriculums.py + :language: python + :pyobject: modify_reward_weight + +**Usage example**: + +.. code-block:: python + + from isaaclab.managers import CurriculumTermCfg + import isaaclab.managers.mdp as mdp + + # After 100k steps, set the "sparse_reward" term weight to 0.5 + sparse_reward_schedule = CurriculumTermCfg( + func=mdp.modify_reward_weight, + params={ + "term_name": "sparse_reward", + "weight": 0.5, + "num_steps": 100_000, + } + ) + + +Dynamically Modifying Environment Parameters +-------------------------------------------- + +The class :class:`modify_env_param` is a :class:`~isaaclab.managers.ManagerTermBase` subclass that lets you target any +dotted attribute path in the environment and apply a user-supplied function to compute a new value at runtime. It +handles nested attributes, dictionary keys, list or tuple indexing, and respects a ``NO_CHANGE`` sentinel if no update +is desired. + +.. literalinclude:: ../../../source/isaaclab/isaaclab/envs/mdp/curriculums.py + :language: python + :pyobject: modify_env_param + +**Usage example**: + +.. code-block:: python + + import torch + from isaaclab.managers import CurriculumTermCfg + import isaaclab.managers.mdp as mdp + + def resample_friction(env, env_ids, old_value, low, high, num_steps): + # After num_steps, sample a new friction coefficient uniformly + if env.common_step_counter > num_steps: + return torch.empty((len(env_ids),), device="cpu").uniform_(low, high) + return mdp.modify_env_param.NO_CHANGE + + friction_curriculum = CurriculumTermCfg( + func=mdp.modify_env_param, + params={ + "address": "event_manager.cfg.object_physics_material.func.material_buckets", + "modify_fn": resample_friction, + "modify_params": { + "low": 0.3, + "high": 1.0, + "num_steps": 120_000, + } + } + ) + + +Modify Term Configuration +------------------------- + +The subclass :class:`modify_term_cfg` provides a more concise style address syntax, using consistent with hydra config +syntax. It otherwise behaves identically to :class:`modify_env_param`. + +.. literalinclude:: ../../../source/isaaclab/isaaclab/envs/mdp/curriculums.py + :language: python + :pyobject: modify_term_cfg + +**Usage example**: + +.. code-block:: python + + def override_command_range(env, env_ids, old_value, value, num_steps): + # Override after num_steps + if env.common_step_counter > num_steps: + return value + return mdp.modify_term_cfg.NO_CHANGE + + range_override = CurriculumTermCfg( + func=mdp.modify_term_cfg, + params={ + "address": "commands.object_pose.ranges.pos_x", + "modify_fn": override_command_range, + "modify_params": { + "value": (-0.75, -0.25), + "num_steps": 12_000, + } + } + ) diff --git a/docs/source/how-to/haply_teleoperation.rst b/docs/source/how-to/haply_teleoperation.rst new file mode 100644 index 000000000000..1f8d1d6e2522 --- /dev/null +++ b/docs/source/how-to/haply_teleoperation.rst @@ -0,0 +1,240 @@ +.. _haply-teleoperation: + +Setting up Haply Teleoperation +=============================== + +.. currentmodule:: isaaclab + +`Haply Devices`_ provides haptic devices that enable intuitive robot teleoperation with +directional force feedback. The Haply Inverse3 paired with the VerseGrip creates an +end-effector control system with force feedback capabilities. + +Isaac Lab supports Haply devices for teleoperation workflows that require precise spatial +control with haptic feedback. This enables operators to feel contact forces during manipulation +tasks, improving control quality and task performance. + +This guide explains how to set up and use Haply devices with Isaac Lab for robot teleoperation. + +.. _Haply Devices: https://haply.co/ + + +Overview +-------- + +Using Haply with Isaac Lab involves the following components: + +* **Isaac Lab** simulates the robot environment and streams contact forces back to the operator + +* **Haply Inverse3** provides 3-DOF position tracking and force feedback in the operator's workspace + +* **Haply VerseGrip** adds orientation sensing and button inputs for gripper control + +* **Haply SDK** manages WebSocket communication between Isaac Lab and the Haply hardware + +This guide will walk you through: + +* :ref:`haply-system-requirements` +* :ref:`haply-installation` +* :ref:`haply-device-setup` +* :ref:`haply-running-demo` +* :ref:`haply-troubleshooting` + + +.. _haply-system-requirements: + +System Requirements +------------------- + +Hardware Requirements +~~~~~~~~~~~~~~~~~~~~~ + +* **Isaac Lab Workstation** + + * Ubuntu 22.04 or Ubuntu 24.04 + * Hardware requirements for 200Hz physics simulation: + + * CPU: 8-Core Intel Core i7 or AMD Ryzen 7 (or higher) + * Memory: 32GB RAM (64GB recommended) + * GPU: RTX 3090 or higher + + * Network: Same local network as Haply devices for WebSocket communication + +* **Haply Devices** + + * Haply Inverse3 - Haptic device for position tracking and force feedback + * Haply VerseGrip - Wireless controller for orientation and button inputs + * Both devices must be powered on and connected to the Haply SDK + +Software Requirements +~~~~~~~~~~~~~~~~~~~~~ + +* Isaac Lab (follow the :ref:`installation guide `) +* Haply SDK (provided by Haply Robotics) +* Python 3.10+ +* ``websockets`` Python package (automatically installed with Isaac Lab) + + +.. _haply-installation: + +Installation +------------ + +1. Install Isaac Lab +~~~~~~~~~~~~~~~~~~~~ + +Follow the Isaac Lab :ref:`installation guide ` to set up your environment. + +The ``websockets`` dependency is automatically included in Isaac Lab's requirements. + +2. Install Haply SDK +~~~~~~~~~~~~~~~~~~~~ + +Download the Haply SDK from the `Haply Devices`_ website. +Install the SDK software and configure the devices. + +3. Verify Installation +~~~~~~~~~~~~~~~~~~~~~~ + +Test that your Haply devices are detected by the Haply Device Manager. +You should see both Inverse3 and VerseGrip as connected. + + +.. _haply-device-setup: + +Device Setup +------------ + +1. Physical Setup +~~~~~~~~~~~~~~~~~ + +* Place the Haply Inverse3 on a stable surface +* Ensure the VerseGrip is charged and paired +* Position yourself comfortably to reach the Inverse3 workspace +* Keep the workspace clear of obstacles + +2. Start Haply SDK +~~~~~~~~~~~~~~~~~~ + +Launch the Haply SDK according to Haply's documentation. The SDK typically: + +* Runs a WebSocket server on ``localhost:10001`` +* Streams device data at 200Hz +* Displays connection status for both devices + +3. Test Communication +~~~~~~~~~~~~~~~~~~~~~ + +You can test the WebSocket connection using the following Python script: + +.. code:: python + + import asyncio + import websockets + import json + + async def test_haply(): + uri = "ws://localhost:10001" + async with websockets.connect(uri) as ws: + response = await ws.recv() + data = json.loads(response) + print("Inverse3:", data.get("inverse3", [])) + print("VerseGrip:", data.get("wireless_verse_grip", [])) + + asyncio.run(test_haply()) + +You should see device data streaming from both Inverse3 and VerseGrip. + + +.. _haply-running-demo: + +Running the Demo +---------------- + +The Haply teleoperation demo showcases robot manipulation with force feedback using +a Franka Panda arm. + +Basic Usage +~~~~~~~~~~~ + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # Ensure Haply SDK is running + ./isaaclab.sh -p scripts/demos/haply_teleoperation.py --websocket_uri ws://localhost:10001 --pos_sensitivity 1.65 + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + REM Ensure Haply SDK is running + isaaclab.bat -p scripts\demos\haply_teleoperation.py --websocket_uri ws://localhost:10001 --pos_sensitivity 1.65 + +The demo will: + +1. Connect to the Haply devices via WebSocket +2. Spawn a Franka Panda robot and a cube in simulation +3. Map Haply position to robot end-effector position +4. Stream contact forces back to the Inverse3 for haptic feedback + +Controls +~~~~~~~~ + +* **Move Inverse3**: Controls the robot end-effector position +* **VerseGrip Button A**: Open gripper +* **VerseGrip Button B**: Close gripper +* **VerseGrip Button C**: Rotate end-effector by 60ยฐ + +Advanced Options +~~~~~~~~~~~~~~~~ + +Customize the demo with command-line arguments: + +.. code:: bash + + # Use custom WebSocket URI + ./isaaclab.sh -p scripts/demos/haply_teleoperation.py \ + --websocket_uri ws://192.168.1.100:10001 + + # Adjust position sensitivity (default: 1.0) + ./isaaclab.sh -p scripts/demos/haply_teleoperation.py \ + --websocket_uri ws://localhost:10001 \ + --pos_sensitivity 2.0 + +Demo Features +~~~~~~~~~~~~~ + +* **Workspace Mapping**: Haply workspace is mapped to robot reachable space with safety limits +* **Inverse Kinematics**: Inverse Kinematics (IK) computes joint positions for desired end-effector pose +* **Force Feedback**: Contact forces from end-effector sensors are sent to Inverse3 for haptic feedback + + +.. _haply-troubleshooting: + +Troubleshooting +--------------- + +No Haptic Feedback +~~~~~~~~~~~~~~~~~~ + +**Problem**: No haptic feedback felt on Inverse3 + +Solutions: + +* Verify Inverse3 is the active device in Haply SDK +* Check contact forces are non-zero in simulation (try grasping the cube) +* Ensure ``limit_force`` is not set too low (default: 2.0N) + + +Next Steps +---------- + +* **Customize the demo**: Modify the workspace mapping or add custom button behaviors +* **Implement your own controller**: Use :class:`~isaaclab.devices.HaplyDevice` in your own scripts + +For more information on device APIs, see :class:`~isaaclab.devices.HaplyDevice` in the API documentation. diff --git a/docs/source/how-to/import_new_asset.rst b/docs/source/how-to/import_new_asset.rst index 52a683c69062..41eacc48673d 100644 --- a/docs/source/how-to/import_new_asset.rst +++ b/docs/source/how-to/import_new_asset.rst @@ -80,24 +80,47 @@ pre-processed URDF and the original URDF are: The following shows the steps to clone the repository and run the converter: -.. code-block:: bash - # create a directory to clone - mkdir ~/git && cd ~/git - # clone a repository with URDF files - git clone git@github.com:isaac-orbit/anymal_d_simple_description.git +.. tab-set:: + :sync-group: os - # go to top of the Isaac Lab repository - cd IsaacLab - # run the converter - ./isaaclab.sh -p scripts/tools/convert_urdf.py \ - ~/git/anymal_d_simple_description/urdf/anymal.urdf \ - source/isaaclab_assets/data/Robots/ANYbotics/anymal_d.usd \ - --merge-joints \ - --joint-stiffness 0.0 \ - --joint-damping 0.0 \ - --joint-target-type none + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + .. code-block:: bash + + # clone a repository with URDF files + git clone git@github.com:isaac-orbit/anymal_d_simple_description.git + + # go to top of the Isaac Lab repository + cd IsaacLab + # run the converter + ./isaaclab.sh -p scripts/tools/convert_urdf.py \ + ../anymal_d_simple_description/urdf/anymal.urdf \ + source/isaaclab_assets/data/Robots/ANYbotics/anymal_d.usd \ + --merge-joints \ + --joint-stiffness 0.0 \ + --joint-damping 0.0 \ + --joint-target-type none + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + :: clone a repository with URDF files + git clone git@github.com:isaac-orbit/anymal_d_simple_description.git + + :: go to top of the Isaac Lab repository + cd IsaacLab + :: run the converter + isaaclab.bat -p scripts\tools\convert_urdf.py ^ + ..\anymal_d_simple_description\urdf\anymal.urdf ^ + source\isaaclab_assets\data\Robots\ANYbotics\anymal_d.usd ^ + --merge-joints ^ + --joint-stiffness 0.0 ^ + --joint-damping 0.0 ^ + --joint-target-type none Executing the above script will create a USD file inside the ``source/isaaclab_assets/data/Robots/ANYbotics/`` directory: @@ -150,21 +173,43 @@ In this example, we use the MuJoCo model of the Unitree's H1 humanoid robot in t The following shows the steps to clone the repository and run the converter: -.. code-block:: bash - # create a directory to clone - mkdir ~/git && cd ~/git - # clone a repository with URDF files - git clone git@github.com:google-deepmind/mujoco_menagerie.git +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + # clone a repository with URDF files + git clone git@github.com:google-deepmind/mujoco_menagerie.git + + # go to top of the Isaac Lab repository + cd IsaacLab + # run the converter + ./isaaclab.sh -p scripts/tools/convert_mjcf.py \ + ../mujoco_menagerie/unitree_h1/h1.xml \ + source/isaaclab_assets/data/Robots/Unitree/h1.usd \ + --import-sites \ + --make-instanceable - # go to top of the Isaac Lab repository - cd IsaacLab - # run the converter - ./isaaclab.sh -p scripts/tools/convert_mjcf.py \ - ~/git/mujoco_menagerie/unitree_h1/h1.xml \ - source/isaaclab_assets/data/Robots/Unitree/h1.usd \ - --import-sites \ - --make-instanceable + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + :: clone a repository with URDF files + git clone git@github.com:google-deepmind/mujoco_menagerie.git + + :: go to top of the Isaac Lab repository + cd IsaacLab + :: run the converter + isaaclab.bat -p scripts\tools\convert_mjcf.py ^ + ..\mujoco_menagerie\unitree_h1\h1.xml ^ + source\isaaclab_assets\data\Robots\Unitree\h1.usd ^ + --import-sites ^ + --make-instanceable Executing the above script will create USD files inside the ``source/isaaclab_assets/data/Robots/Unitree/`` directory: @@ -202,22 +247,44 @@ Example Usage We use an OBJ file of a cube to demonstrate the usage of the mesh converter. The following shows the steps to clone the repository and run the converter: -.. code-block:: bash +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + # clone a repository with URDF files + git clone git@github.com:NVIDIA-Omniverse/IsaacGymEnvs.git + + # go to top of the Isaac Lab repository + cd IsaacLab + # run the converter + ./isaaclab.sh -p scripts/tools/convert_mesh.py \ + ../IsaacGymEnvs/assets/trifinger/objects/meshes/cube_multicolor.obj \ + source/isaaclab_assets/data/Props/CubeMultiColor/cube_multicolor.usd \ + --make-instanceable \ + --collision-approximation convexDecomposition \ + --mass 1.0 + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch - # create a directory to clone - mkdir ~/git && cd ~/git - # clone a repository with URDF files - git clone git@github.com:NVIDIA-Omniverse/IsaacGymEnvs.git + :: clone a repository with URDF files + git clone git@github.com:NVIDIA-Omniverse/IsaacGymEnvs.git - # go to top of the Isaac Lab repository - cd IsaacLab - # run the converter - ./isaaclab.sh -p scripts/tools/convert_mesh.py \ - ~/git/IsaacGymEnvs/assets/trifinger/objects/meshes/cube_multicolor.obj \ - source/isaaclab_assets/data/Props/CubeMultiColor/cube_multicolor.usd \ - --make-instanceable \ - --collision-approximation convexDecomposition \ - --mass 1.0 + :: go to top of the Isaac Lab repository + cd IsaacLab + :: run the converter + isaaclab.bat -p scripts\tools\convert_mesh.py ^ + ..\IsaacGymEnvs\assets\trifinger\objects\meshes\cube_multicolor.obj ^ + source\isaaclab_assets\data\Props\CubeMultiColor\cube_multicolor.usd ^ + --make-instanceable ^ + --collision-approximation convexDecomposition ^ + --mass 1.0 You may need to press 'F' to zoom in on the asset after import. @@ -240,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/index.rst b/docs/source/how-to/index.rst index c884a071def2..02c0ff99ae14 100644 --- a/docs/source/how-to/index.rst +++ b/docs/source/how-to/index.rst @@ -67,6 +67,15 @@ This guide demonstrates how to estimate the number of cameras one can run on the estimate_how_many_cameras_can_run +Configure Rendering +------------------- + +This guide demonstrates how to select rendering mode presets and overwrite preset rendering settings. + +.. toctree:: + :maxdepth: 1 + + configure_rendering Drawing Markers --------------- @@ -103,6 +112,19 @@ This guide explains how to record an animation and video in Isaac Lab. record_animation record_video + +Dynamically Modifying Environment Parameters With CurriculumTerm +---------------------------------------------------------------- + +This guide explains how to dynamically modify environment parameters during training in Isaac Lab. +It covers the use of curriculum utilities to change environment parameters at runtime. + +.. toctree:: + :maxdepth: 1 + + curriculums + + Mastering Omniverse ------------------- @@ -113,3 +135,51 @@ additional resources that help you use Omniverse features in Isaac Lab. :maxdepth: 1 master_omniverse + + +Setting up CloudXR Teleoperation +-------------------------------- + +This guide explains how to use CloudXR and Apple Vision Pro for immersive streaming and +teleoperation in Isaac Lab. + +.. toctree:: + :maxdepth: 1 + + cloudxr_teleoperation + + +Setting up Haply Teleoperation +------------------------------ + +This guide explains how to use Haply Inverse3 and VerseGrip devices for robot teleoperation +with directional force feedback in Isaac Lab. + +.. toctree:: + :maxdepth: 1 + + haply_teleoperation + + +Understanding Simulation Performance +------------------------------------ + +This guide provides tips on optimizing simulation performance for different simulation use cases. +Additional resources are also linked to provide relevant performance guides for Isaac Sim and +Omniverse Physics. + +.. toctree:: + :maxdepth: 1 + + simulation_performance + + +Optimize Stage Creation +----------------------- + +This guide explains 2 features that can speed up stage initialization, **fabric cloning** and **stage in memory**. + +.. toctree:: + :maxdepth: 1 + + optimize_stage_creation diff --git a/docs/source/how-to/master_omniverse.rst b/docs/source/how-to/master_omniverse.rst index 0108ab64821b..ee3e0d55c4e4 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/optimize_stage_creation.rst b/docs/source/how-to/optimize_stage_creation.rst new file mode 100644 index 000000000000..b262878d6671 --- /dev/null +++ b/docs/source/how-to/optimize_stage_creation.rst @@ -0,0 +1,146 @@ +Optimize Stage Creation +======================= + +Isaac Lab supports two experimental features to speed-up stage creation: **fabric cloning** and **stage in memory**. +These features are particularly effective for large-scale RL setups with thousands of environments. + +What These Features Do +----------------------- + +**Fabric Cloning** + +- Clones environments using Fabric library (see `USD Fabric USDRT Documentation `_) +- Partially supported and enabled by default on some environments (see `Limitations`_ section for a list) + +**Stage in Memory** + +- Constructs the stage in memory, rather than with a USD file, avoiding overhead from disk I/O +- After stage creation, if rendering is required, the stage is attached to the USD context, returning to the default stage configuration +- Not enabled by default + +Usage Examples +-------------- + +Fabric cloning can be toggled by setting the :attr:`isaaclab.scene.InteractiveSceneCfg.clone_in_fabric` flag. + +**Using Fabric Cloning with a RL environment** + +.. code-block:: python + + # create environment configuration + env_cfg = CartpoleEnvCfg() + env_cfg.scene.clone_in_fabric = True + # setup RL environment + env = ManagerBasedRLEnv(cfg=env_cfg) + + +Stage in memory can be toggled by setting the :attr:`isaaclab.sim.SimulationCfg.create_stage_in_memory` flag. + +**Using Stage in Memory with a RL environment** + +.. code-block:: python + + # create config and set flag + cfg = CartpoleEnvCfg() + cfg.scene.num_envs = 1024 + cfg.sim.create_stage_in_memory = True + # create env with stage in memory + env = ManagerBasedRLEnv(cfg=cfg) + +Note, if stage in memory is enabled without using an existing RL environment class, a few more steps are need. +The stage creation steps should be wrapped in a :py:keyword:`with` statement to set the stage context. +If the stage needs to be attached, the :meth:`~isaaclab.sim.utils.attach_stage_to_usd_context` function should +be called after the stage is created. + +**Using Stage in Memory with a manual scene setup** + +.. code-block:: python + + # init simulation context with stage in memory + sim = SimulationContext(cfg=SimulationCfg(create_stage_in_memory=True)) + + # grab stage in memory and set stage context + stage_in_memory = sim.get_initial_stage() + with stage_utils.use_stage(stage_in_memory): + # create cartpole scene + scene_cfg = CartpoleSceneCfg(num_envs=1024) + scene = InteractiveScene(scene_cfg) + # attach stage to memory after stage is created + sim_utils.attach_stage_to_usd_context() + + sim.play() + + +Limitations +----------- + +**Fabric Cloning** + +- Fabric-cloned environments must be accessed using USDRT functions, rather than USD functions. +- Fabric cloning is partially supported and enabled by default on some environments, listed here. + +.. code-block:: none + + 1. Isaac-Ant-Direct-v0 + 2. Isaac-Ant-v0 + 3. Isaac-Cartpole-Direct-v0 + 4. Isaac-Cartpole-Showcase-Box-Box-Direct-v0 + 5. Isaac-Cartpole-Showcase-Box-Discrete-Direct-v0 + 6. Isaac-Cartpole-Showcase-Box-MultiDiscrete-Direct-v0 + 7. Isaac-Cartpole-Showcase-Dict-Box-Direct-v0 + 8. Isaac-Cartpole-Showcase-Dict-Discrete-Direct-v0 + 9. Isaac-Cartpole-Showcase-Dict-MultiDiscrete-Direct-v0 + 10. Isaac-Cartpole-Showcase-Discrete-Box-Direct-v0 + 11. Isaac-Cartpole-Showcase-Discrete-Discrete-Direct-v0 + 12. Isaac-Cartpole-Showcase-Discrete-MultiDiscrete-Direct-v0 + 13. Isaac-Cartpole-Showcase-MultiDiscrete-Box-Direct-v0 + 14. Isaac-Cartpole-Showcase-MultiDiscrete-Discrete-Direct-v0 + 15. Isaac-Cartpole-Showcase-MultiDiscrete-MultiDiscrete-Direct-v0 + 16. Isaac-Cartpole-Showcase-Tuple-Box-Direct-v0 + 17. Isaac-Cartpole-Showcase-Tuple-Discrete-Direct-v0 + 18. Isaac-Cartpole-Showcase-Tuple-MultiDiscrete-Direct-v0 + 19. Isaac-Cartpole-v0 + 20. Isaac-Factory-GearMesh-Direct-v0 + 21. Isaac-Factory-NutThread-Direct-v0 + 22. Isaac-Factory-PegInsert-Direct-v0 + 23. Isaac-Franka-Cabinet-Direct-v0 + 24. Isaac-Humanoid-Direct-v0 + 25. Isaac-Humanoid-v0 + 26. Isaac-Quadcopter-Direct-v0 + 27. Isaac-Repose-Cube-Allegro-Direct-v0 + 28. Isaac-Repose-Cube-Allegro-NoVelObs-v0 + 29. Isaac-Repose-Cube-Allegro-v0 + 30. Isaac-Repose-Cube-Shadow-Direct-v0 + 31. Isaac-Repose-Cube-Shadow-OpenAI-FF-Direct-v0 + 32. Isaac-Repose-Cube-Shadow-OpenAI-LSTM-Direct-v0 + +**Stage in Memory** + +- Cannot be currently enabled at the same time as **Fabric Cloning**. + +- Attaching stage in memory to the USD context can be slow, offsetting some or all of the performance benefits. + + - Note, attaching is only necessary when rendering is enabled. For example, in headless mode, attachment is not required. + +- Certain low-level Kit APIs do not yet support stage in memory. + + - In most cases, when these APIs are hit, existing scripts will automatically early attach the stage and print a warning message. + - In one particular case, for some environments, the API call to color the ground plane is skipped, when stage in memory is enabled. + + +Benchmark Results +----------------- + +Performance comparison cloning 4000 ShadowHand robots with rendering enabled + ++--------+-----------------+-------------------+------------------------+---------------------------+------------------------+------------------------+ +| Test # | Stage in Memory | Clone in Fabric | Attach Stage Time (s) | Fabric Attach Time (s) | Clone Paths Time (s) | First Step Time (s) | ++========+=================+===================+========================+===========================+========================+========================+ +| 1 | Yes | Yes | 3.88 | 0.15 | 4.84 | 1.39 | ++--------+-----------------+-------------------+------------------------+---------------------------+------------------------+------------------------+ +| 2 | No | No | โ€” | 60.17 | 4.46 | 3.52 | ++--------+-----------------+-------------------+------------------------+---------------------------+------------------------+------------------------+ +| 3 | No | Yes | โ€” | 0.47 | 4.72 | 2.56 | ++--------+-----------------+-------------------+------------------------+---------------------------+------------------------+------------------------+ +| 4 | Yes | No | 42.64 | 21.75 | 1.87 | 2.16 | ++--------+-----------------+-------------------+------------------------+---------------------------+------------------------+------------------------+ diff --git a/docs/source/how-to/record_animation.rst b/docs/source/how-to/record_animation.rst index ed5eca12e95d..2d675684fbf4 100644 --- a/docs/source/how-to/record_animation.rst +++ b/docs/source/how-to/record_animation.rst @@ -3,32 +3,41 @@ Recording Animations of Simulations .. currentmodule:: isaaclab -Omniverse includes tools to record animations of physics simulations. The `Stage Recorder`_ extension -listens to all the motion and USD property changes within a USD stage and records them to a USD file. -This file contains the time samples of the changes, which can be played back to render the animation. +Isaac Lab supports two approaches for recording animations of physics simulations: the **Stage Recorder** and the **OVD Recorder**. +Both generate USD outputs that can be played back in Omniverse, but they differ in how they work and when youโ€™d use them. -The timeSampled USD file only contains the changes to the stage. It uses the same hierarchy as the original -stage at the time of recording. This allows adding the animation to the original stage, or to a different -stage with the same hierarchy. The timeSampled file can be directly added as a sublayer to the original stage -to play back the animation. +The `Stage Recorder`_ extension listens to all motion and USD property changes in the stage during simulation +and records them as **time-sampled data**. The result is a USD file that captures only the animated changesโ€”**not** the +full sceneโ€”and matches the hierarchy of the original stage at the time of recording. +This makes it easy to add as a sublayer for playback or rendering. + +This method is built into Isaac Labโ€™s UI through the :class:`~isaaclab.envs.ui.BaseEnvWindow`. +However, to record the animation of a simulation, you need to disable `Fabric`_ to allow reading and writing +all the changes (such as motion and USD properties) to the USD stage. + +The **OVD Recorder** is designed for more scalable or automated workflows. It uses OmniPVD to capture simulated physics from a played stage +and then **bakes** that directly into an animated USD file. It works with Fabric enabled and runs with CLI arguments. +The animated USD can be quickly replayed and reviewed by scrubbing through the timeline window, without simulating expensive physics operations. .. note:: - Omniverse only supports playing animation or playing physics on a USD prim at the same time. If you want to - play back the animation of a USD prim, you need to disable the physics simulation on the prim. + Omniverse only supports **either** physics simulation **or** animation playback on a USD primโ€”never both at once. + Disable physics on the prims you want to animate. -In Isaac Lab, we directly use the `Stage Recorder`_ extension to record the animation of the physics simulation. -This is available as a feature in the :class:`~isaaclab.envs.ui.BaseEnvWindow` class. -However, to record the animation of a simulation, you need to disable `Fabric`_ to allow reading and writing -all the changes (such as motion and USD properties) to the USD stage. +Stage Recorder +-------------- +In Isaac Lab, the Stage Recorder is integrated into the :class:`~isaaclab.envs.ui.BaseEnvWindow` class. +Itโ€™s the easiest way to capture physics simulations visually and works directly through the UI. + +To record, Fabric must be disabledโ€”this allows the recorder to track changes to USD and write them out. Stage Recorder Settings ~~~~~~~~~~~~~~~~~~~~~~~ -Isaac Lab integration of the `Stage Recorder`_ extension assumes certain default settings. If you want to change the -settings, you can directly use the `Stage Recorder`_ extension in the Omniverse Create application. +Isaac Lab sets up the Stage Recorder with sensible defaults in ``base_env_window.py``. If needed, +you can override or inspect these by using the Stage Recorder extension directly in Omniverse Create. .. dropdown:: Settings used in base_env_window.py :icon: code @@ -38,39 +47,79 @@ settings, you can directly use the `Stage Recorder`_ extension in the Omniverse :linenos: :pyobject: BaseEnvWindow._toggle_recording_animation_fn - Example Usage ~~~~~~~~~~~~~ -In all environment standalone scripts, Fabric can be disabled by passing the ``--disable_fabric`` flag to the script. -Here we run the state-machine example and record the animation of the simulation. +In standalone Isaac Lab environments, pass the ``--disable_fabric`` flag: .. code-block:: bash ./isaaclab.sh -p scripts/environments/state_machine/lift_cube_sm.py --num_envs 8 --device cpu --disable_fabric +After launching, the Isaac Lab UI window will display a "Record Animation" button. +Click to begin recording. Click again to stop. -On running the script, the Isaac Lab UI window opens with the button "Record Animation" in the toolbar. -Clicking this button starts recording the animation of the simulation. On clicking the button again, the -recording stops. The recorded animation and the original stage (with all physics disabled) are saved -to the ``recordings`` folder in the current working directory. The files are stored in the ``usd`` format: +The following files are saved to the ``recordings/`` folder: -- ``Stage.usd``: The original stage with all physics disabled -- ``TimeSample_tk001.usd``: The timeSampled file containing the recorded animation +- ``Stage.usd`` โ€” the original stage with physics disabled +- ``TimeSample_tk001.usd`` โ€” the animation (time-sampled) layer -You can open Omniverse Isaac Sim application to play back the animation. There are many ways to launch -the application (such as from terminal or `Omniverse Launcher`_). Here we use the terminal to open the -application and play the animation. +To play back: .. code-block:: bash - ./isaaclab.sh -s # Opens Isaac Sim application through _isaac_sim/isaac-sim.sh + ./isaaclab.sh -s # Opens Isaac Sim + +Inside the Layers panel, insert both ``Stage.usd`` and ``TimeSample_tk001.usd`` as sublayers. +The animation will now play back when you hit the play button. + +See the `tutorial on layering in Omniverse`_ for more on working with layers. + + +OVD Recorder +------------ + +The OVD Recorder uses OmniPVD to record simulation data and bake it directly into a new USD stage. +This method is more scalable and better suited for large-scale training scenarios (e.g. multi-env RL). + +Itโ€™s not UI-controlledโ€”the whole process is enabled through CLI flags and runs automatically. + + +Workflow Summary +~~~~~~~~~~~~~~~~ + +1. User runs Isaac Lab with animation recording enabled via CLI +2. Isaac Lab starts simulation +3. OVD data is recorded as the simulation runs +4. At the specified stop time, the simulation is baked into an outputted USD file, and IsaacLab is closed +5. The final result is a fully baked, self-contained USD animation + +Example Usage +~~~~~~~~~~~~~ + +To record an animation: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/tutorials/03_envs/run_cartpole_rl_env.py \ + --anim_recording_enabled \ + --anim_recording_start_time 1 \ + --anim_recording_stop_time 3 + +.. note:: + + The provided ``--anim_recording_stop_time`` should be greater than the simulation time. + +.. warning:: + + Currently, the final recording step can output many warning logs from [omni.usd]. This is a known issue, and these warning messages can be ignored. + +After the stop time is reached, a file will be saved to: + +.. code-block:: none -On a new stage, add the ``Stage.usd`` as a sublayer and then add the ``TimeSample_tk001.usd`` as a sublayer. -You can do this by dragging and dropping the files from the file explorer to the stage. Please check out -the `tutorial on layering in Omniverse`_ for more details. + anim_recordings//baked_animation_recording.usda -You can then play the animation by pressing the play button. .. _Stage Recorder: https://docs.omniverse.nvidia.com/extensions/latest/ext_animation_stage-recorder.html .. _Fabric: https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/usd_fabric_usdrt.html diff --git a/docs/source/how-to/simulation_performance.rst b/docs/source/how-to/simulation_performance.rst new file mode 100644 index 000000000000..3dd113a1285d --- /dev/null +++ b/docs/source/how-to/simulation_performance.rst @@ -0,0 +1,72 @@ +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: + +1. **Use Headless Mode**: Running the simulation in headless mode can significantly improve performance, especially + when rendering is not required. You can enable headless mode by using the ``--headless`` flag when running the + simulator. +2. **Avoid Unnecessary Collisions**: If possible, reduce the number of object overlaps to reduce overhead in the simulation. + Excessive contacts and collisions in the simulation can be expensive in the collision phase in the simulation. +3. **Use Simplified Physics**: Consider using simplified physics collision geometries or lowering simulation fidelity + for better performance. This can be done by modifying the assets and adjusting the physics parameters in the simulation configuration. +4. **Use CPU/GPU Simulation**: If your scene consists of just a few articulations or rigid bodies, consider using CPU simulation + for better performance. For larger scenes, using GPU simulation can significantly improve performance. + +Collision Geometries +-------------------- + +Collision geometries are used to define the shape of objects in the simulation for collision detection. Using +simplified collision geometries can improve performance and reduce the complexity of the simulation. + +For example, if you have a complex mesh, you can create a simplified collision geometry that approximates the shape +of the mesh. This can be done in Isaac Sim through the UI by modifying the collision mesh and approximation methods. + +Additionally, we can often remove collision geometries on areas of the robot that are not important for training. +In the Anymal-C robot, we keep the collision geometries for the kneeds and feet, but remove the collision geometries +on other parts of the legs to optimize for performance. + +Simpler collision geometries such as primitive shapes like spheres will also yield better performance than complex meshes. +For example, an SDF mesh collider will be more expensive than a simple sphere. + +Note that cylinder and cone collision geometries have special support for smooth collisions with triangle meshes for +better wheeled simulation behavior. This comes at a cost of performance and may not always be desired. To disable this feature, +we can set the stage settings ``--/physics/collisionApproximateCylinders=true`` and ``--/physics/collisionApproximateCones=true``. + +Another item to watch out for in GPU RL workloads is warnings about GPU compatibility of ``Convex Hull`` approximated mesh collision geometry. +If the input mesh has a high aspect ratio (e.g. a long thin shape), the convex hull approximation may be incompatible with GPU simulation, +triggering a CPU fallback that can significantly impact performance. + +A CPU-fallback warning looks as follows: ``[Warning] [omni.physx.cooking.plugin] ConvexMeshCookingTask: failed to cook GPU-compatible mesh, +collision detection will fall back to CPU. Collisions with particles and deformables will not work with this mesh.``. +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/how-to/write_articulation_cfg.rst b/docs/source/how-to/write_articulation_cfg.rst index 90db3232e47b..d681f281473b 100644 --- a/docs/source/how-to/write_articulation_cfg.rst +++ b/docs/source/how-to/write_articulation_cfg.rst @@ -17,7 +17,8 @@ properties of an :class:`~assets.Articulation` in Isaac Lab. We will use the Cartpole example to demonstrate how to create an :class:`~assets.ArticulationCfg`. The Cartpole is a simple robot that consists of a cart with a pole attached to it. The cart -is free to move along a rail, and the pole is free to rotate about the cart. +is free to move along a rail, and the pole is free to rotate about the cart. The file for this configuration example is +``source/isaaclab_assets/isaaclab_assets/robots/cartpole.py``. .. dropdown:: Code for Cartpole configuration :icon: code @@ -114,3 +115,154 @@ to combine them into a single actuator model. damping={"slider_to_cart": 10.0, "cart_to_pole": 0.0}, ), }, + + +ActuatorCfg velocity/effort limits considerations +------------------------------------------------- + +In IsaacLab v1.4.0, the plain ``velocity_limit`` and ``effort_limit`` attributes were **not** consistently +pushed into the physics solver: + +- **Implicit actuators** + - velocity_limit was ignored (never set in simulation) + - effort_limit was set into simulation + +- **Explicit actuators** + - both velocity_limit and effort_limit were used only by the drive model, not by the solver + + +In v2.0.1 we accidentally changed this: all velocity_limit & effort_limit, implicit or +explicit, were being applied to the solver. That caused many training under the old default uncaped solver +limits to break. + +To restore the original behavior while still giving users full control over solver limits, we introduced two new flags: + +* **velocity_limit_sim** + Sets the physics-solver's maximum joint-velocity cap in simulation. + +* **effort_limit_sim** + Sets the physics-solver's maximum joint-effort cap in simulation. + + +These explicitly set the solver's joint-velocity and joint-effort caps at simulation level. + +On the other hand, velocity_limit and effort_limit model the motor's hardware-level constraints in torque +computation for all explicit actuators rather than limiting simulation-level constraint. +For implicit actuators, since they do not model motor hardware limitations, ``velocity_limit`` were removed in v2.1.1 +and marked as deprecated. This preserves same behavior as they did in v1.4.0. Eventually, ``velocity_limit`` and +``effort_limit`` will be deprecated for implicit actuators, preserving only ``velocity_limit_sim`` and +``effort_limit_sim`` + + +.. table:: Limit Options Comparison + + .. list-table:: + :header-rows: 1 + :widths: 20 40 40 + + * - **Attribute** + - **Implicit Actuator** + - **Explicit Actuator** + * - ``velocity_limit`` + - Deprecated (alias for ``velocity_limit_sim``) + - Used by the model (e.g. DC motor), not set into simulation + * - ``effort_limit`` + - Deprecated (alias for ``effort_limit_sim``) + - Used by the model, not set into simulation + * - ``velocity_limit_sim`` + - Set into simulation + - Set into simulation + * - ``effort_limit_sim`` + - Set into simulation + - Set into simulation + + + +Users who want to tune the underlying physics-solver limits should set the ``_sim`` flags. + + +USD vs. ActuatorCfg discrepancy resolution +------------------------------------------ + +USD having default value and the fact that ActuatorCfg can be specified with None, or a overriding value can sometime be +confusing what exactly gets written into simulation. The resolution follows these simple rules,per joint and per +property: + +.. table:: Resolution Rules for USD vs. ActuatorCfg + + +------------------------+------------------------+--------------------+ + | **Condition** | **ActuatorCfg Value** | **Applied** | + +========================+========================+====================+ + | No override provided | Not Specified | USD Value | + +------------------------+------------------------+--------------------+ + | Override provided | User's ActuatorCfg | Same as ActuatorCfg| + +------------------------+------------------------+--------------------+ + + +Digging into USD can sometime be unconvinent, to help clarify what exact value is written, we designed a flag +:attr:`~isaaclab.assets.ArticulationCfg.actuator_value_resolution_debug_print`, +to help user figure out what exact value gets used in simulation. + +Whenever an actuator parameter is overridden in the user's ActuatorCfg (or left unspecified), +we compare it to the value read from the USD definition and record any differences. For each joint and each property, +if unmatching value is found, we log the resolution: + + 1. **USD Value** + The default limit or gain parsed from the USD asset. + + 2. **ActuatorCfg Value** + The user-provided override (or โ€œNot Specifiedโ€ if none was given). + + 3. **Applied** + The final value actually used for simulation: if the user didn't override it, this matches the USD value; + otherwise it reflects the user's setting. + +This resolution info is emitted as a warning table only when discrepancies exist. +Here's an example of what you'll see:: + + +----------------+--------------------+---------------------+----+-------------+--------------------+----------+ + | Group | Property | Name | ID | USD Value | ActuatorCfg Value | Applied | + +----------------+--------------------+---------------------+----+-------------+--------------------+----------+ + | panda_shoulder | velocity_limit_sim | panda_joint1 | 0 | 2.17e+00 | Not Specified | 2.17e+00 | + | | | panda_joint2 | 1 | 2.17e+00 | Not Specified | 2.17e+00 | + | | | panda_joint3 | 2 | 2.17e+00 | Not Specified | 2.17e+00 | + | | | panda_joint4 | 3 | 2.17e+00 | Not Specified | 2.17e+00 | + | | stiffness | panda_joint1 | 0 | 2.29e+04 | 8.00e+01 | 8.00e+01 | + | | | panda_joint2 | 1 | 2.29e+04 | 8.00e+01 | 8.00e+01 | + | | | panda_joint3 | 2 | 2.29e+04 | 8.00e+01 | 8.00e+01 | + | | | panda_joint4 | 3 | 2.29e+04 | 8.00e+01 | 8.00e+01 | + | | damping | panda_joint1 | 0 | 4.58e+03 | 4.00e+00 | 4.00e+00 | + | | | panda_joint2 | 1 | 4.58e+03 | 4.00e+00 | 4.00e+00 | + | | | panda_joint3 | 2 | 4.58e+03 | 4.00e+00 | 4.00e+00 | + | | | panda_joint4 | 3 | 4.58e+03 | 4.00e+00 | 4.00e+00 | + | | armature | panda_joint1 | 0 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_joint2 | 1 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_joint3 | 2 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_joint4 | 3 | 0.00e+00 | Not Specified | 0.00e+00 | + | panda_forearm | velocity_limit_sim | panda_joint5 | 4 | 2.61e+00 | Not Specified | 2.61e+00 | + | | | panda_joint6 | 5 | 2.61e+00 | Not Specified | 2.61e+00 | + | | | panda_joint7 | 6 | 2.61e+00 | Not Specified | 2.61e+00 | + | | stiffness | panda_joint5 | 4 | 2.29e+04 | 8.00e+01 | 8.00e+01 | + | | | panda_joint6 | 5 | 2.29e+04 | 8.00e+01 | 8.00e+01 | + | | | panda_joint7 | 6 | 2.29e+04 | 8.00e+01 | 8.00e+01 | + | | damping | panda_joint5 | 4 | 4.58e+03 | 4.00e+00 | 4.00e+00 | + | | | panda_joint6 | 5 | 4.58e+03 | 4.00e+00 | 4.00e+00 | + | | | panda_joint7 | 6 | 4.58e+03 | 4.00e+00 | 4.00e+00 | + | | armature | panda_joint5 | 4 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_joint6 | 5 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_joint7 | 6 | 0.00e+00 | Not Specified | 0.00e+00 | + | | friction | panda_joint5 | 4 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_joint6 | 5 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_joint7 | 6 | 0.00e+00 | Not Specified | 0.00e+00 | + | panda_hand | velocity_limit_sim | panda_finger_joint1 | 7 | 2.00e-01 | Not Specified | 2.00e-01 | + | | | panda_finger_joint2 | 8 | 2.00e-01 | Not Specified | 2.00e-01 | + | | stiffness | panda_finger_joint1 | 7 | 1.00e+06 | 2.00e+03 | 2.00e+03 | + | | | panda_finger_joint2 | 8 | 1.00e+06 | 2.00e+03 | 2.00e+03 | + | | armature | panda_finger_joint1 | 7 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_finger_joint2 | 8 | 0.00e+00 | Not Specified | 0.00e+00 | + | | friction | panda_finger_joint1 | 7 | 0.00e+00 | Not Specified | 0.00e+00 | + | | | panda_finger_joint2 | 8 | 0.00e+00 | Not Specified | 0.00e+00 | + +----------------+--------------------+---------------------+----+-------------+--------------------+----------+ + +To keep the cleaniness of logging, :attr:`~isaaclab.assets.ArticulationCfg.actuator_value_resolution_debug_print` +default to False, remember to turn it on when wishes. diff --git a/docs/source/migration/comparing_simulation_isaacgym.rst b/docs/source/migration/comparing_simulation_isaacgym.rst new file mode 100644 index 000000000000..e70292f99aad --- /dev/null +++ b/docs/source/migration/comparing_simulation_isaacgym.rst @@ -0,0 +1,503 @@ +.. _migrating-from-isaacgymenvs-comparing-simulation: + +Comparing Simulations Between Isaac Gym and Isaac Lab +===================================================== + + +When migrating simulations from Isaac Gym to Isaac Lab, it is sometimes helpful to compare +the simulation configurations in Isaac Gym and Isaac Lab to identify differences between the two setups. +There may be differences in how default values are interpreted, how the importer treats certain +hierarchies of bodies, and how values are scaled. The only way to be certain that two simulations +are equivalent in the eyes of PhysX is to record a simulation trace of both setups and compare +them by inspecting them side-by-side. This approach works because PhysX is the same underlying +engine for both Isaac Gym and Isaac Lab, albeit with different versions. + + +Recording to PXD2 in Isaac Gym Preview Release +---------------------------------------------- + +Simulation traces in Isaac Gym can be recorded using the built-in PhysX Visual Debugger (PVD) +file output feature. Set the operating system environment variable ``GYM_PVD_FILE`` to the +desired output file path; the ``.pxd2`` file extension will be appended automatically. + +For detailed instructions, refer to the tuning documentation included with Isaac Gym: + +.. code-block:: text + + isaacgym/docs/_sources/programming/tuning.rst.txt + +.. note:: + + This file reference is provided because Isaac Gym does not have its documentation available online. + + +Recording to OVD in Isaac Lab +----------------------------- + +To record an OVD simulation trace file in Isaac Lab, you must set the appropriate Isaac Sim Kit +arguments. It is important that the ``omniPvdOvdRecordingDirectory`` variable is set **before** +``omniPvdOutputEnabled`` is set to ``true``. + +.. code-block:: bash + + ./isaaclab.sh -p scripts/benchmarks/benchmark_non_rl.py --task \ + --kit_args="--/persistent/physics/omniPvdOvdRecordingDirectory=/tmp/myovds/ \ + --/physics/omniPvdOutputEnabled=true" + +This example outputs a series of OVD files to the ``/tmp/myovds/`` directory. + +If the ``--kit_args`` argument does not work in your particular setup, you can set the Kit arguments +manually by editing the following file directly within the Isaac Sim source code: + +.. code-block:: text + + source/extensions/isaacsim.simulation_app/isaacsim/simulation_app/simulation_app.py + +Append the following lines after the ``args = []`` block: + +.. code-block:: python + + args.append("--/persistent/physics/omniPvdOvdRecordingDirectory=/path/to/output/ovds/") + args.append("--/physics/omniPvdOutputEnabled=true") + + +Inspecting PXD2 and OVD Files +----------------------------- + +By opening the PXD2 file in a PVD viewer and the OVD file in OmniPVD (a Kit extension), you can +manually compare the two simulation runs and their respective parameters. + +**PhysX Visual Debugger (PVD) for PXD2 Files** + +Download the PVD viewer from the NVIDIA Developer Tools page: + + ``_ + +Both version 2 and version 3 of the PVD viewer are compatible with PXD2 files. + +**OmniPVD for OVD Files** + +To view OVD files, enable the OmniPVD extension in the Isaac Sim application. For detailed +instructions, refer to the OmniPVD developer guide: + + https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/extensions/ux/source/omni.physx.pvd/docs/dev_guide/physx_visual_debugger.html + +**Inspecting Contact Gizmos in OmniPVD** + +To inspect contact points between objects, enable the contact gizmos in OmniPVD. Ensure that the +simulation frame is set to **PRE** (pre-simulation frames of each simulation step) in the OmniPVD +timeline, or set the replay mode to **PRE**. This allows you to visualize contact information before +the solver processes each step. + +**Comparing PVD and OVD Files** + +Using the PVD viewer and the OmniPVD extension, you can now compare the simulations side-by-side +to identify configuration differences. On the left is PVD for PXD2 inspection and on the right is the OmniPVD +extension loaded to inspect OVD files. + +.. image:: ../_static/migration/ovd_pvd_comparison.jpg + + +Parameters to Verify During Simulation Comparison +------------------------------------------------- + +For PhysX articulations, each attribute is useful to inspect because it reveals how the link or shape +will actually behave in contact, under drives, and at constraints. Below, each attribute is expanded +with why it matters for debugging and tuning simulations. + + +PxArticulationLink +^^^^^^^^^^^^^^^^^^ + +Each link behaves like a rigid body with mass properties, damping, velocity limits, and contact-resolution +limits. Inspecting these helps explain stability issues, jitter, and odd responses to forces. + +Mass Properties +""""""""""""""" + +**Mass** + Determines how strongly the link accelerates under forces and how it shares impulses in collisions + and joint constraints. + + *When to inspect:* Understand why a link seems "too heavy" (barely moves when pushed) or "too light" + (flies around from small impulses), and to detect inconsistent mass distribution across a chain that + can cause unrealistic motion or joint stress. + +**Center of Mass (pose)** + Controls where forces effectively act and how the link balances. + + *When to inspect:* A character or mechanism tips over unexpectedly or feels unbalanced; an offset COM + can cause unrealistic torque for the same contact. + +**Inertia Tensor / Inertia Scale** + Defines rotational resistance about each axis. + + *When to inspect:* Links are too easy or too hard to spin relative to their mass, which affects joint + drive tuning and impact responses. + +Damping Properties +"""""""""""""""""" + +**Linear Damping** + Models velocity-proportional drag on translation; higher values make links lose linear speed faster. + + *When to inspect:* Links slide too far (damping too low) or feel "underwater" (damping too high), or + when articulation energy seems to vanish without obvious contact. + +**Angular Damping** + Models drag on rotation; higher values make spinning links slow more quickly. + + *When to inspect:* Links keep spinning after impacts or motor drives (too low), or joints feel "sticky" + and fail to swing freely under gravity (too high). + +Velocity Properties +""""""""""""""""""" + +**Linear Velocity** + Instantaneous world-space translational velocity of the link. + + *When to inspect:* Verify whether joint motors, gravity, or contacts are generating expected motion, + detect numerical explosions (huge spikes), and correlate with CCD thresholds and max linear velocity clamping. + +**Angular Velocity** + Instantaneous world-space rotational velocity. + + *When to inspect:* Verify joint drives, impacts, or constraints are producing the correct rotation; + spot runaway spin that can cause instability or tunneling before clamping takes effect. + +**Max Linear Velocity** + Upper bound PhysX uses to clamp linear speed before solving, intended to prevent numerical issues + from extremely fast motion. + + *When to inspect:* Objects start tunneling or simulations explode at high speeds. If too high, links + can move too far in one step; too low, they may appear unnaturally capped like "speed-limited" robots. + +**Max Angular Velocity** + Upper bound for angular speed; PhysX clamps angular velocity similarly to linear velocity. + + *When to inspect:* Links spin unrealistically fast after collisions or drives (value too large), or + rotation looks unnaturally limited, especially for wheels or rotors that should rotate quickly (value too small). + +Contact Resolution Properties +""""""""""""""""""""""""""""" + +**Max Depenetration Velocity** + Limits how much corrective velocity the solver may add in one step to resolve penetrations at contacts. + + *When to inspect:* Overlapping links "explode" outward or jitter after starting interpenetrating (too high), + or embedded links separate too slowly and appear stuck together (too low). + +**Max Contact Impulse** + Caps the impulse the solver can apply at contacts; per-body limit, with the actual contact limit being + the minimum of the two bodies' values. + + *When to inspect:* Contacts feel too soft (bodies interpenetrate deeply or sink into the environment) or + too rigid (sharp impulses causing ringing or bouncing), or when tuning "soft collisions" like rubber or + skin-like surfaces. + +State and Behavior Flags +"""""""""""""""""""""""" + +**Kinematic vs Dynamic flag / Disable gravity** + Indicates whether a link is driven kinematically or fully simulated, and whether gravity affects it. + + *When to inspect:* Parts appear frozen, snap directly to poses, or ignore gravity, which can drastically + change articulation behavior. + +**Sleep thresholds (linear, angular) and wake counter** + Control when a link is allowed to go to sleep and stop simulating. + + *When to inspect:* Articulations sleep too early (stopping motion) or never sleep (wasting performance + and causing low-amplitude jitter). + + +PxArticulationJoint +^^^^^^^^^^^^^^^^^^^ + +The inbound joint defines relative motion between a link and its parent. Inspecting motion and related +parameters explains limits, constraints, and how drives shape articulation pose and stability. + +Joint Configuration +""""""""""""""""""" + +**Motion** + Per-axis setting (locked, limited, free) that defines which degrees of freedom the joint allows and + whether ranges are restricted. + + *When to inspect:* A link moves in an unexpected direction (axis wrongly set to free), hits a hard stop + sooner or later than expected (limit vs locked), or seems unconstrained because an axis is mistakenly left free. + +**Joint Type / Axes definition** + Choice of revolute, prismatic, spherical, etc., and the local joint frames that define axes. + + *When to inspect:* A "hinge" behaves more like a ball joint or slides unexpectedly; incorrect type or + frame alignment easily produces weird motions. + +**Limits (swing, twist, linear)** + Specify allowed angular or linear ranges and often include stiffness/damping. + + *When to inspect:* Joints hyper-extend, clip through geometry, or suddenly snap at boundaries; mis-set + limits cause popping and instability. + +Drive Properties +"""""""""""""""" + +**Drive target position (orientation) and target velocity** + Desired relative pose and relative velocity that drives the articulation, often using spring-damper models. + + *When to inspect:* Controllers are too slow or overshoot and oscillateโ€”target values and drive parameters + must match link mass and inertia. + +**Drive stiffness and damping (spring strength, tangential damping)** + Control how aggressively the joint tries to reach the target pose and how much overshoot is damped. + + *When to inspect:* Joints buzz or oscillate under load (stiffness high, damping low) or feel unresponsive + and "rubbery" (stiffness low). + +**Joint friction / resistance (if configured)** + Adds resistance even without explicit damping in drives. + + *When to inspect:* Passive joints keep swinging too long, or appear stuck even without drives. + + +PxShape +^^^^^^^ + +Shapes attached to links determine collision representation and contact behavior. Even if they are internal +in OmniPhysics, their properties have a strong impact on stability, contact timing, and visual alignment. + +Collision Offsets +""""""""""""""""" + +**Rest Offset** + Distance at which two shapes come to rest; sum of their rest offsets defines the separation where they "settle". + + *When to inspect:* Graphics and collision appear misaligned (gaps or visible intersections), or sliding + over meshes is rough. Small positive offsets can smooth sliding, while zero offset tends to align exactly + but may catch on geometry. + +**Contact Offset** + Distance at which contact generation begins; shapes whose distance is less than the sum of contact offsets + generate contacts. + + *When to inspect:* Contacts appear "too early" (objects seem to collide before visually touching, increasing + contact count) or "too late" (tunneling or jitter). The difference between contact and rest offsets is + crucial for predictive, stable contacts. + +Geometry and Materials +"""""""""""""""""""""" + +**Geometry type and dimensions** + Box, sphere, capsule, convex, mesh, and the associated size parameters. + + *When to inspect:* Collision footprint does not match the visual meshโ€”overly large shapes cause premature + contacts; small shapes allow visual intersection and change leverage at contacts. + +**Material(s): friction, restitution, compliance** + Friction coefficients and restitution define sliding and bounciness. + + *When to inspect:* An articulation foot skids too easily, sticks to the ground, or bounces unexpectedly. + Wrong materials can make mechanisms unstable or unresponsive. + +Shape Flags +""""""""""" + +**Flag for simulation / query / trigger** + Whether the shape participates in simulation contacts, raycasts only, or trigger events. + + *When to inspect:* Contacts do not appear (shape set as query only) or triggers unexpectedly create + physical collisions. + +**Contact density (CCD flags, if used)** + Continuous collision detection flags affecting how fast-moving links are handled. + + *When to inspect:* Fast articulation parts tunnel through thin obstacles, or CCD is too aggressive and + reduces performance. + + +PxRigidDynamic +^^^^^^^^^^^^^^ + +``PxRigidDynamic`` is the core simulated rigid body type in PhysX, so inspecting its attributes is crucial +for understanding individual object behavior, stability, and performance in the scene. Many attributes +mirror ``PxArticulationLink``, but a rigid dynamic is not constrained by articulation joints and can also +be used in kinematic mode. + +Mass and Mass-Related Properties +"""""""""""""""""""""""""""""""" + +**Mass** + Controls translational response to forces and impulses; for the same impulse, lower mass gives higher + velocity change. + + *When to inspect:* An object barely reacts to hits (mass too large) or flies away from small forces + (mass too small), or mass ratios between interacting bodies cause overly dominant or easily bullied bodies. + +**Center of Mass (COM) pose** + Defines where forces effectively act and around which point the body rotates. + + *When to inspect:* Objects tip over unexpectedly, roll in unintuitive ways, or feel "unbalanced." A COM + too high or off-center can cause strong torques from small contacts. + +**Inertia tensor / inertia scaling** + Determines resistance to angular acceleration around each axis for a given torque. + + *When to inspect:* Bodies are too easy or too hard to spin (e.g., a large object spinning quickly from + small hits), or when anisotropic behavior is needed (e.g., wheels that spin easily around one axis but + resist others). + +Damping and Velocity Limits +""""""""""""""""""""""""""" + +**Linear Damping** + Adds velocity-proportional drag on translation. + + *When to inspect:* Bodies slide too far or for too long (damping too low) or appear as if moving through + thick fluid (damping too high), and when scenes lose energy faster than friction alone would suggest. + +**Angular Damping** + Adds drag on rotation, reducing angular velocity over time. + + *When to inspect:* Spinning objects never settle or spin unrealistically long (too low), or they stop + rotating almost immediately after impact or motor impulses (too high). + +**Linear Velocity** + Current translational velocity used by the integrator and solver. + + *When to inspect:* Debug impulses, gravity, or applied forces to see whether the body is accelerating + as expected; detect spikes or non-physical jumps in speed. + +**Angular Velocity** + Current rotational speed around each axis. + + *When to inspect:* Rotations look jittery, explode numerically, or fail to respond to applied torques. + High values relative to time step and object scale can indicate instability. + +**Max Linear Velocity** + Upper bound used to clamp linear velocity before solving. + + *When to inspect:* Very fast bodies cause tunneling or simulation explosions (value too high), or they + appear unnaturally "speed-limited," especially projectiles or debris in high-energy scenes (value too low). + +**Max Angular Velocity** + Upper bound used to clamp angular velocity. + + *When to inspect:* Thin or small bodies spin so fast they destabilize the scene (value too high), or + spinning elements such as wheels, propellers, or debris appear artificially capped (value too low). + +Contact Resolution and Impulses +""""""""""""""""""""""""""""""" + +**Max Depenetration Velocity** + Limits the corrective velocity the solver may introduce in one step to resolve interpenetrations. + + *When to inspect:* Intersecting bodies "explode" apart or jitter violently after overlap (too high), or + separate very slowly and appear stuck or interpenetrated for several frames (too low). + +**Max Contact Impulse** + Caps the impulse that can be applied at contacts involving this body; the effective limit is the minimum + between the two bodies, or the dynamic body for staticโ€“dynamic contacts. + + *When to inspect:* Create softer contacts (lower limit) or very rigid, almost unyielding bodies (high or + default limit); objects sink into each other or bounce unrealistically. + +Sleep and Activation Behavior +""""""""""""""""""""""""""""""" + +**Sleep Threshold** + Mass-normalized kinetic energy below which a body becomes a candidate for sleeping. + + *When to inspect:* Bodies fall asleep too early while they should still move (threshold too high) or + constantly jitter and never sleep (threshold too low), which can hurt performance. + +**Wake Counter / isSleeping flag** + Internal timer and state indicating whether the body is active. + + *When to inspect:* Bodies refuse to wake up on interactions or wake too easily. Bad sleep behavior can + make scenes feel "dead" or too noisy. + +Kinematic Mode and Locking +"""""""""""""""""""""""""" + +**Kinematic Flag (PxRigidBodyFlag::eKINEMATIC)** + When set, the body is moved by ``setKinematicTarget`` and ignores forces and gravity, while still + affecting dynamic bodies it touches. + + *When to inspect:* Objects appear to have infinite mass (pushing others but not reacting) or ignore + gravity and impulses. Mismatched expectations here commonly cause odd behavior in characters, moving + platforms, or doors. + +**Rigid Dynamic Lock Flags (PxRigidDynamicLockFlag)** + Per-axis linear and angular DOF locks, effectively constraining motion without a joint. + + *When to inspect:* Bodies unexpectedly move in constrained directions (lock not set) or fail to + move/rotate where they should (lock set by mistake), especially for 2D-style movement or simple + constrained mechanisms. + +**Disable Gravity (PxActorFlag::eDISABLE_GRAVITY)** + Toggles whether the body is affected by scene gravity. + + *When to inspect:* Objects float in mid-air or drop unexpectedly. A common source of confusion in + mixed setups with some gravity-less bodies. + +Forces and Solver Overrides +""""""""""""""""""""""""""" + +**Applied force and torque (accumulated per step)** + Net forces/torques that will be integrated into velocity. + + *When to inspect:* Debug gameplay forces (thrusters, character pushes, explosions) to see if the expected + input is actually reaching the body. + +**Per-body solver iteration counts (minPositionIters, minVelocityIters)** + Overrides for how many solver iterations this body gets in constraints and contacts. + + *When to inspect:* Certain bodies (e.g., characters, stacked crates, fragile structures) need higher + stability or more accurate stacking. Low iterations can cause jitter and penetration; too high wastes + performance. + +Shape-Related Aspects +""""""""""""""""""""" + +While not properties of ``PxRigidDynamic`` itself, the shapes attached to it heavily influence behavior. + +**Attached Shapes' Rest and Contact Offsets** + Control predictive contact generation and visual separation as described earlier. + + *When to inspect:* A dynamic body seems to collide too early/late or appears to float above surfaces + or intersect them visually. + +**Attached Materials (friction, restitution)** + Define sliding and bounciness for this body's contacts. + + *When to inspect:* Rigid dynamics skid, stick, or bounce in unexpected ways. Often the "behavior issue" + is material configuration rather than mass or damping. + + +Summary: What to Inspect and Why +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The table below summarizes the key inspection areas for each PhysX component: + +.. list-table:: + :header-rows: 1 + :widths: 20 40 40 + + * - Component + - Key Attributes + - Debugging Focus + * - **Links** + - Mass, damping, velocities, limits + - Overall energy, stability, and response to joints/contacts + * - **Joints** + - Motion, limits, drives + - How articulation pose evolves; over/under-constrained motion + * - **Shapes** + - Offsets, materials, geometry + - Contact timing, friction behavior, visual vs physical alignment + * - **Rigid Dynamics** + - Mass, inertia, damping, velocity limits, sleep, kinematic flags + - Acceleration, settling, extreme motion, body state + +All of these attributes together provide a comprehensive picture of why an articulation or rigid body +behaves as it does and where to adjust parameters for stability, realism, or control performance. diff --git a/docs/source/migration/migrating_from_isaacgymenvs.rst b/docs/source/migration/migrating_from_isaacgymenvs.rst index bbe55233eb55..db6371c40c9d 100644 --- a/docs/source/migration/migrating_from_isaacgymenvs.rst +++ b/docs/source/migration/migrating_from_isaacgymenvs.rst @@ -355,6 +355,10 @@ of ``1/deg`` in the Isaac Sim UI but ``1/rad`` in Isaac Gym Preview Release. - 100.0 (rad) +For more details on performing thorough simulation comparisons between Isaac Gym and Isaac Lab, +please refer to the :ref:`migrating-from-isaacgymenvs-comparing-simulation` section. + + Cloner ------ @@ -692,18 +696,18 @@ the need to set simulation parameters for actors in the task implementation. | asset_root = os.path.dirname(asset_path) | actuators={ | | asset_file = os.path.basename(asset_path) | "cart_actuator": ImplicitActuatorCfg( | | | joint_names_expr=["slider_to_cart"], | -| asset_options = gymapi.AssetOptions() | effort_limit=400.0, | -| asset_options.fix_base_link = True | velocity_limit=100.0, | +| asset_options = gymapi.AssetOptions() | effort_limit_sim=400.0, | +| asset_options.fix_base_link = True | velocity_limit_sim=100.0, | | cartpole_asset = self.gym.load_asset(self.sim, | stiffness=0.0, | | asset_root, asset_file, asset_options) | damping=10.0, | | self.num_dof = self.gym.get_asset_dof_count( | ), | | cartpole_asset) | "pole_actuator": ImplicitActuatorCfg( | -| | joint_names_expr=["cart_to_pole"], effort_limit=400.0, | -| pose = gymapi.Transform() | velocity_limit=100.0, stiffness=0.0, damping=0.0 | -| if self.up_axis == 'z': | ), | -| pose.p.z = 2.0 | }, | -| pose.r = gymapi.Quat(0.0, 0.0, 0.0, 1.0) | ) | -| else: | | +| | joint_names_expr=["cart_to_pole"], | +| pose = gymapi.Transform() | effort_limit_sim=400.0, velocity_limit_sim=100.0, | +| if self.up_axis == 'z': | stiffness=0.0, damping=0.0 | +| pose.p.z = 2.0 | ), | +| pose.r = gymapi.Quat(0.0, 0.0, 0.0, 1.0) | }, | +| else: | ) | | pose.p.y = 2.0 | | | pose.r = gymapi.Quat( | | | -np.sqrt(2)/2, 0.0, 0.0, np.sqrt(2)/2) | | @@ -924,6 +928,15 @@ To launch inferencing in Isaac Lab, use the command: python scripts/reinforcement_learning/rl_games/play.py --task=Isaac-Cartpole-Direct-v0 --num_envs=25 --checkpoint= +Additional Resources +~~~~~~~~~~~~~~~~~~~~ + +.. toctree:: + :maxdepth: 1 + + comparing_simulation_isaacgym + + .. _IsaacGymEnvs: https://github.com/isaac-sim/IsaacGymEnvs .. _Isaac Gym Preview Release: https://developer.nvidia.com/isaac-gym .. _release notes: https://github.com/isaac-sim/IsaacLab/releases diff --git a/docs/source/migration/migrating_from_omniisaacgymenvs.rst b/docs/source/migration/migrating_from_omniisaacgymenvs.rst index f33fabd2bd44..b3a46f0a518f 100644 --- a/docs/source/migration/migrating_from_omniisaacgymenvs.rst +++ b/docs/source/migration/migrating_from_omniisaacgymenvs.rst @@ -295,8 +295,8 @@ including file path, simulation parameters, actuator properties, and initial sta actuators={ "cart_actuator": ImplicitActuatorCfg( joint_names_expr=["slider_to_cart"], - effort_limit=400.0, - velocity_limit=100.0, + effort_limit_sim=400.0, + velocity_limit_sim=100.0, stiffness=0.0, damping=10.0, ), diff --git a/docs/source/overview/core-concepts/actuators.rst b/docs/source/overview/core-concepts/actuators.rst index 4f8748300363..de34e4202868 100644 --- a/docs/source/overview/core-concepts/actuators.rst +++ b/docs/source/overview/core-concepts/actuators.rst @@ -35,6 +35,7 @@ maximum effort: .. math:: \tau_{j, computed} & = k_p * (q_{des} - q) + k_d * (\dot{q}_{des} - \dot{q}) + \tau_{ff} \\ + \tau_{j, max} & = \gamma \times \tau_{motor, max} \\ \tau_{j, applied} & = clip(\tau_{computed}, -\tau_{j, max}, \tau_{j, max}) @@ -75,3 +76,25 @@ The following figure shows the actuator groups for a legged mobile manipulator: We provide implementations for various explicit actuator models. These are detailed in `isaaclab.actuators <../../api/lab/isaaclab.actuators.html>`_ sub-package. + +Considerations when using actuators +----------------------------------- + +As explained in the previous sections, there are two main types of actuator models: implicit and explicit. +The implicit actuator model is provided by the physics engine. This means that when the user sets either +a desired position or velocity, the physics engine will internally compute the efforts that need to be +applied to the joints to achieve the desired behavior. In PhysX, the PD controller adds numerical damping +to the desired effort, which results in more stable behavior. + +The explicit actuator model is provided by the user. This means that when the user sets either a desired +position or velocity, the user's model will compute the efforts that need to be applied to the joints to +achieve the desired behavior. While this provides more flexibility, it can also lead to some numerical +instabilities. One way to mitigate this is to use the ``armature`` parameter of the actuator model, either in +the USD file or in the articulation config. This parameter is used to dampen the joint response and helps +improve the numerical stability of the simulation. More details on how to improve articulation stability +can be found in the `OmniPhysics documentation `_. + +What does this mean for the user? It means that policies trained with implicit actuators may not transfer +to the exact same robot model when using explicit actuators. If you are running into issues like this, or +in cases where policies do not converge on explicit actuators while they do on implicit ones, increasing +or setting the ``armature`` parameter to a higher value may help. diff --git a/docs/source/overview/core-concepts/sensors/camera.rst b/docs/source/overview/core-concepts/sensors/camera.rst index d5b0018940fc..6a34c6fab30c 100644 --- a/docs/source/overview/core-concepts/sensors/camera.rst +++ b/docs/source/overview/core-concepts/sensors/camera.rst @@ -157,14 +157,3 @@ Instance Segmentation - If ``colorize_instance_segmentation=False``, a buffer of dimension (B, H, W, 1) of type ``torch.int32`` will be returned, containing the instance ID of each pixel. The info ``idToLabels`` dictionary will be the mapping from color to USD prim path of that semantic entity. The info ``idToSemantics`` dictionary will be the mapping from color to semantic labels of that semantic entity. - - -Current Limitations -------------------- - -For performance reasons, we default to using DLSS for denoising, which generally provides better performance. -This may result in renders of lower quality, which may be especially evident at lower resolutions. -Due to this, we recommend using per-tile or per-camera resolution of at least 100 x 100. -For renders at lower resolutions, we advice setting the ``antialiasing_mode`` attribute in :class:`~sim.RenderCfg` to -``DLAA``, and also potentially enabling ``enable_dl_denoiser``. Both of these settings should help improve render -quality, but also comes at a cost of performance. Additional rendering parameters can also be specified in :class:`~sim.RenderCfg`. diff --git a/docs/source/overview/core-concepts/sensors/contact_sensor.rst b/docs/source/overview/core-concepts/sensors/contact_sensor.rst index b1cee5241a34..4ddd2d10c077 100644 --- a/docs/source/overview/core-concepts/sensors/contact_sensor.rst +++ b/docs/source/overview/core-concepts/sensors/contact_sensor.rst @@ -72,7 +72,14 @@ Here, we print both the net contact force and the filtered force matrix for each Received force matrix of: tensor([[[[0., 0., 0.]]]], device='cuda:0') Received contact force of: tensor([[[1.3529e-05, 0.0000e+00, 1.0069e+02]]], device='cuda:0') -Notice that even with filtering, both sensors report the net contact force acting on the foot. However only the left foot has a non zero "force matrix", because the right foot isn't standing on the filtered body, ``/World/envs/env_.*/Cube``. Now, checkout the data coming from the hind feet! + +.. figure:: ../../../_static/overview/sensors/contact_visualization.jpg + :align: center + :figwidth: 100% + :alt: The contact sensor visualization + + +Notice that even with filtering, both sensors report the net contact force acting on the foot. However, the "force matrix" on the right foot is zero because that foot isn't in contact with the filtered body, ``/World/envs/env_.*/Cube``. Now, checkout the data coming from the hind feet! .. code-block:: bash diff --git a/docs/source/overview/core-concepts/sensors/index.rst b/docs/source/overview/core-concepts/sensors/index.rst index 31baaa9258b1..d2c63f212b76 100644 --- a/docs/source/overview/core-concepts/sensors/index.rst +++ b/docs/source/overview/core-concepts/sensors/index.rst @@ -19,3 +19,4 @@ The following pages describe the available sensors in more detail: frame_transformer imu ray_caster + visuo_tactile_sensor diff --git a/docs/source/overview/core-concepts/sensors/visuo_tactile_sensor.rst b/docs/source/overview/core-concepts/sensors/visuo_tactile_sensor.rst new file mode 100644 index 000000000000..d761c4a2e8a8 --- /dev/null +++ b/docs/source/overview/core-concepts/sensors/visuo_tactile_sensor.rst @@ -0,0 +1,205 @@ +.. _overview_sensors_tactile: + +.. currentmodule:: isaaclab + +Visuo-Tactile Sensor +==================== + + +The visuo-tactile sensor in Isaac Lab provides realistic tactile feedback through integration with TacSL (Tactile Sensor Learning) [Akinola2025]_. It is designed to simulate high-fidelity tactile interactions, generating both visual and force-based data that mirror real-world tactile sensors like GelSight devices. The sensor can provide tactile RGB images, force field distributions, and other intermediate tactile measurements essential for robotic manipulation tasks requiring fine tactile feedback. + + +.. figure:: ../../../_static/overview/sensors/tacsl_diagram.jpg + :align: center + :figwidth: 100% + :alt: Tactile sensor with RGB visualization and force fields + + +Configuration +~~~~~~~~~~~~~ + +Tactile sensors require specific configuration parameters to define their behavior and data collection properties. The sensor can be configured with various parameters including sensor resolution, force sensitivity, and output data types. + +.. code-block:: python + + from isaaclab.sensors import TiledCameraCfg + from isaaclab_assets.sensors import GELSIGHT_R15_CFG + import isaaclab.sim as sim_utils + + from isaaclab_contrib.sensors.tacsl_sensor import VisuoTactileSensorCfg + + # Tactile sensor configuration + tactile_sensor = VisuoTactileSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer/tactile_sensor", + ## Sensor configuration + render_cfg=GELSIGHT_R15_CFG, + enable_camera_tactile=True, + enable_force_field=True, + ## Elastomer configuration + tactile_array_size=(20, 25), + tactile_margin=0.003, + ## Contact object configuration + contact_object_prim_path_expr="{ENV_REGEX_NS}/contact_object", + ## Force field physics parameters + normal_contact_stiffness=1.0, + friction_coefficient=2.0, + tangential_stiffness=0.1, + ## Camera configuration + camera_cfg=TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer_tip/cam", + update_period=1 / 60, # 60 Hz + height=320, + width=240, + data_types=["distance_to_image_plane"], + spawn=None, # camera already spawned in USD file + ), + ) + +The configuration supports customization of: + +* **Render Configuration**: Specify the GelSight sensor rendering parameters using predefined configs + (e.g., ``GELSIGHT_R15_CFG``, ``GELSIGHT_MINI_CFG`` from ``isaaclab_assets.sensors``) +* **Tactile Modalities**: + * ``enable_camera_tactile`` - Enable tactile RGB imaging through camera sensors + * ``enable_force_field`` - Enable force field computation and visualization +* **Force Field Grid**: Set tactile grid dimensions (``tactile_array_size``) and margins, which directly affects the spatial resolution of the computed force field +* **Contact Object Configuration**: Define properties of interacting objects using prim path expressions to locate objects with SDF collision meshes +* **Physics Parameters**: Control the sensor's force field computation: + * ``normal_contact_stiffness``, ``friction_coefficient``, ``tangential_stiffness`` - Normal stiffness, friction coefficient, and tangential stiffness +* **Camera Settings**: Configure resolution, update rates, and data types, currently only ``distance_to_image_plane`` (alias for ``depth``) is supported. + ``spawn`` is set to ``None`` by default, which means that the camera is already spawned in the USD file. + If you want to spawn the camera yourself and set focal length, etc., you can set the spawn configuration to a valid spawn configuration. + +Configuration Requirements +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. important:: + The following requirements must be satisfied for proper sensor operation: + + **Camera Tactile Imaging** + If ``enable_camera_tactile=True``, a valid ``camera_cfg`` (TiledCameraCfg) must be provided with appropriate camera parameters. + + **Force Field Computation** + If ``enable_force_field=True``, the following parameters are required: + + * ``contact_object_prim_path_expr`` - Prim path expression to locate contact objects with SDF collision meshes + + **SDF Computation** + When force field computation is enabled, penalty-based normal and shear forces are computed using Signed Distance Field (SDF) queries. To achieve GPU acceleration: + + * Interacting objects should have SDF collision meshes + * An SDFView must be defined during initialization, therefore interacting objects should be specified before simulation. + + **Elastomer Configuration** + The sensor's ``prim_path`` must be configured as a child of the elastomer prim in the USD hierarchy. + The query points for the force field computation is computed from the surface of the elastomer mesh, which is searched for under the prim path of the elastomer. + + **Physics Materials** + The sensor uses physics materials to configure the compliant contact properties of the elastomer. + By default, physics material properties are pre-configured in the USD asset. However, you can override + these properties by specifying the following parameters in ``UsdFileWithCompliantContactCfg`` when + spawning the robot: + + * ``compliant_contact_stiffness`` - Contact stiffness for the elastomer surface + * ``compliant_contact_damping`` - Contact damping for the elastomer surface + * ``physics_material_prim_path`` - Prim path where physics material is applied (typically ``"elastomer"``) + + If any parameter is set to ``None``, the corresponding property from the USD asset will be retained. + + +Usage Example +~~~~~~~~~~~~~ + +To use the tactile sensor in a simulation environment, run the demo: + +.. code-block:: bash + + cd scripts/demos/sensors + python tacsl_sensor.py --use_tactile_rgb --use_tactile_ff --tactile_compliance_stiffness 100.0 --tactile_compliant_damping 1.0 --contact_object_type nut --num_envs 16 --save_viz --enable_cameras + +Available command-line options include: + +* ``--use_tactile_rgb``: Enable camera-based tactile sensing +* ``--use_tactile_ff``: Enable force field tactile sensing +* ``--contact_object_type``: Specify the type of contact object (nut, cube, etc.) +* ``--num_envs``: Number of parallel environments +* ``--save_viz``: Save visualization outputs for analysis +* ``--tactile_compliance_stiffness``: Override compliant contact stiffness (default: use USD asset values) +* ``--tactile_compliant_damping``: Override compliant contact damping (default: use USD asset values) +* ``--normal_contact_stiffness``: Normal contact stiffness for force field computation +* ``--tangential_stiffness``: Tangential stiffness for shear forces +* ``--friction_coefficient``: Friction coefficient for shear forces +* ``--debug_sdf_closest_pts``: Visualize closest SDF points for debugging +* ``--debug_tactile_sensor_pts``: Visualize tactile sensor points for debugging +* ``--trimesh_vis_tactile_points``: Enable trimesh-based visualization of tactile points + +For a complete list of available options: + +.. code-block:: bash + + python tacsl_sensor.py -h + +.. note:: + The demo examples are based on the Gelsight R1.5, which is a prototype sensor that is now discontinued. The same procedure can be adapted for other visuotactile sensors. + +.. figure:: ../../../_static/overview/sensors/tacsl_demo.jpg + :align: center + :figwidth: 100% + :alt: TacSL tactile sensor demo showing RGB tactile images and force field visualizations + +The tactile sensor supports multiple data modalities that provide comprehensive information about contact interactions: + + +Output Tactile Data +~~~~~~~~~~~~~~~~~~~ +**RGB Tactile Images** + Real-time generation of tactile RGB images as objects make contact with the sensor surface. These images show deformation patterns and contact geometry similar to gel-based tactile sensors [Si2022]_ + + +**Force Fields** + Detailed contact force field and pressure distributions across the sensor surface, including normal and shear components. + +.. list-table:: + :widths: 50 50 + :class: borderless + + * - .. figure:: ../../../_static/overview/sensors/tacsl_taxim_example.jpg + :align: center + :figwidth: 80% + :alt: Tactile output with RGB visualization + + - .. figure:: ../../../_static/overview/sensors/tacsl_force_field_example.jpg + :align: center + :figwidth: 80% + :alt: Tactile output with force field visualization + +Integration with Learning Frameworks +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The tactile sensor is designed to integrate seamlessly with reinforcement learning and imitation learning frameworks. The structured tensor outputs can be directly used as observations in learning algorithms: + +.. code-block:: python + + def get_tactile_observations(self): + """Extract tactile observations for learning.""" + tactile_data = self.scene["tactile_sensor"].data + + # tactile RGB image + tactile_rgb = tactile_data.tactile_rgb_image + + # tactile depth image + tactile_depth = tactile_data.tactile_depth_image + + # force field + tactile_normal_force = tactile_data.tactile_normal_force + tactile_shear_force = tactile_data.tactile_shear_force + + return [tactile_rgb, tactile_depth, tactile_normal_force, tactile_shear_force] + + + +References +~~~~~~~~~~ + +.. [Akinola2025] Akinola, I., Xu, J., Carius, J., Fox, D., & Narang, Y. (2025). TacSL: A library for visuotactile sensor simulation and learning. *IEEE Transactions on Robotics*. +.. [Si2022] Si, Z., & Yuan, W. (2022). Taxim: An example-based simulation model for GelSight tactile sensors. *IEEE Robotics and Automation Letters*, 7(2), 2361-2368. diff --git a/docs/source/overview/developer-guide/development.rst b/docs/source/overview/developer-guide/development.rst index a88a57f1e88c..48a5019609fa 100644 --- a/docs/source/overview/developer-guide/development.rst +++ b/docs/source/overview/developer-guide/development.rst @@ -1,7 +1,7 @@ Extension Development ======================= -Everything in Omniverse is either an extension, or a collection of extensions (an application). They are +Everything in Omniverse is either an extension or a collection of extensions (an application). They are modularized packages that form the atoms of the Omniverse ecosystem. Each extension provides a set of functionalities that can be used by other extensions or standalone applications. A folder is recognized as an extension if it contains @@ -32,12 +32,12 @@ for the extension with more detailed information about the extension and a CHANG file that contains the changes made to the extension in each version. The ```` directory contains the main python package for the extension. -It may also contains the ``scripts`` directory for keeping python-based applications -that are loaded into Omniverse when then extension is enabled using the +It may also contain the ``scripts`` directory for keeping python-based applications +that are loaded into Omniverse when the extension is enabled using the `Extension Manager `__. More specifically, when an extension is enabled, the python module specified in the -``config/extension.toml`` file is loaded and scripts that contains children of the +``config/extension.toml`` file is loaded and scripts that contain children of the :class:`omni.ext.IExt` class are executed. .. code:: python @@ -60,7 +60,7 @@ More specifically, when an extension is enabled, the python module specified in While loading extensions into Omniverse happens automatically, using the python package in standalone applications requires additional steps. To simplify the build process and -avoiding the need to understand the `premake `__ +avoid the need to understand the `premake `__ build system used by Omniverse, we directly use the `setuptools `__ python package to build the python module provided by the extensions. This is done by the ``setup.py`` file in the extension directory. diff --git a/docs/source/overview/developer-guide/index.rst b/docs/source/overview/developer-guide/index.rst index e77ebc6cc464..59f603fbfad2 100644 --- a/docs/source/overview/developer-guide/index.rst +++ b/docs/source/overview/developer-guide/index.rst @@ -13,4 +13,3 @@ using VSCode. VS Code repo_structure development - template diff --git a/docs/source/overview/developer-guide/repo_structure.rst b/docs/source/overview/developer-guide/repo_structure.rst index 22c5cb518208..a201886c0f8d 100644 --- a/docs/source/overview/developer-guide/repo_structure.rst +++ b/docs/source/overview/developer-guide/repo_structure.rst @@ -5,7 +5,6 @@ Repository organization IsaacLab โ”œโ”€โ”€ .vscode - โ”œโ”€โ”€ .flake8 โ”œโ”€โ”€ CONTRIBUTING.md โ”œโ”€โ”€ CONTRIBUTORS.md โ”œโ”€โ”€ LICENSE diff --git a/docs/source/overview/developer-guide/template.rst b/docs/source/overview/developer-guide/template.rst deleted file mode 100644 index 34e66f3d1f9a..000000000000 --- a/docs/source/overview/developer-guide/template.rst +++ /dev/null @@ -1,92 +0,0 @@ -.. _template-generator: - -Build your Own Project or Task -============================== - -Traditionally, building new projects that utilize Isaac Lab's features required creating your own -extensions within the Isaac Lab repository. However, this approach can obscure project visibility and -complicate updates from one version of Isaac Lab to another. To circumvent these challenges, -we now provide a command-line tool (**template generator**) for creating Isaac Lab-based projects and tasks. - -The template generator enables you to create an: - -* **External project** (recommended): An isolated project that is not part of the Isaac Lab repository. This approach - works outside of the core Isaac Lab repository, ensuring that your development efforts remain self-contained. Also, - it allows your code to be run as an extension in Omniverse. - - .. hint:: - - For the external project, the template generator will initialize a new Git repository in the specified directory. - You can push the generated content to your own remote repository (e.g. GitHub) and share it with others. - -* **Internal task**: A task that is part of the Isaac Lab repository. This approach should only be used to create - new tasks within the Isaac Lab repository in order to contribute to it. - -Running the template generator ------------------------------- - -Install Isaac Lab by following the `installation guide <../../setup/installation/index.html>`_. -We recommend using conda installation as it simplifies calling Python scripts from the terminal. - -Then, run the following command to generate a new external project or internal task: - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code-block:: bash - - ./isaaclab.sh --new # or "./isaaclab.sh -n" - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code-block:: batch - - isaaclab.bat --new :: or "isaaclab.bat -n" - -The generator will guide you in setting up the project/task for your needs by asking you the following questions: - -* Type of project/task (external or internal), and project/task path or names according to the selected type. -* Isaac Lab workflows (see :ref:`feature-workflows`). -* Reinforcement learning libraries (see :ref:`rl-frameworks`), and algorithms (if the selected libraries support multiple algorithms). - -External project usage (once generated) ---------------------------------------- - -Once the external project is generated, a ``README.md`` file will be created in the specified directory. -This file will contain instructions on how to install the project and run the tasks. - -Here are some general commands to get started with it: - -.. note:: - - If Isaac Lab is not installed in a conda environment or in a (virtual) Python environment, use ``FULL_PATH_TO_ISAACLAB/isaaclab.sh -p`` - (or ``FULL_PATH_TO_ISAACLAB\isaaclab.bat -p`` on Windows) instead of ``python`` to run the commands below. - -* Install the project (in editable mode). - - .. code:: bash - - python -m pip install -e source/ - -* List the tasks available in the project. - - .. warning:: - - If the task names change, it may be necessary to update the search pattern ``"Template-"`` - (in the ``scripts/list_envs.py`` file) so that they can be listed. - - .. code:: bash - - python scripts/list_envs.py - -* Run a task. - - .. code:: bash - - python scripts//train.py --task= - -For more details, please follow the instructions in the generated project's ``README.md`` file. diff --git a/docs/source/overview/developer-guide/vs_code.rst b/docs/source/overview/developer-guide/vs_code.rst index f9ea07b6da35..a19889d1bdb8 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 @@ -69,10 +68,25 @@ python executable provided by Omniverse. This is specified in the "python.defaultInterpreterPath": "${workspaceFolder}/_isaac_sim/python.sh", } -If you want to use a different python interpreter (for instance, from your conda environment), +If you want to use a different python interpreter (for instance, from your conda or uv environment), you need to change the python interpreter used by selecting and activating the python interpreter of your choice in the bottom left corner of VSCode, or opening the command palette (``Ctrl+Shift+P``) and selecting ``Python: Select Interpreter``. For more information on how to set python interpreter for VSCode, please refer to the `VSCode documentation `_. + + +Setting up formatting and linting +--------------------------------- + +We use `ruff `_ as a formatter and linter. +These are configured in the ``.vscode/settings.json`` file: + +.. code-block:: json + + { + "ruff.configuration": "${workspaceFolder}/pyproject.toml", + } + +The ruff linter will show warnings and errors in your code to help you follow Python best practices and the project's coding standards. diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index 34ebda6d2523..c1cfb7dcb2c4 100644 --- a/docs/source/overview/environments.rst +++ b/docs/source/overview/environments.rst @@ -3,7 +3,7 @@ Available Environments ====================== -The following lists comprises of all the RL tasks implementations that are available in Isaac Lab. +The following lists comprises of all the RL and IL tasks implementations that are available in Isaac Lab. While we try to keep this list up-to-date, you can always get the latest list of environments by running the following command: @@ -13,16 +13,23 @@ running the following command: .. tab-item:: :icon:`fa-brands fa-linux` Linux :sync: linux + .. note:: + Use ``--keyword `` (optional) to filter environments by keyword. + .. code:: bash - ./isaaclab.sh -p scripts/environments/list_envs.py + ./isaaclab.sh -p scripts/environments/list_envs.py --keyword .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows + .. note:: + Use ``--keyword `` (optional) to filter environments by keyword. + .. code:: batch - isaaclab.bat -p scripts\environments\list_envs.py + isaaclab.bat -p scripts\environments\list_envs.py --keyword + We are actively working on adding more environments to the list. If you have any environments that you would like to add to Isaac Lab, please feel free to open a pull request! @@ -54,7 +61,7 @@ Classic environments that are based on IsaacGymEnvs implementation of MuJoCo-sty | | |cartpole-direct-link| | | +------------------+-----------------------------+-------------------------------------------------------------------------+ | |cartpole| | |cartpole-rgb-link| | Move the cart to keep the pole upwards in the classic cartpole control | - | | | and perceptive inputs | + | | | and perceptive inputs. Requires running with ``--enable_cameras``. | | | |cartpole-depth-link| | | | | | | | | |cartpole-rgb-direct-link| | | @@ -63,7 +70,7 @@ Classic environments that are based on IsaacGymEnvs implementation of MuJoCo-sty +------------------+-----------------------------+-------------------------------------------------------------------------+ | |cartpole| | |cartpole-resnet-link| | Move the cart to keep the pole upwards in the classic cartpole control | | | | based off of features extracted from perceptive inputs with pre-trained | - | | |cartpole-theia-link| | frozen vision encoders | + | | |cartpole-theia-link| | frozen vision encoders. Requires running with ``--enable_cameras``. | +------------------+-----------------------------+-------------------------------------------------------------------------+ .. |humanoid| image:: ../_static/tasks/classic/humanoid.jpg @@ -100,44 +107,105 @@ 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 | - +--------------------+-------------------------+-----------------------------------------------------------------------------+ - | |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 | - +--------------------+-------------------------+-----------------------------------------------------------------------------+ - | |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 | - +--------------------+-------------------------+-----------------------------------------------------------------------------+ + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | 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_openarm_bi| | |reach_openarm_bi-link| | Move the end-effector to sampled target poses with the OpenArm robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |reach_openarm_uni| | |reach_openarm_uni-link| | Move the end-effector to a sampled target pose with the OpenArm robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |lift_openarm_uni| | |lift_openarm_uni-link| | Pick a cube and bring it to a sampled target position with the OpenArm robot| + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |cabi_openarm_uni| | |cabi_openarm_uni-link| | Grasp the handle of a cabinet's drawer and open it with the OpenArm robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ .. |reach-franka| image:: ../_static/tasks/manipulation/franka_reach.jpg .. |reach-ur10| image:: ../_static/tasks/manipulation/ur10_reach.jpg +.. |deploy-reach-ur10e| image:: ../_static/tasks/manipulation/ur10e_reach.jpg .. |lift-cube| image:: ../_static/tasks/manipulation/franka_lift.jpg .. |cabi-franka| image:: ../_static/tasks/manipulation/franka_open_drawer.jpg .. |cube-allegro| image:: ../_static/tasks/manipulation/allegro_cube.jpg .. |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 +.. |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_openarm_bi| image:: ../_static/tasks/manipulation/openarm_bi_reach.jpg +.. |reach_openarm_uni| image:: ../_static/tasks/manipulation/openarm_uni_reach.jpg +.. |lift_openarm_uni| image:: ../_static/tasks/manipulation/openarm_uni_lift.jpg +.. |cabi_openarm_uni| image:: ../_static/tasks/manipulation/openarm_uni_open_drawer.jpg .. |reach-franka-link| replace:: `Isaac-Reach-Franka-v0 `__ .. |reach-ur10-link| replace:: `Isaac-Reach-UR10-v0 `__ +.. |deploy-reach-ur10e-link| replace:: `Isaac-Deploy-Reach-UR10e-v0 `__ .. |lift-cube-link| replace:: `Isaac-Lift-Cube-Franka-v0 `__ .. |lift-cube-ik-abs-link| replace:: `Isaac-Lift-Cube-Franka-IK-Abs-v0 `__ .. |lift-cube-ik-rel-link| replace:: `Isaac-Lift-Cube-Franka-IK-Rel-v0 `__ @@ -146,11 +214,28 @@ for the lift-cube environment: .. |cube-allegro-link| replace:: `Isaac-Repose-Cube-Allegro-v0 `__ .. |allegro-direct-link| replace:: `Isaac-Repose-Cube-Allegro-Direct-v0 `__ .. |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 `__ +.. |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 `__ +.. |reach_openarm_bi-link| replace:: `Isaac-Reach-OpenArm-Bi-v0 `__ +.. |reach_openarm_uni-link| replace:: `Isaac-Reach-OpenArm-v0 `__ +.. |lift_openarm_uni-link| replace:: `Isaac-Lift-Cube-OpenArm-v0 `__ +.. |cabi_openarm_uni-link| replace:: `Isaac-Open-Drawer-OpenArm-v0 `__ + Contact-rich Manipulation ~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -185,6 +270,95 @@ For example: .. |factory-gear-link| replace:: `Isaac-Factory-GearMesh-Direct-v0 `__ .. |factory-nut-link| replace:: `Isaac-Factory-NutThread-Direct-v0 `__ +AutoMate +~~~~~~~~ + +Environments based on 100 diverse assembly tasks, each involving the insertion of a plug into a socket. These tasks share a common configuration and differ by th geometry and properties of the parts. + +You can switch between tasks by specifying the corresponding asset ID. Available asset IDs include: + +'00004', '00007', '00014', '00015', '00016', '00021', '00028', '00030', '00032', '00042', '00062', '00074', '00077', '00078', '00081', '00083', '00103', '00110', '00117', '00133', '00138', '00141', '00143', '00163', '00175', '00186', '00187', '00190', '00192', '00210', '00211', '00213', '00255', '00256', '00271', '00293', '00296', '00301', '00308', '00318', '00319', '00320', '00329', '00340', '00345', '00346', '00360', '00388', '00410', '00417', '00422', '00426', '00437', '00444', '00446', '00470', '00471', '00480', '00486', '00499', '00506', '00514', '00537', '00553', '00559', '00581', '00597', '00614', '00615', '00638', '00648', '00649', '00652', '00659', '00681', '00686', '00700', '00703', '00726', '00731', '00741', '00755', '00768', '00783', '00831', '00855', '00860', '00863', '01026', '01029', '01036', '01041', '01053', '01079', '01092', '01102', '01125', '01129', '01132', '01136'. + +We provide environments for both disassembly and assembly. + +.. attention:: + + 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 --toolkit + + When using conda, cuda toolkit can be installed with: + + .. code-block:: bash + + conda install cudatoolkit + + 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. + + * To train an assembly policy, we run the command ``python source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py --assembly_id=ASSEMBLY_ID --train``. We can customize the training process using the optional flags: ``--headless`` to run without opening the GUI windows, ``--max_iterations=MAX_ITERATIONS`` to set the number of training iterations, ``--num_envs=NUM_ENVS`` to set the number of parallel environments during training, ``--seed=SEED`` to assign the random seed. The policy checkpoints will be saved automatically during training in the directory ``logs/rl_games/Assembly/test``. + * To evaluate an assembly policy, we run the command ``python source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py --assembly_id=ASSEMBLY_ID --checkpoint=CHECKPOINT --log_eval``. The evaluation results are stored in ``evaluation_{ASSEMBLY_ID}.h5``. + +.. table:: + :widths: 33 37 30 + + +--------------------+-------------------------+-----------------------------------------------------------------------------+ + | World | Environment ID | Description | + +====================+=========================+=============================================================================+ + | |disassembly| | |disassembly-link| | Lift a plug out of the socket with the Franka robot | + +--------------------+-------------------------+-----------------------------------------------------------------------------+ + | |assembly| | |assembly-link| | Insert a plug into its corresponding socket with the Franka robot | + +--------------------+-------------------------+-----------------------------------------------------------------------------+ + +.. |assembly| image:: ../_static/tasks/automate/00004.jpg +.. |disassembly| image:: ../_static/tasks/automate/01053_disassembly.jpg + +.. |assembly-link| replace:: `Isaac-AutoMate-Assembly-Direct-v0 `__ +.. |disassembly-link| replace:: `Isaac-AutoMate-Disassembly-Direct-v0 `__ + +FORGE +~~~~~~~~ + +FORGE environments extend Factory environments with: + +* Force sensing: Add observations for force experienced by the end-effector. +* Excessive force penalty: Add an option to penalize the agent for excessive contact forces. +* Dynamics randomization: Randomize controller gains, asset properties (friction, mass), and dead-zone. +* Success prediction: Add an extra action that predicts task success. + +These tasks share the same task configurations and control options. You can switch between them by specifying the task name. + +* |forge-peg-link|: Peg insertion with the Franka arm +* |forge-gear-link|: Gear meshing with the Franka arm +* |forge-nut-link|: Nut-Bolt fastening with the Franka arm + +.. table:: + :widths: 33 37 30 + + +--------------------+-------------------------+-----------------------------------------------------------------------------+ + | World | Environment ID | Description | + +====================+=========================+=============================================================================+ + | |forge-peg| | |forge-peg-link| | Insert peg into the socket with the Franka robot | + +--------------------+-------------------------+-----------------------------------------------------------------------------+ + | |forge-gear| | |forge-gear-link| | Insert and mesh gear into the base with other gears, using the Franka robot | + +--------------------+-------------------------+-----------------------------------------------------------------------------+ + | |forge-nut| | |forge-nut-link| | Thread the nut onto the first 2 threads of the bolt, using the Franka robot | + +--------------------+-------------------------+-----------------------------------------------------------------------------+ + +.. |forge-peg| image:: ../_static/tasks/factory/peg_insert.jpg +.. |forge-gear| image:: ../_static/tasks/factory/gear_mesh.jpg +.. |forge-nut| image:: ../_static/tasks/factory/nut_thread.jpg + +.. |forge-peg-link| replace:: `Isaac-Forge-PegInsert-Direct-v0 `__ +.. |forge-gear-link| replace:: `Isaac-Forge-GearMesh-Direct-v0 `__ +.. |forge-nut-link| replace:: `Isaac-Forge-NutThread-Direct-v0 `__ + + Locomotion ~~~~~~~~~~ @@ -234,6 +408,12 @@ Environments based on legged locomotion tasks. +------------------------------+----------------------------------------------+------------------------------------------------------------------------------+ | |velocity-rough-g1| | |velocity-rough-g1-link| | Track a velocity command on rough terrain with the Unitree G1 robot | +------------------------------+----------------------------------------------+------------------------------------------------------------------------------+ + | |velocity-flat-digit| | |velocity-flat-digit-link| | Track a velocity command on flat terrain with the Agility Digit robot | + +------------------------------+----------------------------------------------+------------------------------------------------------------------------------+ + | |velocity-rough-digit| | |velocity-rough-digit-link| | Track a velocity command on rough terrain with the Agility Digit robot | + +------------------------------+----------------------------------------------+------------------------------------------------------------------------------+ + | |tracking-loco-manip-digit| | |tracking-loco-manip-digit-link| | Track a root velocity and hand pose command with the Agility Digit robot | + +------------------------------+----------------------------------------------+------------------------------------------------------------------------------+ .. |velocity-flat-anymal-b-link| replace:: `Isaac-Velocity-Flat-Anymal-B-v0 `__ .. |velocity-rough-anymal-b-link| replace:: `Isaac-Velocity-Rough-Anymal-B-v0 `__ @@ -264,6 +444,9 @@ Environments based on legged locomotion tasks. .. |velocity-flat-g1-link| replace:: `Isaac-Velocity-Flat-G1-v0 `__ .. |velocity-rough-g1-link| replace:: `Isaac-Velocity-Rough-G1-v0 `__ +.. |velocity-flat-digit-link| replace:: `Isaac-Velocity-Flat-Digit-v0 `__ +.. |velocity-rough-digit-link| replace:: `Isaac-Velocity-Rough-Digit-v0 `__ +.. |tracking-loco-manip-digit-link| replace:: `Isaac-Tracking-LocoManip-Digit-v0 `__ .. |velocity-flat-anymal-b| image:: ../_static/tasks/locomotion/anymal_b_flat.jpg .. |velocity-rough-anymal-b| image:: ../_static/tasks/locomotion/anymal_b_rough.jpg @@ -282,6 +465,9 @@ Environments based on legged locomotion tasks. .. |velocity-rough-h1| image:: ../_static/tasks/locomotion/h1_rough.jpg .. |velocity-flat-g1| image:: ../_static/tasks/locomotion/g1_flat.jpg .. |velocity-rough-g1| image:: ../_static/tasks/locomotion/g1_rough.jpg +.. |velocity-flat-digit| image:: ../_static/tasks/locomotion/agility_digit_flat.jpg +.. |velocity-rough-digit| image:: ../_static/tasks/locomotion/agility_digit_rough.jpg +.. |tracking-loco-manip-digit| image:: ../_static/tasks/locomotion/agility_digit_loco_manip.jpg Navigation ~~~~~~~~~~ @@ -300,6 +486,28 @@ Navigation .. |anymal_c_nav| image:: ../_static/tasks/navigation/anymal_c_nav.jpg +Multirotor +~~~~~~~~~~ + +.. note:: + The multirotor entry provides an environment configuration for flying the ARL robot. + See the `drone_arl` folder and the ARL robot config + (`ARL_ROBOT_1_CFG`) in the codebase for details. + +.. |arl_robot_track_position_state_based-link| replace:: `Isaac-TrackPositionNoObstacles-ARL-Robot-1-v0 `__ + +.. |arl_robot_track_position_state_based| image:: ../_static/tasks/drone_arl/arl_robot_1_track_position_state_based.jpg + +.. table:: + :widths: 33 37 30 + + +----------------------------------------+---------------------------------------------+----------------------------------------------------------------------------------------+ + | World | Environment ID | Description | + +========================================+=============================================+========================================================================================+ + | |arl_robot_track_position_state_based| | |arl_robot_track_position_state_based-link| | Setpoint position control for the ARL robot using the track_position_state_based task. | + +----------------------------------------+---------------------------------------------+----------------------------------------------------------------------------------------+ + + Others ~~~~~~ @@ -500,6 +708,10 @@ Environments based on fixed-arm manipulation tasks. Comprehensive List of Environments ================================== +For environments that have a different task name listed under ``Inference Task Name``, please use the Inference Task Name +provided when running ``play.py`` or any inferencing workflows. These tasks provide more suitable configurations for +inferencing, including reading from an already trained checkpoint and disabling runtime perturbations used for training. + .. list-table:: :widths: 33 25 19 25 @@ -519,47 +731,47 @@ Comprehensive List of Environments - - Direct - **rl_games** (PPO), **skrl** (IPPO, PPO, MAPPO) - * - Isaac-Cartpole-Camera-Showcase-Box-Box-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Box-Box-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Box-Discrete-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Box-Discrete-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Box-MultiDiscrete-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Box-MultiDiscrete-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Dict-Box-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Dict-Box-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Dict-Discrete-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Dict-Discrete-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Dict-MultiDiscrete-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Dict-MultiDiscrete-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Tuple-Box-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Tuple-Box-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Tuple-Discrete-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Tuple-Discrete-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Camera-Showcase-Tuple-MultiDiscrete-Direct-v0 + * - Isaac-Cartpole-Camera-Showcase-Tuple-MultiDiscrete-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **skrl** (PPO) - * - Isaac-Cartpole-Depth-Camera-Direct-v0 + * - Isaac-Cartpole-Depth-Camera-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **rl_games** (PPO), **skrl** (PPO) - * - Isaac-Cartpole-Depth-v0 + * - Isaac-Cartpole-Depth-v0 (Requires running with ``--enable_cameras``) - - Manager Based - **rl_games** (PPO) @@ -567,19 +779,19 @@ Comprehensive List of Environments - - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) - * - Isaac-Cartpole-RGB-Camera-Direct-v0 + * - Isaac-Cartpole-RGB-Camera-Direct-v0 (Requires running with ``--enable_cameras``) - - Direct - **rl_games** (PPO), **skrl** (PPO) - * - Isaac-Cartpole-RGB-ResNet18-v0 + * - Isaac-Cartpole-RGB-ResNet18-v0 (Requires running with ``--enable_cameras``) - - Manager Based - **rl_games** (PPO) - * - Isaac-Cartpole-RGB-TheiaTiny-v0 + * - Isaac-Cartpole-RGB-TheiaTiny-v0 (Requires running with ``--enable_cameras``) - - Manager Based - **rl_games** (PPO) - * - Isaac-Cartpole-RGB-v0 + * - Isaac-Cartpole-RGB-v0 (Requires running with ``--enable_cameras``) - - Manager Based - **rl_games** (PPO) @@ -659,6 +871,26 @@ Comprehensive List of Environments - - Direct - **rl_games** (PPO) + * - Isaac-AutoMate-Assembly-Direct-v0 + - + - Direct + - **rl_games** (PPO) + * - Isaac-AutoMate-Disassembly-Direct-v0 + - + - Direct + - + * - Isaac-Forge-GearMesh-Direct-v0 + - + - Direct + - **rl_games** (PPO) + * - Isaac-Forge-NutThread-Direct-v0 + - + - Direct + - **rl_games** (PPO) + * - Isaac-Forge-PegInsert-Direct-v0 + - + - Direct + - **rl_games** (PPO) * - Isaac-Franka-Cabinet-Direct-v0 - - Direct @@ -699,6 +931,10 @@ Comprehensive List of Environments - - Manager Based - + * - Isaac-Tracking-LocoManip-Digit-v0 + - Isaac-Tracking-LocoManip-Digit-Play-v0 + - Manager Based + - **rsl_rl** (PPO) * - Isaac-Navigation-Flat-Anymal-C-v0 - Isaac-Navigation-Flat-Anymal-C-Play-v0 - Manager Based @@ -739,6 +975,10 @@ Comprehensive List of Environments - Isaac-Reach-UR10-Play-v0 - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) + * - Isaac-Deploy-Reach-UR10e-v0 + - Isaac-Deploy-Reach-UR10e-Play-v0 + - Manager Based + - **rsl_rl** (PPO) * - Isaac-Repose-Cube-Allegro-Direct-v0 - - Direct @@ -763,8 +1003,8 @@ Comprehensive List of Environments - - Direct - **rl_games** (LSTM) - * - Isaac-Repose-Cube-Shadow-Vision-Direct-v0 - - Isaac-Repose-Cube-Shadow-Vision-Direct-Play-v0 + * - Isaac-Repose-Cube-Shadow-Vision-Direct-v0 (Requires running with ``--enable_cameras``) + - Isaac-Repose-Cube-Shadow-Vision-Direct-Play-v0 (Requires running with ``--enable_cameras``) - Direct - **rsl_rl** (PPO), **rl_games** (VISION) * - Isaac-Shadow-Hand-Over-Direct-v0 @@ -775,6 +1015,14 @@ Comprehensive List of Environments - - 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 @@ -787,6 +1035,59 @@ Comprehensive List of Environments - - Manager Based - + * - Isaac-PickPlace-G1-InspireFTP-Abs-v0 + - + - Manager Based + - + * - Isaac-Stack-Cube-UR10-Long-Suction-IK-Rel-v0 + - + - Manager Based + - + * - Isaac-Stack-Cube-UR10-Short-Suction-IK-Rel-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-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 @@ -807,6 +1108,10 @@ Comprehensive List of Environments - Isaac-Velocity-Flat-Cassie-Play-v0 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) + * - Isaac-Velocity-Flat-Digit-v0 + - Isaac-Velocity-Flat-Digit-Play-v0 + - Manager Based + - **rsl_rl** (PPO) * - Isaac-Velocity-Flat-G1-v0 - Isaac-Velocity-Flat-G1-Play-v0 - Manager Based @@ -822,7 +1127,7 @@ Comprehensive List of Environments * - Isaac-Velocity-Flat-Unitree-A1-v0 - Isaac-Velocity-Flat-Unitree-A1-Play-v0 - Manager Based - - **rsl_rl** (PPO), **skrl** (PPO) + - **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) * - Isaac-Velocity-Flat-Unitree-Go1-v0 - Isaac-Velocity-Flat-Unitree-Go1-Play-v0 - Manager Based @@ -851,6 +1156,10 @@ Comprehensive List of Environments - Isaac-Velocity-Rough-Cassie-Play-v0 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) + * - Isaac-Velocity-Rough-Digit-v0 + - Isaac-Velocity-Rough-Digit-Play-v0 + - Manager Based + - **rsl_rl** (PPO) * - Isaac-Velocity-Rough-G1-v0 - Isaac-Velocity-Rough-G1-Play-v0 - Manager Based @@ -862,7 +1171,7 @@ Comprehensive List of Environments * - Isaac-Velocity-Rough-Unitree-A1-v0 - Isaac-Velocity-Rough-Unitree-A1-Play-v0 - Manager Based - - **rsl_rl** (PPO), **skrl** (PPO) + - **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) * - Isaac-Velocity-Rough-Unitree-Go1-v0 - Isaac-Velocity-Rough-Unitree-Go1-Play-v0 - Manager Based @@ -871,3 +1180,19 @@ Comprehensive List of Environments - Isaac-Velocity-Rough-Unitree-Go2-Play-v0 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) + * - Isaac-Reach-OpenArm-Bi-v0 + - Isaac-Reach-OpenArm-Bi-Play-v0 + - Manager Based + - **rsl_rl** (PPO), **rl_games** (PPO) + * - Isaac-Reach-OpenArm-v0 + - Isaac-Reach-OpenArm-Play-v0 + - Manager Based + - **rsl_rl** (PPO), **skrl** (PPO), **rl_games** (PPO) + * - Isaac-Lift-Cube-OpenArm-v0 + - Isaac-Lift-Cube-OpenArm-Play-v0 + - Manager Based + - **rsl_rl** (PPO), **rl_games** (PPO) + * - Isaac-Open-Drawer-OpenArm-v0 + - Isaac-Open-Drawer-OpenArm-Play-v0 + - Manager Based + - **rsl_rl** (PPO), **rl_games** (PPO) diff --git a/docs/source/overview/imitation-learning/augmented_imitation.rst b/docs/source/overview/imitation-learning/augmented_imitation.rst new file mode 100644 index 000000000000..b3593f22e622 --- /dev/null +++ b/docs/source/overview/imitation-learning/augmented_imitation.rst @@ -0,0 +1,436 @@ +.. _augmented-imitation-learning: + +Augmented Imitation Learning +============================ + +This section describes how to use Isaac Lab's imitation learning capabilities with the visual augmentation capabilities of `Cosmos `_ models to generate demonstrations at scale to train visuomotor policies robust against visual variations. + +Generating Demonstrations +~~~~~~~~~~~~~~~~~~~~~~~~~ + +We use the Isaac Lab Mimic feature that allows the generation of additional demonstrations automatically from a handful of annotated demonstrations. + +.. note:: + This section assumes you already have an annotated dataset of collected demonstrations. If you don't, you can follow the instructions in :ref:`teleoperation-imitation-learning` to collect and annotate your own demonstrations. + +In the following example, we will show you how to use Isaac Lab Mimic to generate additional demonstrations that can be used to train a visuomotor policy directly or can be augmented with visual variations using Cosmos (using the ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-Mimic-v0`` environment). + +.. note:: + The ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-Mimic-v0`` environment is similar to the standard visuomotor environment (``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Mimic-v0``), but with the addition of segmentation masks, depth maps, and normal maps in the generated dataset. These additional modalities are required to get the best results from the visual augmentation done using Cosmos. + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu --enable_cameras --headless --num_envs 10 --generation_num_trials 1000 \ + --input_file ./datasets/annotated_dataset.hdf5 --output_file ./datasets/mimic_dataset_1k.hdf5 \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-Mimic-v0 \ + --rendering_mode performance + +The number of demonstrations can be increased or decreased, 1000 demonstrations have been shown to provide good training results for this task. + +Additionally, the number of environments in the ``--num_envs`` parameter can be adjusted to speed up data generation. +The suggested number of 10 can be executed on a moderate laptop CPU. +On a more powerful desktop machine, use a larger number of environments for a significant speedup of this step. + +Cosmos Augmentation +~~~~~~~~~~~~~~~~~~~ + +HDF5 to MP4 Conversion +^^^^^^^^^^^^^^^^^^^^^^ + +The ``hdf5_to_mp4.py`` script converts camera frames stored in HDF5 demonstration files to MP4 videos. It supports multiple camera modalities including RGB, segmentation, depth and normal maps. This conversion is necessary for visual augmentation using Cosmos as it only works with video files rather than HDF5 data. + +.. rubric:: Required Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--input_file`` + - Path to the input HDF5 file. + * - ``--output_dir`` + - Directory to save the output MP4 files. + +.. rubric:: Optional Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--input_keys`` + - List of input keys to process from the HDF5 file. (default: ["table_cam", "wrist_cam", "table_cam_segmentation", "table_cam_normals", "table_cam_shaded_segmentation", "table_cam_depth"]) + * - ``--video_height`` + - Height of the output video in pixels. (default: 704) + * - ``--video_width`` + - Width of the output video in pixels. (default: 1280) + * - ``--framerate`` + - Frames per second for the output video. (default: 30) + +.. note:: + The default input keys cover all camera modalities as per the naming convention followed in the ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-Mimic-v0`` environment. We include an additional modality "table_cam_shaded_segmentation" which is not a part of the generated modalities from simulation in the HDF5 data file. Instead, it is automatically generated by this script using a combination of the segmentation and normal maps to get a pseudo-textured segmentation video for better controlling the Cosmos augmentation. + +.. note:: + We recommend using the default values given above for the output video height, width and framerate for the best results with Cosmos augmentation. + +Example usage for the cube stacking task: + +.. code:: bash + + python scripts/tools/hdf5_to_mp4.py \ + --input_file datasets/mimic_dataset_1k.hdf5 \ + --output_dir datasets/mimic_dataset_1k_mp4 + +.. _running-cosmos: + +Running Cosmos for Visual Augmentation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +After converting the demonstrations to MP4 format, you can use a `Cosmos`_ model to visually augment the videos. Follow the Cosmos documentation for details on the augmentation process. Visual augmentation can include changes to lighting, textures, backgrounds, and other visual elements while preserving the essential task-relevant features. + +We use the RGB, depth and shaded segmentation videos from the previous step as input to the Cosmos model as seen below: + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/cosmos_inputs.gif + :width: 100% + :align: center + :alt: RGB, depth and segmentation control inputs to Cosmos + +We provide an example augmentation output from `Cosmos Transfer1 `_ below: + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/cosmos_output.gif + :width: 100% + :align: center + :alt: Cosmos Transfer1 augmentation output + +We recommend using the `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:: + :widths: 30 70 + :header-rows: 0 + + * - ``negative_prompt`` + - "The video captures a game playing, with bad crappy graphics and cartoonish frames. It represents a recording of old outdated games. The images are very pixelated and of poor CG quality. There are many subtitles in the footage. Overall, the video is unrealistic and appears cg. Plane background." + * - ``sigma_max`` + - 50 + * - ``control_weight`` + - "0.3,0.3,0.6,0.7" + * - ``hint_key`` + - "blur,canny,depth,segmentation" + +Another crucial aspect to get good augmentations is the set of prompts used to control the Cosmos generation. We provide a script, ``cosmos_prompt_gen.py``, to construct prompts from a set of carefully chosen templates that handle various aspects of the augmentation process. + +.. rubric:: Required Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--templates_path`` + - Path to the file containing templates for the prompts. + +.. rubric:: Optional Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--num_prompts`` + - Number of prompts to generate (default: 1). + * - ``--output_path`` + - Path to the output file to write generated prompts. (default: prompts.txt) + +.. code:: bash + + python scripts/tools/cosmos/cosmos_prompt_gen.py \ + --templates_path scripts/tools/cosmos/transfer1_templates.json \ + --num_prompts 10 --output_path prompts.txt + +In case you want to create your own prompts, we suggest you refer to the following guidelines: + +1. Keep the prompts as detailed as possible. It is best to have some instruction on how the generation should handle each visible object/region of interest. For instance, the prompts that we provide cover explicit details for the table, lighting, background, robot arm, cubes, and the general setting. + +2. Try to keep the augmentation instructions as realistic and coherent as possible. The more unrealistic or unconventional the prompt is, the worse the model does at retaining key features of the input control video(s). + +3. Keep the augmentation instructions in-sync for each aspect. What we mean by this is that the augmentation for all the objects/regions of interest should be coherent and conventional with respect to each other. For example, it is better to have a prompt such as "The table is of old dark wood with faded polish and food stains and the background consists of a suburban home" instead of something like "The table is of old dark wood with faded polish and food stains and the background consists of a spaceship hurtling through space". + +4. It is vital to include details on key aspects of the input control video(s) that should be retained or left unchanged. In our prompts, we very clearly mention that the cube colors should be left unchanged such that the bottom cube is blue, the middle is red and the top is green. Note that we not only mention what should be left unchanged but also give details on what form that aspect currently has. + +Example command to use the Cosmos Transfer1 model for this usecase: + +.. code:: bash + + export CUDA_VISIBLE_DEVICES="${CUDA_VISIBLE_DEVICES:=0}" + export CHECKPOINT_DIR="${CHECKPOINT_DIR:=./checkpoints}" + export NUM_GPU="${NUM_GPU:=1}" + PYTHONPATH=$(pwd) torchrun --nproc_per_node=$NUM_GPU --nnodes=1 --node_rank=0 cosmos_transfer1/diffusion/inference/transfer.py \ + --checkpoint_dir $CHECKPOINT_DIR \ + --video_save_folder outputs/cosmos_dataset_1k_mp4 \ + --controlnet_specs ./controlnet_specs/demo_0.json \ + --offload_text_encoder_model \ + --offload_guardrail_models \ + --num_gpus $NUM_GPU + +Example ``./controlnet_specs/demo_0.json`` json file to use with the above command: + +.. code:: json + + { + "prompt": "A robotic arm is picking up and stacking cubes inside a foggy industrial scrapyard at dawn, surrounded by piles of old robotic parts and twisted metal. The background includes large magnetic cranes, rusted conveyor belts, and flickering yellow floodlights struggling to penetrate the fog. The robot arm is bright teal with a glossy surface and silver stripes on the outer edges; the joints rotate smoothly and the pistons reflect a pale cyan hue. The robot arm is mounted on a table that is light oak wood with a natural grain pattern and a glossy varnish that reflects overhead lights softly; small burn marks dot one corner. The arm is connected to the base mounted on the table. The bottom cube is deep blue, the second cube is bright red, and the top cube is vivid green, maintaining their correct order after stacking. Sunlight pouring in from a large, open window bathes the table and robotic arm in a warm golden light. The shadows are soft, and the scene feels natural and inviting with a slight contrast between light and shadow.", + "negative_prompt": "The video captures a game playing, with bad crappy graphics and cartoonish frames. It represents a recording of old outdated games. The images are very pixelated and of poor CG quality. There are many subtitles in the footage. Overall, the video is unrealistic and appears cg. Plane background.", + "input_video_path" : "mimic_dataset_1k_mp4/demo_0_table_cam.mp4", + "sigma_max": 50, + "vis": { + "input_control": "mimic_dataset_1k_mp4/demo_0_table_cam.mp4", + "control_weight": 0.3 + }, + "edge": { + "control_weight": 0.3 + }, + "depth": { + "input_control": "mimic_dataset_1k_mp4/demo_0_table_cam_depth.mp4", + "control_weight": 0.6 + }, + "seg": { + "input_control": "mimic_dataset_1k_mp4/demo_0_table_cam_shaded_segmentation.mp4", + "control_weight": 0.7 + } + } + +MP4 to HDF5 Conversion +^^^^^^^^^^^^^^^^^^^^^^ + +The ``mp4_to_hdf5.py`` script converts the visually augmented MP4 videos back to HDF5 format for training. This step is crucial as it ensures the augmented visual data is in the correct format for training visuomotor policies in Isaac Lab and pairs the videos with the corresponding demonstration data from the original dataset. + +.. rubric:: Required Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--input_file`` + - Path to the input HDF5 file containing the original demonstrations. + * - ``--videos_dir`` + - Directory containing the visually augmented MP4 videos. + * - ``--output_file`` + - Path to save the new HDF5 file with augmented videos. + +.. note:: + The input HDF5 file is used to preserve the non-visual data (such as robot states and actions) while replacing the visual data with the augmented versions. + +.. important:: + The visually augmented MP4 files must follow the naming convention ``demo_{demo_id}_*.mp4``, where: + + - ``demo_id`` matches the demonstration ID from the original MP4 file + + - ``*`` signifies that the file name can be as per user preference starting from this point + + This naming convention is required for the script to correctly pair the augmented videos with their corresponding demonstrations. + +Example usage for the cube stacking task: + +.. code:: bash + + python scripts/tools/mp4_to_hdf5.py \ + --input_file datasets/mimic_dataset_1k.hdf5 \ + --videos_dir datasets/cosmos_dataset_1k_mp4 \ + --output_file datasets/cosmos_dataset_1k.hdf5 + +Pre-generated Dataset +^^^^^^^^^^^^^^^^^^^^^ + +We provide a pre-generated dataset in HDF5 format containing visually augmented demonstrations for the cube stacking task. This dataset can be used if you do not wish to run Cosmos locally to generate your own augmented data. The dataset is available on `Hugging Face `_ and contains both (as separate dataset files), original and augmented demonstrations, that can be used for training visuomotor policies. + +Merging Datasets +^^^^^^^^^^^^^^^^ + +The ``merge_hdf5_datasets.py`` script combines multiple HDF5 datasets into a single file. This is useful when you want to combine the original demonstrations with the augmented ones to create a larger, more diverse training dataset. + +.. rubric:: Required Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--input_files`` + - A list of paths to HDF5 files to merge. + +.. rubric:: Optional Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--output_file`` + - File path to merged output. (default: merged_dataset.hdf5) + +.. tip:: + Merging datasets can help improve policy robustness by exposing the model to both original and augmented visual conditions during training. + +Example usage for the cube stacking task: + +.. code:: bash + + python scripts/tools/merge_hdf5_datasets.py \ + --input_files datasets/mimic_dataset_1k.hdf5 datasets/cosmos_dataset_1k.hdf5 \ + --output_file datasets/mimic_cosmos_dataset.hdf5 + +Model Training and Evaluation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Robomimic Setup +^^^^^^^^^^^^^^^ + +As an example, we will train a BC agent implemented in `Robomimic `__ to train a policy. Any other framework or training method could be used. + +To install the robomimic framework, use the following commands: + +.. code:: bash + + # install the dependencies + sudo apt install cmake build-essential + # install python module (for robomimic) + ./isaaclab.sh -i robomimic + +Training an agent +^^^^^^^^^^^^^^^^^ + +Using the generated data, we can now train a visuomotor BC agent for ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-v0``: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-v0 --algo bc \ + --dataset ./datasets/mimic_cosmos_dataset.hdf5 \ + --name bc_rnn_image_franka_stack_mimic_cosmos + +.. note:: + By default the trained models and logs will be saved to ``IssacLab/logs/robomimic``. + +Evaluation +^^^^^^^^^^ + +The ``robust_eval.py`` script evaluates trained visuomotor policies in simulation. This evaluation helps assess how well the policy generalizes to different visual variations and whether the visually augmented data has improved the policy's robustness. + +Below is an explanation of the different settings used for evaluation: + +.. rubric:: Evaluation Settings + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``Vanilla`` + - Exact same setting as that used during Mimic data generation. + * - ``Light Intensity`` + - Light intensity/brightness is varied, all other aspects remain the same. + * - ``Light Color`` + - Light color is varied, all other aspects remain the same. + * - ``Light Texture (Background)`` + - Light texture/background is varied, all other aspects remain the same. + * - ``Table Texture`` + - Table's visual texture is varied, all other aspects remain the same. + * - ``Robot Arm Texture`` + - Robot arm's visual texture is varied, all other aspects remain the same. + +.. rubric:: Required Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--task`` + - Name of the environment. + * - ``--input_dir`` + - Directory containing the model checkpoints to evaluate. + +.. rubric:: Optional Arguments + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - ``--start_epoch`` + - Epoch of the checkpoint to start the evaluation from. (default: 100) + * - ``--horizon`` + - Step horizon of each rollout. (default: 400) + * - ``--num_rollouts`` + - Number of rollouts per model per setting. (default: 15) + * - ``--num_seeds`` + - Number of random seeds to evaluate. (default: 3) + * - ``--seeds`` + - List of specific seeds to use instead of random ones. + * - ``--log_dir`` + - Directory to write results to. (default: /tmp/policy_evaluation_results) + * - ``--log_file`` + - Name of the output file. (default: results) + * - ``--norm_factor_min`` + - Minimum value of the action space normalization factor. + * - ``--norm_factor_max`` + - Maximum value of the action space normalization factor. + * - ``--disable_fabric`` + - Whether to disable fabric and use USD I/O operations. + * - ``--enable_pinocchio`` + - Whether to enable Pinocchio for IK controllers. + +.. note:: + The evaluation results will help you understand if the visual augmentation has improved the policy's performance and robustness. Compare these results with evaluations on the original dataset to measure the impact of augmentation. + +Example usage for the cube stacking task: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/robust_eval.py \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-v0 \ + --input_dir logs/robomimic/Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-v0/bc_rnn_image_franka_stack_mimic_cosmos/*/models \ + --log_dir robust_results/bc_rnn_image_franka_stack_mimic_cosmos \ + --log_file result \ + --enable_cameras \ + --seeds 0 \ + --num_rollouts 15 \ + --rendering_mode performance + +.. note:: + This script can take over a day or even longer to run (depending on the hardware being used). This behavior is expected. + +We use the above script to compare models trained with 1000 Mimic-generated demonstrations, 2000 Mimic-generated demonstrations and 2000 Cosmos-Mimic-generated demonstrations (1000 original mimic + 1000 Cosmos augmented) respectively. We use the same seeds (0, 1000 and 5000) for all three models and provide the metrics (averaged across best checkpoints for each seed) below: + +.. rubric:: Model Comparison + +.. list-table:: + :widths: 25 25 25 25 + :header-rows: 0 + + * - **Evaluation Setting** + - **Mimic 1k Baseline** + - **Mimic 2k Baseline** + - **Cosmos-Mimic 2k** + * - ``Vanilla`` + - 62% + - 96.6% + - 86.6% + * - ``Light Intensity`` + - 11.1% + - 20% + - 62.2% + * - ``Light Color`` + - 24.6% + - 30% + - 77.7% + * - ``Light Texture (Background)`` + - 16.6% + - 20% + - 68.8% + * - ``Table Texture`` + - 0% + - 0% + - 20% + * - ``Robot Arm Texture`` + - 0% + - 0% + - 4.4% + +The above trained models' checkpoints can be accessed `here `_ in case you wish to use the models directly. diff --git a/docs/source/overview/imitation-learning/index.rst b/docs/source/overview/imitation-learning/index.rst new file mode 100644 index 000000000000..f8f77d031fb0 --- /dev/null +++ b/docs/source/overview/imitation-learning/index.rst @@ -0,0 +1,12 @@ +Imitation Learning +================== + +In this section, we show existing scripts for running imitation learning +with Isaac Lab. + +.. toctree:: + :maxdepth: 1 + + teleop_imitation + augmented_imitation + skillgen diff --git a/docs/source/overview/imitation-learning/skillgen.rst b/docs/source/overview/imitation-learning/skillgen.rst new file mode 100644 index 000000000000..b577f82e13ae --- /dev/null +++ b/docs/source/overview/imitation-learning/skillgen.rst @@ -0,0 +1,548 @@ +.. _skillgen: + +SkillGen for Automated Demonstration Generation +=============================================== + +SkillGen is an advanced demonstration generation system that enhances Isaac Lab Mimic by integrating motion planning. It generates high-quality, adaptive, collision-free robot demonstrations by combining human-provided subtask segments with automated motion planning. + +What is SkillGen? +~~~~~~~~~~~~~~~~~ + +SkillGen addresses key limitations in traditional demonstration generation: + +* **Motion Quality**: Uses cuRobo's GPU-accelerated motion planner to generate smooth, collision-free trajectories +* **Validity**: Generates kinematically feasible plans between skill segments +* **Diversity**: Generates varied demonstrations through configurable sampling and planning parameters +* **Adaptability**: Generates demonstrations that can be adapted to new object placements and scene configurations during data generation + +The system works by taking manually annotated human demonstrations, extracting localized subtask skills (see `Subtasks in SkillGen`_), and using cuRobo to plan feasible motions between these skill segments while respecting robot kinematics and collision constraints. + +Prerequisites +~~~~~~~~~~~~~ + +Before using SkillGen, you must understand: + +1. **Teleoperation**: How to control robots and record demonstrations using keyboard, SpaceMouse, or hand tracking +2. **Isaac Lab Mimic**: The complete workflow including data collection, annotation, generation, and policy training + +.. important:: + + Review the :ref:`teleoperation-imitation-learning` documentation thoroughly before proceeding with SkillGen. + +.. _skillgen-installation: + +Installation +~~~~~~~~~~~~ + +SkillGen requires Isaac Lab, Isaac Sim, and cuRobo. Follow these steps in your Isaac Lab conda environment. + +Step 1: Install and verify Isaac Sim and Isaac Lab +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Follow the official Isaac Sim and Isaac Lab installation guide `here `__. + +Step 2: Install cuRobo +^^^^^^^^^^^^^^^^^^^^^^ + +cuRobo provides the motion planning capabilities for SkillGen. This installation is tested to work with Isaac Lab's PyTorch and CUDA requirements: + +.. code:: bash + + # One line installation of cuRobo (formatted for readability) + conda install -c nvidia cuda-toolkit=12.8 -y && \ + export CUDA_HOME="$CONDA_PREFIX" && \ + export PATH="$CUDA_HOME/bin:$PATH" && \ + export LD_LIBRARY_PATH="$CUDA_HOME/lib:$LD_LIBRARY_PATH" && \ + export TORCH_CUDA_ARCH_LIST="8.0+PTX" && \ + pip install -e "git+https://github.com/NVlabs/curobo.git@ebb71702f3f70e767f40fd8e050674af0288abe8#egg=nvidia-curobo" --no-build-isolation + +.. note:: + * The commit hash ``ebb71702f3f70e767f40fd8e050674af0288abe8`` is tested with Isaac Lab - using other versions may cause compatibility issues. This commit has the support for quad face mesh triangulation, required for cuRobo to parse usds as collision objects. + + * 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 +^^^^^^^^^^^^^^^^^^^^^ + +For trajectory visualization during development: + +.. code:: bash + + pip install rerun-sdk==0.23 + +.. note:: + + **Rerun Visualization Setup:** + + * Rerun is optional but highly recommended for debugging and validating planned trajectories during development + * Enable trajectory visualization by setting ``visualize_plan = True`` in the cuRobo planner configuration + * When enabled, cuRobo planner interface will stream planned end-effector trajectories, waypoints, and collision data to Rerun for interactive inspection + * Visualization helps identify planning issues, collision problems, and trajectory smoothness before full dataset generation + * Can also be ran with ``--headless`` to disable isaacsim visualization but still visualize and debug end effector trajectories + +Step 4: Verify Installation +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Test that cuRobo works with Isaac Lab: + +.. code:: bash + + # This should run without import errors + python -c "import curobo; print('cuRobo installed successfully')" + +.. tip:: + + If you run into ``libstdc++.so.6: version 'GLIBCXX_3.4.30' not found`` error, you can try these commands to fix it: + + .. code:: bash + + conda config --env --set channel_priority strict + conda config --env --add channels conda-forge + conda install -y -c conda-forge "libstdcxx-ng>=12" "libgcc-ng>=12" + +Download the SkillGen Dataset +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +We provide a pre-annotated dataset to help you get started quickly with SkillGen. + +Dataset Contents +^^^^^^^^^^^^^^^^ + +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 cube stacking tasks + +Download and Setup +^^^^^^^^^^^^^^^^^^ + +1. Download the pre-annotated dataset by clicking `here `__. + +2. Prepare the datasets directory and move the downloaded file: + +.. code:: bash + + # Make sure you are in the root directory of your Isaac Lab workspace + cd /path/to/your/IsaacLab + + # Create the datasets directory if it does not exist + mkdir -p datasets + + # Move the downloaded dataset into the datasets directory + mv /path/to/annotated_dataset_skillgen.hdf5 datasets/annotated_dataset_skillgen.hdf5 + +.. tip:: + + A major advantage of SkillGen is that the same annotated dataset can be reused across multiple related tasks (e.g., basic stacking and adaptive bin stacking). This avoids collecting and annotating new data per variant. + +.. admonition:: {Optional for the tasks in this tutorial} Collect a fresh dataset (source + annotated) + + If you want to collect a fresh source dataset and then create an annotated dataset for SkillGen, follow these commands. The user is expected to have knowledge of the Isaac Lab Mimic workflow. + + **Important pointers before you begin** + + * Using the provided annotated dataset is the fastest path to get started with SkillGen tasks in this tutorial. + * If you create your own dataset, SkillGen requires manual annotation of both subtask start and termination boundaries (no auto-annotation). + * Start boundary signals are mandatory for SkillGen; use ``--annotate_subtask_start_signals`` during annotation or data generation will fail. + * Keep your subtask definitions (``object_ref``, ``subtask_term_signal``) consistent with the SkillGen environment config. + + **Record demonstrations** (any teleop device is supported; replace ``spacemouse`` if needed): + + .. code:: bash + + ./isaaclab.sh -p scripts/tools/record_demos.py \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0 \ + --teleop_device spacemouse \ + --dataset_file ./datasets/dataset_skillgen.hdf5 \ + --num_demos 10 + + **Annotate demonstrations for SkillGen** (writes both term and start boundaries): + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \ + --device cpu \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0 \ + --input_file ./datasets/dataset_skillgen.hdf5 \ + --output_file ./datasets/annotated_dataset_skillgen.hdf5 \ + --annotate_subtask_start_signals + +Understanding Dataset Annotation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +SkillGen requires datasets with annotated subtask start and termination boundaries. Auto-annotation is not supported. + +Subtasks in SkillGen +^^^^^^^^^^^^^^^^^^^^ + +**Technical definition:** A subtask is a contiguous demo segment that achieves a manipulation objective, defined via ``SubTaskConfig``: + +* ``object_ref``: the object (or ``None``) used as the spatial reference for this subtask +* ``subtask_term_signal``: the binary termination signal name (transitions 0 to 1 when the subtask completes) +* ``subtask_start_signal``: the binary start signal name (transitions 0 to 1 when the subtask begins; required for SkillGen) + +The subtask localization process performs: + +* detection of signal transition points (0 to 1) to identify subtask boundaries ``[t_start, t_end]``; +* extraction of the subtask segment between boundaries; +* computation of end-effector trajectories and key poses in an object- or task-relative frame (using ``object_ref`` if provided); + +This converts absolute, scene-specific motions into object-relative skill segments that can be adapted to new object placements and scene configurations during data generation. + +Manual Annotation Workflow +^^^^^^^^^^^^^^^^^^^^^^^^^^ +Contrary to the Isaac Lab Mimic workflow, SkillGen requires manual annotation of subtask start and termination boundaries. For example, for grasping a cube, the start signal is right before the gripper closes and the termination signal is right after the object is grasped. You can adjust the start and termination signals to fit your subtask definition. + +.. tip:: + + **Manual Annotation Controls:** + + * Press ``N`` to start/continue playback + * Press ``B`` to pause + * Press ``S`` to mark subtask boundary + * Press ``Q`` to skip current demonstration + + When annotating the start and end signals for a skill segment (e.g., grasp, stack, etc.), pause the playback using ``B`` a few steps before the skill, annotate the start signal using ``S``, and then resume playback using ``N``. After the skill is completed, pause again a few steps later to annotate the end signal using ``S``. + +Data Generation with SkillGen +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +SkillGen transforms annotated demonstrations into diverse, high-quality datasets using motion planning. + +How SkillGen Works +^^^^^^^^^^^^^^^^^^ + +The SkillGen pipeline uses your annotated dataset and the environment's Mimic API to synthesize new demonstrations: + +1. **Subtask boundary use**: Reads per-subtask start and termination indices from the annotated dataset +2. **Goal sampling**: Samples target poses per subtask according to task constraints and datagen config +3. **Trajectory planning**: Plans collision-free motions between subtask segments using cuRobo (when ``--use_skillgen``) +4. **Trajectory stitching**: Stitches skill segments and planned trajectories into complete demonstrations. +5. **Success evaluation**: Validates task success terms; only successful trials are written to the output dataset + +Usage Parameters +^^^^^^^^^^^^^^^^ + +Key parameters for SkillGen data generation: + +* ``--use_skillgen``: Enables SkillGen planner (required) +* ``--generation_num_trials``: Number of demonstrations to generate +* ``--num_envs``: Parallel environments (tune based on GPU memory) +* ``--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 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Generate demonstrations for the standard Isaac Lab Mimic cube stacking task. In this task, the Franka robot must: + +1. Pick up the red cube and place it on the blue cube +2. Pick up the green cube and place it on the red cube +3. Final stack order: blue (bottom), red (middle), green (top). + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/cube_stack_data_gen_skillgen.gif + :width: 75% + :align: center + :alt: Cube stacking task generated with SkillGen + :figclass: align-center + + Cube stacking dataset example. + +Small-Scale Generation +^^^^^^^^^^^^^^^^^^^^^^ + +Start with a small dataset to verify everything works: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu \ + --num_envs 1 \ + --generation_num_trials 10 \ + --input_file ./datasets/annotated_dataset_skillgen.hdf5 \ + --output_file ./datasets/generated_dataset_small_skillgen_cube_stack.hdf5 \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0 \ + --use_skillgen + +Full-Scale Generation +^^^^^^^^^^^^^^^^^^^^^ + +Once satisfied with small-scale results, generate a full training dataset: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu \ + --headless \ + --num_envs 1 \ + --generation_num_trials 1000 \ + --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 + +.. 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 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. + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/bin_cube_stack_data_gen_skillgen.gif + :width: 75% + :align: center + :alt: Adaptive bin cube stacking task generated with SkillGen + :figclass: align-center + + Adaptive bin stacking data generation example. + +Small-Scale Generation +^^^^^^^^^^^^^^^^^^^^^^ + +Test the adaptive stacking setup: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu \ + --num_envs 1 \ + --generation_num_trials 10 \ + --input_file ./datasets/annotated_dataset_skillgen.hdf5 \ + --output_file ./datasets/generated_dataset_small_skillgen_bin_cube_stack.hdf5 \ + --task Isaac-Stack-Cube-Bin-Franka-IK-Rel-Mimic-v0 \ + --use_skillgen + +Full-Scale Generation +^^^^^^^^^^^^^^^^^^^^^ + +Generate the complete adaptive stacking dataset: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu \ + --headless \ + --num_envs 1 \ + --generation_num_trials 1000 \ + --input_file ./datasets/annotated_dataset_skillgen.hdf5 \ + --output_file ./datasets/generated_dataset_skillgen_bin_cube_stack.hdf5 \ + --task Isaac-Stack-Cube-Bin-Franka-IK-Rel-Mimic-v0 \ + --use_skillgen + +.. warning:: + + 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 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Similar to the Isaac Lab Mimic workflow, you can train imitation learning policies using the generated SkillGen datasets with Robomimic. + +Basic Cube Stacking Policy +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Train a state-based policy for the basic cube stacking task: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0 \ + --algo bc \ + --dataset ./datasets/generated_dataset_skillgen_cube_stack.hdf5 + +Adaptive Bin Cube Stacking Policy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Train a policy for the more complex adaptive bin stacking: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-Stack-Cube-Bin-Franka-IK-Rel-Mimic-v0 \ + --algo bc \ + --dataset ./datasets/generated_dataset_skillgen_bin_cube_stack.hdf5 + +.. note:: + + The training script will save the model checkpoints in the model directory under ``IssacLab/logs/robomimic``. + +Evaluating Trained Policies +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Test your trained policies: + +.. code:: bash + + # Basic cube stacking evaluation + ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ + --device cpu \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0 \ + --num_rollouts 50 \ + --checkpoint /path/to/model_checkpoint.pth + +.. code:: bash + + # 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 +~~~~~~~~~~~~~~~~~~~~~~~~~ + +This section summarizes the cuRobo planner interface and features. The SkillGen pipeline uses the cuRobo planner to generate collision-free motions between subtask segments. However, the user can use cuRobo as a standalone motion planner for your own tasks. The user can also implement their own motion planner by subclassing the base motion planner and implementing the same API. + +Base Motion Planner (Extensible) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +* Location: ``isaaclab_mimic/motion_planners/base_motion_planner.py`` +* Purpose: Uniform interface for all motion planners used by SkillGen +* Extensibility: New planners can be added by subclassing and implementing the same API; SkillGen consumes the API without code changes + +cuRobo Planner (GPU, collision-aware) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +* Location: ``isaaclab_mimic/motion_planners/curobo`` +* Multi-phase planning: + + * Retreat โ†’ Contact โ†’ Approach phases per subtask + * Configurable collision filtering in contact phases + * For SkillGen, retreat and approach phases are collision-free. The transit phase is collision-checked. + +* World synchronization: + + * Updates robot state, attached objects, and collision spheres from the Isaac Lab scene each trial + * Dynamic attach/detach of objects during grasp/place + +* Collision representation: + + * Contact-aware sphere sets with per-phase enables/filters + +* Outputs: + + * Time-parameterized, collision-checked trajectories for stitching + +* Tests: + + * ``source/isaaclab_mimic/test/test_curobo_planner_cube_stack.py`` + * ``source/isaaclab_mimic/test/test_curobo_planner_franka.py`` + * ``source/isaaclab_mimic/test/test_generate_dataset_skillgen.py`` + +.. list-table:: + :widths: 50 50 + :header-rows: 0 + + * - .. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/cube_stack_end_to_end_curobo.gif + :height: 260px + :align: center + :alt: cuRobo planner test on cube stack using Franka Panda robot + + Cube stack planner test. + - .. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/obstacle_avoidance_curobo.gif + :height: 260px + :align: center + :alt: cuRobo planner test on obstacle avoidance using Franka Panda robot + + Franka planner test. + +These tests can also serve as a reference for how to use cuRobo as a standalone motion planner. + +.. note:: + + For detailed cuRobo config creation and parameters, please see the file ``isaaclab_mimic/motion_planners/curobo/curobo_planner_config.py``. + +Generation Pipeline Integration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +When ``--use_skillgen`` is enabled in ``generate_dataset.py``, the following pipeline is executed: + +1. **Randomize subtask boundaries**: Randomize per-demo start and termination indices for each subtask using task-configured offset ranges. + +2. **Build per-subtask trajectories**: + For each end-effector and subtask: + + - Select a source demonstration segment (strategy-driven; respects coordination/sequential constraints) + - Transform the segment to the current scene (object-relative or coordination delta; optional first-pose interpolation) + - Wrap the transformed segment into a waypoint trajectory + +3. **Transition between subtasks**: + - Plan a collision-aware transition with cuRobo to the subtask's first waypoint (world sync, optional attach/detach), execute the planned waypoints, then resume the subtask trajectory + +4. **Execute with constraints**: + - Execute waypoints step-by-step across end-effectors while enforcing subtask constraints (sequential, coordination with synchronous steps); optionally update planner visualization if enabled + +5. **Record and export**: + - Accumulate states/observations/actions, set the episode success flag, and export the episode (the outer pipeline filters/consumes successes) + +Visualization and Debugging +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Users can visualize the planned trajectories and debug for collisions using Rerun-based plan visualizer. This can be enabled by setting ``visualize_plan = True`` in the cuRobo planner configuration. Note that rerun needs to be installed to visualize the planned trajectories. Refer to Step 3 in :ref:`skillgen-installation` for installation instructions. + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/rerun_cube_stack.gif + :width: 80% + :align: center + :alt: Rerun visualization of planned trajectories and collisions + :figclass: align-center + + Rerun integration: planned trajectories with collision spheres. + +.. note:: + + Check cuRobo usage license in ``docs/licenses/dependencies/cuRobo-license.txt`` diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst new file mode 100644 index 000000000000..14017e65b5da --- /dev/null +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -0,0 +1,1070 @@ +.. _teleoperation-imitation-learning: + +Teleoperation and Imitation Learning with Isaac Lab Mimic +========================================================= + + +Teleoperation +~~~~~~~~~~~~~ + +We provide interfaces for providing commands in SE(2) and SE(3) space +for robot control. In case of SE(2) teleoperation, the returned command +is the linear x-y velocity and yaw rate, while in SE(3), the returned +command is a 6-D vector representing the change in pose. + +.. note:: + + Presently, Isaac Lab Mimic is only supported in Linux. + +To play inverse kinematics (IK) control with a keyboard device: + +.. code:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device keyboard + +For smoother operation and off-axis operation, we recommend using a SpaceMouse as the input device. Providing smoother demonstrations will make it easier for the policy to clone the behavior. To use a SpaceMouse, simply change the teleop device accordingly: + +.. code:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device spacemouse + +.. note:: + + If the SpaceMouse is not detected, you may need to grant additional user permissions by running ``sudo chmod 666 /dev/hidraw<#>`` where ``<#>`` corresponds to the device index + of the connected SpaceMouse. + + To determine the device index, list all ``hidraw`` devices by running ``ls -l /dev/hidraw*``. + Identify the device corresponding to the SpaceMouse by running ``cat /sys/class/hidraw/hidraw<#>/device/uevent`` on each of the devices listed + from the prior step. + + We recommend using local deployment of Isaac Lab to use the SpaceMouse. If using container deployment (:ref:`deployment-docker`), you must manually mount the SpaceMouse to the ``isaac-lab-base`` container by + adding a ``devices`` attribute with the path to the device in your ``docker-compose.yaml`` file: + + .. code:: yaml + + devices: + - /dev/hidraw<#>:/dev/hidraw<#> + + where ``<#>`` is the device index of the connected SpaceMouse. + + If you are using the IsaacLab + CloudXR container deployment (:ref:`cloudxr-teleoperation`), you can add the ``devices`` attribute under the ``services -> isaac-lab-base`` section of the + ``docker/docker-compose.cloudxr-runtime.patch.yaml`` file. + + Isaac Lab is only compatible with the SpaceMouse Wireless and SpaceMouse Compact models from 3Dconnexion. + + +For tasks that benefit from the use of an extended reality (XR) device with hand tracking, Isaac Lab supports using NVIDIA CloudXR to immersively stream the scene to compatible XR devices for teleoperation. Note that when using hand tracking we recommend using the absolute variant of the task (``Isaac-Stack-Cube-Franka-IK-Abs-v0``), which requires the ``handtracking`` device: + +.. code:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Abs-v0 --teleop_device handtracking --device cpu + +.. note:: + + 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, +these are as follows: + +.. code:: text + + Keyboard Controller for SE(3): Se3Keyboard + Reset all commands: R + Toggle gripper (open/close): K + Move arm along x-axis: W/S + Move arm along y-axis: A/D + Move arm along z-axis: Q/E + Rotate arm along x-axis: Z/X + Rotate arm along y-axis: T/G + Rotate arm along z-axis: C/V + +For SpaceMouse, these are as follows: + +.. code:: text + + SpaceMouse Controller for SE(3): Se3SpaceMouse + Reset all commands: Right click + Toggle gripper (open/close): Click the left button on the SpaceMouse + Move arm along x/y-axis: Tilt the SpaceMouse + Move arm along z-axis: Push or pull the SpaceMouse + Rotate arm: Twist the SpaceMouse + +The next section describes how teleoperation devices can be used for data collection for imitation learning. + + +Imitation Learning with Isaac Lab Mimic +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Using the teleoperation devices, it is also possible to collect data for +learning from demonstrations (LfD). For this, we provide scripts to collect data into the open HDF5 format. + +Collecting demonstrations +^^^^^^^^^^^^^^^^^^^^^^^^^ + +To collect demonstrations with teleoperation for the environment ``Isaac-Stack-Cube-Franka-IK-Rel-v0``, use the following commands: + +.. code:: bash + + # step a: create folder for datasets + mkdir -p datasets + # step b: collect data with a selected teleoperation device. Replace with your preferred input device. + # Available options: spacemouse, keyboard, handtracking + ./isaaclab.sh -p scripts/tools/record_demos.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --device cpu --teleop_device --dataset_file ./datasets/dataset.hdf5 --num_demos 10 + # step a: replay the collected dataset + ./isaaclab.sh -p scripts/tools/replay_demos.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --device cpu --dataset_file ./datasets/dataset.hdf5 + + +.. note:: + + The order of the stacked cubes should be blue (bottom), red (middle), green (top). + +.. tip:: + + When using an XR device, we suggest collecting demonstrations with the ``Isaac-Stack-Cube-Frank-IK-Abs-v0`` version of the task and ``--teleop_device handtracking``, which controls the end effector using the absolute position of the hand. + +About 10 successful demonstrations are required in order for the following steps to succeed. + +Here are some tips to perform demonstrations that lead to successful policy training: + +* Keep demonstrations short. Shorter demonstrations mean fewer decisions for the policy, making training easier. +* Take a direct path. Do not follow along arbitrary axis, but move straight toward the goal. +* Do not pause. Perform smooth, continuous motions instead. It is not obvious for a policy why and when to pause, hence continuous motions are easier to learn. + +If, while performing a demonstration, a mistake is made, or the current demonstration should not be recorded for some other reason, press the ``R`` key to discard the current demonstration, and reset to a new starting position. + +.. note:: + Non-determinism may be observed during replay as physics in IsaacLab are not determimnistically reproducible when using ``env.reset``. + +Pre-recorded demonstrations +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +We provide a pre-recorded ``dataset.hdf5`` containing 10 human demonstrations for ``Isaac-Stack-Cube-Franka-IK-Rel-v0`` +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 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Additional demonstrations can be generated using Isaac Lab Mimic. + +Isaac Lab Mimic is a feature in Isaac Lab that allows generation of additional demonstrations automatically, allowing a policy to learn successfully even from just a handful of manual demonstrations. + +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. + +In order to use Isaac Lab Mimic with the recorded dataset, first annotate the subtasks in the recording: + +.. tab-set:: + :sync-group: policy_type + + .. tab-item:: State-based policy + :sync: state + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \ + --device cpu --task Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0 --auto \ + --input_file ./datasets/dataset.hdf5 --output_file ./datasets/annotated_dataset.hdf5 + + .. tab-item:: Visuomotor policy + :sync: visuomotor + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \ + --device cpu --enable_cameras --task Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Mimic-v0 --auto \ + --input_file ./datasets/dataset.hdf5 --output_file ./datasets/annotated_dataset.hdf5 + + +Then, use Isaac Lab Mimic to generate some additional demonstrations: + +.. tab-set:: + :sync-group: policy_type + + .. tab-item:: State-based policy + :sync: state + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu --num_envs 10 --generation_num_trials 10 \ + --input_file ./datasets/annotated_dataset.hdf5 --output_file ./datasets/generated_dataset_small.hdf5 + + .. tab-item:: Visuomotor policy + :sync: visuomotor + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu --enable_cameras --num_envs 10 --generation_num_trials 10 \ + --input_file ./datasets/annotated_dataset.hdf5 --output_file ./datasets/generated_dataset_small.hdf5 + +.. note:: + + The output_file of the ``annotate_demos.py`` script is the input_file to the ``generate_dataset.py`` script + +Inspect the output of generated data (filename: ``generated_dataset_small.hdf5``), and if satisfactory, generate the full dataset: + +.. tab-set:: + :sync-group: policy_type + + .. tab-item:: State-based policy + :sync: state + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu --headless --num_envs 10 --generation_num_trials 1000 \ + --input_file ./datasets/annotated_dataset.hdf5 --output_file ./datasets/generated_dataset.hdf5 + + .. tab-item:: Visuomotor policy + :sync: visuomotor + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu --enable_cameras --headless --num_envs 10 --generation_num_trials 1000 \ + --input_file ./datasets/annotated_dataset.hdf5 --output_file ./datasets/generated_dataset.hdf5 + + +The number of demonstrations can be increased or decreased, 1000 demonstrations have been shown to provide good training results for this task. + +Additionally, the number of environments in the ``--num_envs`` parameter can be adjusted to speed up data generation. +The suggested number of 10 can be executed on a moderate laptop GPU. +On a more powerful desktop machine, use a larger number of environments for a significant speedup of this step. + +Robomimic setup +^^^^^^^^^^^^^^^ + +As an example, we will train a BC agent implemented in `Robomimic `__ to train a policy. Any other framework or training method could be used. + +To install the robomimic framework, use the following commands: + +.. code:: bash + + # install the dependencies + sudo apt install cmake build-essential + # install python module (for robomimic) + ./isaaclab.sh -i robomimic + +Training an agent +^^^^^^^^^^^^^^^^^ + +Using the Mimic generated data we can now train a state-based BC agent for ``Isaac-Stack-Cube-Franka-IK-Rel-v0``, or a visuomotor BC agent for ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0``: + +.. tab-set:: + :sync-group: policy_type + + .. tab-item:: State-based policy + :sync: state + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --algo bc \ + --dataset ./datasets/generated_dataset.hdf5 + + .. tab-item:: Visuomotor policy + :sync: visuomotor + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0 --algo bc \ + --dataset ./datasets/generated_dataset.hdf5 + +.. note:: + By default the trained models and logs will be saved to ``IssacLab/logs/robomimic``. + +Visualizing results +^^^^^^^^^^^^^^^^^^^ + +.. tip:: + + **Important: Testing Multiple Checkpoint Epochs** + + When evaluating policy performance, it is common for different training epochs to yield significantly different results. + If you don't see the expected performance, **always test policies from various epochs** (not just the final checkpoint) + to find the best-performing model. Model performance can vary substantially across training, and the final epoch + is not always optimal. + +By inferencing using the generated model, we can visualize the results of the policy: + +.. tab-set:: + :sync-group: policy_type + + .. tab-item:: State-based policy + :sync: state + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ + --device cpu --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_rollouts 50 \ + --checkpoint /PATH/TO/desired_model_checkpoint.pth + + .. tab-item:: Visuomotor policy + :sync: visuomotor + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ + --device cpu --enable_cameras --task Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0 --num_rollouts 50 \ + --checkpoint /PATH/TO/desired_model_checkpoint.pth + +.. tip:: + + **If you don't see expected performance results:** Test policies from multiple checkpoint epochs, not just the final one. + Policy performance can vary significantly across training epochs, and intermediate checkpoints often outperform the final model. + +.. 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) + * **Recommendation:** Evaluate checkpoints from various epochs throughout training to identify the best-performing model + + +Demo 1: Data Generation and Policy Training for a Humanoid Robot +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/gr-1_steering_wheel_pick_place.gif + :width: 100% + :align: center + :alt: GR-1 humanoid robot performing a pick and place task + :figclass: align-center + + +Isaac Lab Mimic supports data generation for robots with multiple end effectors. In the following demonstration, we will show how to generate data +to train a Fourier GR-1 humanoid robot to perform a pick and place task. + +Optional: Collect and annotate demonstrations +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Collect human demonstrations +"""""""""""""""""""""""""""" +.. note:: + + Data collection for the GR-1 humanoid robot environment requires use of an Apple Vision Pro headset. If you do not have access to + an Apple Vision Pro, you may skip this step and continue on to the next step: `Generate the dataset`_. + A pre-recorded annotated dataset is provided in the next step. + +.. tip:: + The GR1 scene utilizes the wrist poses from the Apple Vision Pro (AVP) as setpoints for a differential IK controller (Pink-IK). + The differential IK controller requires the user's wrist pose to be close to the robot's initial or current pose for optimal performance. + Rapid movements of the user's wrist may cause it to deviate significantly from the goal state, which could prevent the IK controller from finding the optimal solution. + This may result in a mismatch between the user's wrist and the robot's wrist. + You can increase the gain of all the `Pink-IK controller's FrameTasks `__ to track the AVP wrist poses with lower latency. + However, this may lead to more jerky motion. + Separately, the finger joints of the robot are retargeted to the user's finger joints using the `dex-retargeting `_ library. + +Set up the CloudXR Runtime and Apple Vision Pro for teleoperation by following the steps in :ref:`cloudxr-teleoperation`. +CPU simulation is used in the following steps for better XR performance when running a single environment. + +Collect a set of human demonstrations. +A success demo requires the object to be placed in the bin and for the robot's right arm to be retracted to the starting position. + +The Isaac Lab Mimic Env GR-1 humanoid robot is set up such that the left hand has a single subtask, while the right hand has two subtasks. +The first subtask involves the right hand remaining idle while the left hand picks up and moves the object to the position where the right hand will grasp it. +This setup allows Isaac Lab Mimic to interpolate the right hand's trajectory accurately by using the object's pose, especially when poses are randomized during data generation. +Therefore, avoid moving the right hand while the left hand picks up the object and brings it to a stable position. + + +.. |good_demo| image:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/gr-1_steering_wheel_pick_place_good_demo.gif + :width: 49% + :alt: GR-1 humanoid robot performing a good pick and place demonstration + +.. |bad_demo| image:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/gr-1_steering_wheel_pick_place_bad_demo.gif + :width: 49% + :alt: GR-1 humanoid robot performing a bad pick and place demonstration + +|good_demo| |bad_demo| + +.. centered:: Left: A good human demonstration with smooth and steady motion. Right: A bad demonstration with jerky and exaggerated motion. + + +Collect five demonstrations by running the following command: + +.. code:: bash + + ./isaaclab.sh -p scripts/tools/record_demos.py \ + --device cpu \ + --task Isaac-PickPlace-GR1T2-Abs-v0 \ + --teleop_device handtracking \ + --dataset_file ./datasets/dataset_gr1.hdf5 \ + --num_demos 5 --enable_pinocchio + +.. note:: + We also provide a GR-1 pick and place task with waist degrees-of-freedom enabled ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0`` (see :ref:`environments` for details on the available environments, including the GR1 Waist Enabled variant). The same command above applies but with the task name changed to ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0``. + +.. tip:: + If a demo fails during data collection, the environment can be reset using the teleoperation controls panel in the XR teleop client + on the Apple Vision Pro or via voice control by saying "reset". See :ref:`teleoperate-apple-vision-pro` for more details. + + The robot uses simplified collision meshes for physics calculations that differ from the detailed visual meshes displayed in the simulation. Due to this difference, you may occasionally observe visual artifacts where parts of the robot appear to penetrate other objects or itself, even though proper collision handling is occurring in the physics simulation. + +You can replay the collected demonstrations by running the following command: + +.. code:: bash + + ./isaaclab.sh -p scripts/tools/replay_demos.py \ + --device cpu \ + --task Isaac-PickPlace-GR1T2-Abs-v0 \ + --dataset_file ./datasets/dataset_gr1.hdf5 --enable_pinocchio + +.. note:: + Non-determinism may be observed during replay as physics in IsaacLab are not determimnistically reproducible when using ``env.reset``. + + +Annotate the demonstrations +""""""""""""""""""""""""""" + +Unlike the prior Franka stacking task, the GR-1 pick and place task uses manual annotation to define subtasks. + +The pick and place task has one subtask for the left arm (pick) and two subtasks for the right arm (idle, place). +Annotations denote the end of a subtask. For the pick and place task, this means there are no annotations for the left arm and one annotation for the right arm (the end of the final subtask is always implicit). + +Each demo requires a single annotation between the first and second subtask of the right arm. This annotation ("S" button press) should be done when the right robot arm finishes the "idle" subtask and begins to +move towards the target object. An example of a correct annotation is shown below: + +.. figure:: ../../_static/tasks/manipulation/gr-1_pick_place_annotation.jpg + :width: 100% + :align: center + +Annotate the demonstrations by running the following command: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \ + --device cpu \ + --task Isaac-PickPlace-GR1T2-Abs-Mimic-v0 \ + --input_file ./datasets/dataset_gr1.hdf5 \ + --output_file ./datasets/dataset_annotated_gr1.hdf5 --enable_pinocchio + +.. note:: + + The script prints the keyboard commands for manual annotation and the current subtask being annotated: + + .. code:: text + + Annotating episode #0 (demo_0) + Playing the episode for subtask annotations for eef "right". + Subtask signals to annotate: + - Termination: ['idle_right'] + + Press "N" to begin. + Press "B" to pause. + Press "S" to annotate subtask signals. + Press "Q" to skip the episode. + +.. tip:: + + If the object does not get placed in the bin during annotation, you can press "N" to replay the episode and annotate again. Or you can press "Q" to skip the episode and annotate the next one. + +Generate the dataset +^^^^^^^^^^^^^^^^^^^^ + +If you skipped the prior collection and annotation step, download the pre-recorded annotated dataset ``dataset_annotated_gr1.hdf5`` from +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 + + ./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_gr1.hdf5 --output_file ./datasets/generated_dataset_gr1.hdf5 + +Train a policy +^^^^^^^^^^^^^^ + +Use `Robomimic `__ to train a policy for the generated dataset. + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-PickPlace-GR1T2-Abs-v0 --algo bc \ + --normalize_training_actions \ + --dataset ./datasets/generated_dataset_gr1.hdf5 + +The training script will normalize the actions in the dataset to the range [-1, 1]. +The normalization parameters are saved in the model directory under ``PATH_TO_MODEL_DIRECTORY/logs/normalization_params.txt``. +Record the normalization parameters for later use in the visualization step. + +.. note:: + By default the trained models and logs will be saved to ``IssacLab/logs/robomimic``. + +Visualize the results +^^^^^^^^^^^^^^^^^^^^^ + +Visualize the results of the trained policy by running the following command, using the normalization parameters recorded in the prior training step: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ + --device cpu \ + --enable_pinocchio \ + --task Isaac-PickPlace-GR1T2-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. + +.. tip:: + + **If you don't see expected performance results:** It is critical to test policies from various checkpoint epochs. + Performance can vary significantly between epochs, and the best-performing checkpoint is often not the final one. + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/gr-1_steering_wheel_pick_place_policy.gif + :width: 100% + :align: center + :alt: GR-1 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 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 1000th and 2000th epochs** to select the best-performing policy. Testing various epochs is essential for finding optimal performance. + + +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 + +.. note:: + **Locomotion policy training** + + The locomotion policy used in this integration example was trained using the `AGILE `__ framework. + AGILE is an officially supported humanoid control training pipeline that leverages the manager based environment in Isaac Lab. It will also be + seamlessly integrated with other evaluation and deployment tools across Isaac products. This allows teams to rely on a single, maintained stack + covering all necessary infrastructure and tooling for policy training, with easy export to real-world deployment. The AGILE repository contains + updated pre-trained policies with separate upper and lower body policies for flexibtility. They have been verified in the real world and can be + directly deployed. Users can also train their own locomotion or whole-body control policies using the AGILE framework. + +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 + + +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. + +.. tip:: + + **If you don't see expected performance results:** Always test policies from various checkpoint epochs. + Different epochs can produce significantly different results, so evaluate multiple checkpoints to find the optimal model. + +.. 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 1000th and 2000th epochs** to select the best-performing policy. Testing various epochs is essential for finding optimal performance. + +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`` +(**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:: + + If desired, data collection, annotation, and generation can be done using the same commands as the prior examples. + + The robot first picks up the red beaker and pours the contents into the yellow bowl. + 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. + + **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: + + .. code:: bash + + ./isaaclab.sh -p scripts/tools/record_demos.py \ + --device cpu \ + --task Isaac-NutPour-GR1T2-Pink-IK-Abs-v0 \ + --teleop_device handtracking \ + --dataset_file ./datasets/dataset_gr1_nut_pouring.hdf5 \ + --num_demos 5 --enable_pinocchio + + Since this is a visuomotor environment, the ``--enable_cameras`` flag must be added to the annotation and data generation commands. + + To annotate the demonstrations: + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \ + --device cpu \ + --enable_cameras \ + --rendering_mode balanced \ + --task Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic-v0 \ + --input_file ./datasets/dataset_gr1_nut_pouring.hdf5 \ + --output_file ./datasets/dataset_annotated_gr1_nut_pouring.hdf5 --enable_pinocchio + + .. warning:: + There are multiple right eef annotations for this task. Annotations for subtasks for the same eef cannot have the same action index. + Make sure to annotate the right eef subtasks with different action indices. + + + To generate the dataset: + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu \ + --headless \ + --enable_pinocchio \ + --enable_cameras \ + --rendering_mode balanced \ + --task Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic-v0 \ + --generation_num_trials 1000 \ + --num_envs 5 \ + --input_file ./datasets/dataset_annotated_gr1_nut_pouring.hdf5 \ + --output_file ./datasets/generated_dataset_gr1_nut_pouring.hdf5 + + +Train a policy +^^^^^^^^^^^^^^ + +Use `Robomimic `__ to train a visuomotor BC agent for the task. + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-NutPour-GR1T2-Pink-IK-Abs-v0 --algo bc \ + --normalize_training_actions \ + --dataset ./datasets/generated_dataset_gr1_nut_pouring.hdf5 + +The training script will normalize the actions in the dataset to the range [-1, 1]. +The normalization parameters are saved in the model directory under ``PATH_TO_MODEL_DIRECTORY/logs/normalization_params.txt``. +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 +^^^^^^^^^^^^^^^^^^^^^ + +Visualize the results of the trained policy by running the following command, using the normalization parameters recorded in the prior training step: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ + --device cpu \ + --enable_pinocchio \ + --enable_cameras \ + --rendering_mode balanced \ + --task Isaac-NutPour-GR1T2-Pink-IK-Abs-v0 \ + --num_rollouts 50 \ + --horizon 350 \ + --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. + +.. tip:: + + **If you don't see expected performance results:** Test policies from various checkpoint epochs, not just the final one. + Policy performance can vary substantially across training, and intermediate checkpoints often yield better results. + +.. 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 + + 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. Testing various epochs is critical for achieving optimal performance. + +.. _common-pitfalls-generating-data: + +Common Pitfalls when Generating Data +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +**Demonstrations are too long:** + +* Longer time horizon is harder to learn for a policy +* Start close to the first object and minimize motions + +**Demonstrations are not smooth:** + +* Irregular motion is hard for policy to decipher +* Better teleop devices result in better data (i.e. SpaceMouse is better than Keyboard) + +**Pauses in demonstrations:** + +* Pauses are difficult to learn +* Keep the human motions smooth and fluid + +**Excessive number of subtasks:** + +* Minimize the number of defined subtasks for completing a given task +* Less subtacks results in less stitching of trajectories, yielding higher data generation success rate + +**Lack of action noise:** + +* Action noise makes policies more robust + +**Recording cropped too tight:** + +* If recording stops on the frame the success term triggers, it may not re-trigger during replay +* Allow for some buffer at the end of recording + +**Non-deterministic replay:** + +* Physics in IsaacLab are not deterministically reproducible when using ``env.reset`` so demonstrations may fail on replay +* Collect more human demos than needed, use the ones that succeed during annotation +* All data in Isaac Lab Mimic generated HDF5 file represent a successful demo and can be used for training (even if non-determinism causes failure when replayed) + + +Creating Your Own Isaac Lab Mimic Compatible Environments +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +How it works +^^^^^^^^^^^^ + +Isaac Lab Mimic works by splitting the input demonstrations into subtasks. Subtasks are user-defined segments in the demonstrations that are common to all demonstrations. Examples for subtasks are "grasp an object", "move end effector to some pre-defined position", "release object" etc.. Note that most subtasks are defined with respect to some object that the robot interacts with. + +Subtasks need to be defined, and then annotated for each input demonstration. Annotation can either happen algorithmically by defining heuristics for subtask detection, as was done in the example above, or it can be done manually. + +With subtasks defined and annotated, Isaac Lab Mimic utilizes a small number of helper methods to then transform the subtask segments, and generate new demonstrations by stitching them together to match the new task at hand. + +For each thusly generated candidate demonstration, Isaac Lab Mimic uses a boolean success criteria to determine whether the demonstration succeeded in performing the task, and if so, add it to the output dataset. Success rate of candidate demonstrations can be as high as 70% in simple cases, and as low as <1%, depending on the difficulty of the task, and the complexity of the robot itself. + +Configuration and subtask definition +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Subtasks, among other configuration settings for Isaac Lab Mimic, are defined in a Mimic compatible environment configuration class that is created by extending the existing environment config with additional Mimic required parameters. + +All Mimic required config parameters are specified in the :class:`~isaaclab.envs.MimicEnvCfg` class. + +The config class :class:`~isaaclab_mimic.envs.FrankaCubeStackIKRelMimicEnvCfg` serves as an example of creating a Mimic compatible environment config class for the Franka stacking task that was used in the examples above. + +The ``DataGenConfig`` member contains various parameters that influence how data is generated. It is initially sufficient to just set the ``name`` parameter, and revise the rest later. + +Subtasks are a list of :class:`~isaaclab.envs.SubTaskConfig` objects, of which the most important members are: + +* ``object_ref`` is the object that is being interacted with. This will be used to adjust motions relative to this object during data generation. Can be ``None`` if the current subtask does not involve any object. +* ``subtask_term_signal`` is the ID of the signal indicating whether the subtask is active or not. + +For multi end-effector environments, subtask ordering between end-effectors can be enforced by specifying subtask constraints. These constraints are defined in the :class:`~isaaclab.envs.SubTaskConstraintConfig` class. + +Subtask annotation +^^^^^^^^^^^^^^^^^^ + +Once the subtasks are defined, they need to be annotated in the source data. There are two methods to annotate source demonstrations for subtask boundaries: Manual annotation or using heuristics. + +It is often easiest to perform manual annotations, since the number of input demonstrations is usually very small. To perform manual annotations, use the ``annotate_demos.py`` script without the ``--auto`` flag. Then press ``B`` to pause, ``N`` to continue, and ``S`` to annotate a subtask boundary. + +For more accurate boundaries, or to speed up repeated processing of a given task for experiments, heuristics can be implemented to perform the same task. Heuristics are observations in the environment. An example how to add subtask terms can be found in ``source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py``, where they are added as an observation group called ``SubtaskCfg``. This example is using prebuilt heuristics, but custom heuristics are easily implemented. + + +Helpers for demonstration generation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Helpers needed for Isaac Lab Mimic are defined in the environment. All tasks that are to be used with Isaac Lab Mimic are derived from the :class:`~isaaclab.envs.ManagerBasedRLMimicEnv` base class, and must implement the following functions: + +* ``get_robot_eef_pose``: Returns the current robot end effector pose in the same frame as used by the robot end effector controller. + +* ``target_eef_pose_to_action``: Takes a target pose and a gripper action for the end effector controller and returns an action which achieves the target pose. + +* ``action_to_target_eef_pose``: Takes an action and returns a target pose for the end effector controller. + +* ``actions_to_gripper_actions``: Takes a sequence of actions and returns the gripper actuation part of the actions. + +* ``get_object_poses``: Returns the pose of each object in the scene that is used for data generation. + +* ``get_subtask_term_signals``: Returns a dictionary of binary flags for each subtask in a task. The flag of true is set when the subtask has been completed and false otherwise. + +The class :class:`~isaaclab_mimic.envs.FrankaCubeStackIKRelMimicEnv` shows an example of creating a Mimic compatible environment from an existing Isaac Lab environment. + +Registering the environment +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Once both Mimic compatible environment and environment config classes have been created, a new Mimic compatible environment can be registered using ``gym.register``. For the Franka stacking task in the examples above, the Mimic environment is registered as ``Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0``. + +The registered environment is now ready to be used with Isaac Lab Mimic. + + +Tips for Successful Data Generation with Isaac Lab Mimic +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Splitting subtasks +^^^^^^^^^^^^^^^^^^ + +A general rule of thumb is to split the task into as few subtasks as possible, while still being able to complete the task. Isaac Lab Mimic data generation uses linear interpolation to bridge and stitch together subtask segments. +More subtasks result in more stitching of trajectories which can result in less smooth motions and more failed demonstrations. For this reason, it is often best to annoatate subtask boundaries where the robot's motion is unlikely to collide with other objects. + +For example, in the scenario below, there is a subtask partition after the robot's left arm grasps the object. On the left, the subtask annotation is marked immediately after the grasp, while on the right, the annotation is marked after the robot has grasped and lifted the object. +In the left case, the interpolation causes the robot's left arm to collide with the table and it's motion lags while on the right the motion is continuous and smooth. + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/lagging_subtask.gif + :width: 99% + :align: center + :alt: Subtask splitting example + :figclass: align-center + +.. centered:: Motion lag/collision caused by poor subtask splitting (left) + + +Selecting number of interpolation steps +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The number of interpolation steps between subtask segments can be specified in the :class:`~isaaclab.envs.SubTaskConfig` class. Once transformed, the subtask segments don't start/end at the same spot, thus to create a continuous motion, Isaac Lab Mimic +will apply linear interpolation between the last point of the previous subtask and the first point of the next subtask. + +The number of interpolation steps can be tuned to control the smoothness of the generated demonstrations during this stitching process. +The appropriate number of interpolation steps depends on the speed of the robot and the complexity of the task. A complex task with a large object reset distribution will have larger gaps between subtask segments and require more interpolation steps to create a smooth motion. +Alternatively, a task with small gaps between subtask segments should use a small number of interpolation steps to avoid unnecessary motion lag caused by too many steps. + +An example of how the number of interpolation steps can affect the generated demonstrations is shown below. +In the example, an interpolation is applied to the right arm of the robot to bridge the gap between the left arm's grasp and the right arm's placement. With 0 steps, the right arm exhibits a jerky jump in motion while with 20 steps, the motion is laggy. With 5 steps, the motion is +smooth and natural. + +.. |0_interp_steps| image:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/0_interpolation_steps.gif + :width: 32% + :alt: GR-1 robot with 0 interpolation steps + +.. |5_interp_steps| image:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/5_interpolation_steps.gif + :width: 32% + :alt: GR-1 robot with 5 interpolation steps + +.. |20_interp_steps| image:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/20_interpolation_steps.gif + :width: 32% + :alt: GR-1 robot with 20 interpolation steps + +|0_interp_steps| |5_interp_steps| |20_interp_steps| + +.. centered:: Left: 0 steps. Middle: 5 steps. Right: 20 steps. diff --git a/docs/source/overview/own-project/index.rst b/docs/source/overview/own-project/index.rst new file mode 100644 index 000000000000..36ef4443f5b1 --- /dev/null +++ b/docs/source/overview/own-project/index.rst @@ -0,0 +1,14 @@ +.. _own-project: + +Build your Own Project or Task +============================== + +To get started, first create a new project or task with the template generator :ref:`template-generator`. +For more detailed information on how your project is structured, see :ref:`project-structure`. + +.. toctree:: + :maxdepth: 1 + :titlesonly: + + template + project_structure diff --git a/docs/source/overview/own-project/project_structure.rst b/docs/source/overview/own-project/project_structure.rst new file mode 100644 index 000000000000..a0e17f0344d4 --- /dev/null +++ b/docs/source/overview/own-project/project_structure.rst @@ -0,0 +1,44 @@ +.. _project-structure: + + +Project Structure +================= + +There are four nested structures you need to be aware of when working in the direct workflow with an Isaac Lab template +project: the **Project**, the **Extension**, the **Modules**, and the **Task**. + +.. figure:: ../../_static/setup/walkthrough_project_setup.svg + :align: center + :figwidth: 100% + :alt: The structure of the isaac lab template project. + +The **Project** is the root directory of the generated template. It contains the source and scripts directories, as well as +a ``README.md`` file. When we created the template, we named the project *IsaacLabTutorial* and this defined the root directory +of a git repository. If you examine the project root with hidden files visible you will see a number of files defining +the behavior of the project with respect to git. The ``scripts`` directory contains the ``train.py`` and ``play.py`` scripts for the +various RL libraries you chose when generating the template, while the source directory contains the python packages for the project. + +The **Extension** is the name of the python package we installed via pip. By default, the template generates a project +with a single extension of the same name. A project can have multiple extensions, and so they are kept in a common ``source`` +directory. Traditional python packages are defined by the presence of a ``pyproject.toml`` file that describes the package +metadata, but packages using Isaac Lab must also be Isaac Sim extensions and so require a ``config`` directory and an accompanying +``extension.toml`` file that describes the metadata needed by the Isaac Sim extension manager. Finally, because the template +is intended to be installed via pip, it needs a ``setup.py`` file to complete the setup procedure using the ``extension.toml`` +config. A project can have multiple extensions, as evidenced by the Isaac Lab repository itself! + +The **Modules** are what actually gets loaded by Isaac Lab to run training (the meat of the code). By default, the template +generates an extension with a single module that is named the same as the project. The structure of the various sub-modules +in the extension is what determines the ``entry_point`` for an environment in Isaac Lab. This is why our template project needed +to be installed before we could call ``train.py``: the path to the necessary components to run the task needed to be exposed +to python for Isaac Lab to find them. + +Finally, the **Task** is the heart of the direct workflow. By default, the template generates a single task with the same name +as the project. The environment and configuration files are stored here, as well as placeholder, RL library dependent ``agents``. +Critically, note the contents of the ``__init__.py``! Specifically, the ``gym.register`` function needs to be called at least once +before an environment and task can be used with the Isaac Lab ``train.py`` and ``play.py`` scripts. +This function should be included in one of the module ``__init__.py`` files so it is called at installation. The path to +this init file is what defines the entry point for the task! + +For the template, ``gym.register`` is called within ``isaac_lab_tutorial/source/isaac_lab_tutorial/isaac_lab_tutorial/tasks/direct/isaac_lab_tutorial/__init__.py``. +The repeated name is a consequence of needing default names for the template, but now we can see the structure of the project. +**Project**/source/**Extension**/**Module**/tasks/direct/**Task**/__init__.py diff --git a/docs/source/overview/own-project/template.rst b/docs/source/overview/own-project/template.rst new file mode 100644 index 000000000000..cb52effde62c --- /dev/null +++ b/docs/source/overview/own-project/template.rst @@ -0,0 +1,230 @@ +.. _template-generator: + + +Create new project or task +========================== + +Traditionally, building new projects that utilize Isaac Lab's features required creating your own +extensions within the Isaac Lab repository. However, this approach can obscure project visibility and +complicate updates from one version of Isaac Lab to another. To circumvent these challenges, +we now provide a command-line tool (**template generator**) for creating Isaac Lab-based projects and tasks. + +The template generator enables you to create an: + +* **External project** (recommended): An isolated project that is not part of the Isaac Lab repository. This approach + works outside of the core Isaac Lab repository, ensuring that your development efforts remain self-contained. Also, + it allows your code to be run as an extension in Omniverse. + + .. hint:: + + For the external project, the template generator will initialize a new Git repository in the specified directory. + You can push the generated content to your own remote repository (e.g. GitHub) and share it with others. + +* **Internal task**: A task that is part of the Isaac Lab repository. This approach should only be used to create + new tasks within the Isaac Lab repository in order to contribute to it. + + .. warning:: + + Pip installations of Isaac Lab do not support *Internal* templates. + If ``isaaclab`` is loaded from ``site-packages`` or ``dist-packages``, the *Internal* option is disabled + and the *External* template will be used instead. + +Running the template generator +------------------------------ + +Install Isaac Lab by following the `installation guide <../../setup/installation/index.html>`_. +We recommend using conda or uv installation as it simplifies calling Python scripts from the terminal. + +Then, run the following command to generate a new external project or internal task: + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + ./isaaclab.sh --new # or "./isaaclab.sh -n" + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + isaaclab.bat --new :: or "isaaclab.bat -n" + +The generator will guide you in setting up the project/task for your needs by asking you the following questions: + +* Type of project/task (external or internal), and project/task path or names according to the selected type. +* Isaac Lab workflows (see :ref:`feature-workflows`). +* Reinforcement learning libraries (see :ref:`rl-frameworks`), and algorithms (if the selected libraries support multiple algorithms). + +External project usage (once generated) +--------------------------------------- + +Once the external project is generated, a ``README.md`` file will be created in the specified directory. +This file will contain instructions on how to install the project and run the tasks. + +Here are some general commands to get started with it: + +.. note:: + + If Isaac Lab is not installed in a conda environment or in a (virtual) Python environment, use ``FULL_PATH_TO_ISAACLAB/isaaclab.sh -p`` + (or ``FULL_PATH_TO_ISAACLAB\isaaclab.bat -p`` on Windows) instead of ``python`` to run the commands below. + +* Install the project (in editable mode). + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + python -m pip install -e source/ + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python -m pip install -e source\ + +* List the tasks available in the project. + + .. warning:: + + If the task names change, it may be necessary to update the search pattern ``"Template-"`` + (in the ``scripts/list_envs.py`` file) so that they can be listed. + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + python scripts/list_envs.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python scripts\list_envs.py + +* Run a task. + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + python scripts//train.py --task= + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python scripts\\train.py --task= + +For more details, please follow the instructions in the generated project's ``README.md`` file. + +Internal task usage (once generated) +--------------------------------------- + +Once the internal task is generated, it will be available along with the rest of the Isaac Lab tasks. + +Here are some general commands to get started with it: + +.. note:: + + If Isaac Lab is not installed in a conda environment or in a (virtual) Python environment, use ``./isaaclab.sh -p`` + (or ``isaaclab.bat -p`` on Windows) instead of ``python`` to run the commands below. + +* List the tasks available in Isaac Lab. + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + python scripts/environments/list_envs.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python scripts\environments\list_envs.py + +* Run a task. + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + python scripts/reinforcement_learning//train.py --task= + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python scripts\reinforcement_learning\\train.py --task= + +* Run a task with dummy agents. + + These include dummy agents that output zero or random agents. They are useful to ensure that the environments are configured correctly. + + * Zero-action agent + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + python scripts/zero_agent.py --task= + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python scripts\zero_agent.py --task= + + * Random-action agent + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + python scripts/random_agent.py --task= + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python scripts\random_agent.py --task= diff --git a/docs/source/overview/reinforcement-learning/rl_existing_scripts.rst b/docs/source/overview/reinforcement-learning/rl_existing_scripts.rst index b48fb494b9ca..9ffd47b401e2 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``: @@ -65,7 +71,7 @@ RSL-RL # run script for training ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Reach-Franka-v0 --headless # run script for playing with 32 environments - ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/play.py --task Isaac-Reach-Franka-v0 --num_envs 32 --load_run run_folder_name --checkpoint model.pt + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/play.py --task Isaac-Reach-Franka-v0 --num_envs 32 --load_run run_folder_name --checkpoint /PATH/TO/model.pt # run script for playing a pre-trained checkpoint with 32 environments ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/play.py --task Isaac-Reach-Franka-v0 --num_envs 32 --use_pretrained_checkpoint # run script for recording video of a trained agent (requires installing `ffmpeg`) @@ -81,12 +87,46 @@ RSL-RL :: run script for training isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\train.py --task Isaac-Reach-Franka-v0 --headless :: run script for playing with 32 environments - isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\play.py --task Isaac-Reach-Franka-v0 --num_envs 32 --load_run run_folder_name --checkpoint model.pt + isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\play.py --task Isaac-Reach-Franka-v0 --num_envs 32 --load_run run_folder_name --checkpoint /PATH/TO/model.pt :: run script for playing a pre-trained checkpoint with 32 environments isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\play.py --task Isaac-Reach-Franka-v0 --num_envs 32 --use_pretrained_checkpoint :: run script for recording video of a trained agent (requires installing `ffmpeg`) isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\play.py --task Isaac-Reach-Franka-v0 --headless --video --video_length 200 +- Training and distilling an agent with + `RSL-RL `__ on ``Isaac-Velocity-Flat-Anymal-D-v0``: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # install python module (for rsl-rl) + ./isaaclab.sh -i rsl_rl + # run script for rl training of the teacher agent + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --headless + # run script for distilling the teacher agent into a student agent + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --headless --agent rsl_rl_distillation_cfg_entry_point --load_run teacher_run_folder_name + # run script for playing the student with 64 environments + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/play.py --task Isaac-Velocity-Flat-Anymal-D-v0 --num_envs 64 --agent rsl_rl_distillation_cfg_entry_point + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + :: install python module (for rsl-rl) + isaaclab.bat -i rsl_rl + :: run script for rl training of the teacher agent + isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --headless + :: run script for distilling the teacher agent into a student agent + isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --headless --agent rsl_rl_distillation_cfg_entry_point --load_run teacher_run_folder_name + :: run script for playing the student with 64 environments + isaaclab.bat -p scripts\reinforcement_learning\rsl_rl\play.py --task Isaac-Velocity-Flat-Anymal-D-v0 --num_envs 64 --agent rsl_rl_distillation_cfg_entry_point + SKRL ---- @@ -136,14 +176,19 @@ SKRL .. warning:: - It is recommended to `install JAX `_ manually before proceeding to install skrl and its dependencies, as JAX installs its CPU version by default. For example, ``pip install -U "jax[cuda12]"`` can be used to install JAX for CUDA 12. + It is recommended to `install JAX `_ manually before proceeding to install skrl and its dependencies, as JAX installs its CPU version by default. Visit the **skrl** `installation `_ page for more details. 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" "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"] # run script for training @@ -187,7 +232,7 @@ Stable-Baselines3 - Training an agent with `Stable-Baselines3 `__ - on ``Isaac-Cartpole-v0``: + on ``Isaac-Velocity-Flat-Unitree-A1-v0``: .. tab-set:: :sync-group: os @@ -200,14 +245,13 @@ Stable-Baselines3 # install python module (for stable-baselines3) ./isaaclab.sh -i sb3 # run script for training - # note: we set the device to cpu since SB3 doesn't optimize for GPU anyway - ./isaaclab.sh -p scripts/reinforcement_learning/sb3/train.py --task Isaac-Cartpole-v0 --headless --device cpu + ./isaaclab.sh -p scripts/reinforcement_learning/sb3/train.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --headless # run script for playing with 32 environments - ./isaaclab.sh -p scripts/reinforcement_learning/sb3/play.py --task Isaac-Cartpole-v0 --num_envs 32 --checkpoint /PATH/TO/model.zip + ./isaaclab.sh -p scripts/reinforcement_learning/sb3/play.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --num_envs 32 --checkpoint /PATH/TO/model.zip # run script for playing a pre-trained checkpoint with 32 environments - ./isaaclab.sh -p scripts/reinforcement_learning/sb3/play.py --task Isaac-Cartpole-v0 --num_envs 32 --use_pretrained_checkpoint + ./isaaclab.sh -p scripts/reinforcement_learning/sb3/play.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --num_envs 32 --use_pretrained_checkpoint # run script for recording video of a trained agent (requires installing `ffmpeg`) - ./isaaclab.sh -p scripts/reinforcement_learning/sb3/play.py --task Isaac-Cartpole-v0 --headless --video --video_length 200 + ./isaaclab.sh -p scripts/reinforcement_learning/sb3/play.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --headless --video --video_length 200 .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows @@ -217,14 +261,13 @@ Stable-Baselines3 :: install python module (for stable-baselines3) isaaclab.bat -i sb3 :: run script for training - :: note: we set the device to cpu since SB3 doesn't optimize for GPU anyway - isaaclab.bat -p scripts\reinforcement_learning\sb3\train.py --task Isaac-Cartpole-v0 --headless --device cpu + isaaclab.bat -p scripts\reinforcement_learning\sb3\train.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --headless :: run script for playing with 32 environments - isaaclab.bat -p scripts\reinforcement_learning\sb3\play.py --task Isaac-Cartpole-v0 --num_envs 32 --checkpoint /PATH/TO/model.zip + isaaclab.bat -p scripts\reinforcement_learning\sb3\play.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --num_envs 32 --checkpoint /PATH/TO/model.zip :: run script for playing a pre-trained checkpoint with 32 environments - isaaclab.bat -p scripts\reinforcement_learning\sb3\play.py --task Isaac-Cartpole-v0 --num_envs 32 --use_pretrained_checkpoint + isaaclab.bat -p scripts\reinforcement_learning\sb3\play.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --num_envs 32 --use_pretrained_checkpoint :: run script for recording video of a trained agent (requires installing `ffmpeg`) - isaaclab.bat -p scripts\reinforcement_learning\sb3\play.py --task Isaac-Cartpole-v0 --headless --video --video_length 200 + isaaclab.bat -p scripts\reinforcement_learning\sb3\play.py --task Isaac-Velocity-Flat-Unitree-A1-v0 --headless --video --video_length 200 All the scripts above log the training progress to `Tensorboard`_ in the ``logs`` directory in the root of the repository. The logs directory follows the pattern ``logs///``, where ```` diff --git a/docs/source/overview/reinforcement-learning/rl_frameworks.rst b/docs/source/overview/reinforcement-learning/rl_frameworks.rst index 80d41412aacd..5f9d25e06e05 100644 --- a/docs/source/overview/reinforcement-learning/rl_frameworks.rst +++ b/docs/source/overview/reinforcement-learning/rl_frameworks.rst @@ -27,7 +27,7 @@ Feature Comparison - Stable Baselines3 * - Algorithms Included - PPO, SAC, A2C - - PPO + - PPO, Distillation - `Extensive List `__ - `Extensive List `__ * - Vectorized Training @@ -37,7 +37,7 @@ Feature Comparison - No * - Distributed Training - Yes - - No + - Yes - Yes - No * - ML Frameworks Supported @@ -71,17 +71,26 @@ Training Performance -------------------- We performed training with each RL library on the same ``Isaac-Humanoid-v0`` environment -with ``--headless`` on a single RTX 4090 GPU -and logged the total training time for 65.5M steps for each RL library. +with ``--headless`` on a single NVIDIA GeForce RTX 4090 and logged the total training time +for 65.5M steps (4096 environments x 32 rollout steps x 500 iterations). +--------------------+-----------------+ | RL Library | Time in seconds | +====================+=================+ -| RL-Games | 203 | +| RL-Games | 201 | +--------------------+-----------------+ -| SKRL | 204 | +| SKRL | 201 | +--------------------+-----------------+ -| RSL RL | 207 | +| RSL RL | 198 | +--------------------+-----------------+ -| Stable-Baselines3 | 6320 | +| Stable-Baselines3 | 287 | +--------------------+-----------------+ + +Training commands (check for the *'Training time: XXX seconds'* line in the terminal output): + +.. code:: bash + + python scripts/reinforcement_learning/rl_games/train.py --task Isaac-Humanoid-v0 --max_iterations 500 --headless + python scripts/reinforcement_learning/skrl/train.py --task Isaac-Humanoid-v0 --max_iterations 500 --headless + python scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Humanoid-v0 --max_iterations 500 --headless + python scripts/reinforcement_learning/sb3/train.py --task Isaac-Humanoid-v0 --max_iterations 500 --headless diff --git a/docs/source/overview/showroom.rst b/docs/source/overview/showroom.rst index 7005ef650c7e..bb2248375749 100644 --- a/docs/source/overview/showroom.rst +++ b/docs/source/overview/showroom.rst @@ -8,7 +8,8 @@ interfaces within a code in a minimal way. A few quick showroom scripts to run and checkout: -- Spawn different quadrupeds and make robots stand using position commands: + +- Spawn different arms and apply random joint position commands: .. tab-set:: :sync-group: os @@ -18,20 +19,21 @@ A few quick showroom scripts to run and checkout: .. code:: bash - ./isaaclab.sh -p scripts/demos/quadrupeds.py + ./isaaclab.sh -p scripts/demos/arms.py .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows .. code:: batch - isaaclab.bat -p scripts\demos\quadrupeds.py + isaaclab.bat -p scripts\demos\arms.py - .. image:: ../_static/demos/quadrupeds.jpg + .. image:: ../_static/demos/arms.jpg :width: 100% - :alt: Quadrupeds in Isaac Lab + :alt: Arms in Isaac Lab -- Spawn different arms and apply random joint position commands: + +- Spawn different biped robots: .. tab-set:: :sync-group: os @@ -41,18 +43,83 @@ A few quick showroom scripts to run and checkout: .. code:: bash - ./isaaclab.sh -p scripts/demos/arms.py + ./isaaclab.sh -p scripts/demos/bipeds.py .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows .. code:: batch - isaaclab.bat -p scripts\demos\arms.py + isaaclab.bat -p scripts\demos\bipeds.py - .. image:: ../_static/demos/arms.jpg + .. image:: ../_static/demos/bipeds.jpg :width: 100% - :alt: Arms in Isaac Lab + :alt: Biped robots in Isaac Lab + + +- Spawn different deformable (soft) bodies and let them fall from a height: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/demos/deformables.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts\demos\deformables.py + + .. image:: ../_static/demos/deformables.jpg + :width: 100% + :alt: Deformable primitive-shaped objects in Isaac Lab + + +- Interactive inference of trained H1 rough terrain locomotion policy: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/demos/h1_locomotion.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts\demos\h1_locomotion.py + + .. image:: ../_static/demos/h1_locomotion.jpg + :width: 100% + :alt: H1 locomotion in Isaac Lab + + This is an interactive demo that can be run using the mouse and keyboard. + To enter third-person perspective, click on a humanoid character in the scene. + Once entered into third-person view, the humanoid can be controlled by keyboard using: + + * ``UP``: go forward + * ``LEFT``: turn left + * ``RIGHT``: turn right + * ``DOWN``: stop + * ``C``: switch between third-person and perspective views + * ``ESC``: exit current third-person view + + If a misclick happens outside of the humanoid bodies when selecting a humanoid, + a message is printed to console indicating the error, such as + ``The selected prim was not a H1 robot`` or + ``Multiple prims are selected. Please only select one!``. + - Spawn different hands and command them to open and close: @@ -77,7 +144,8 @@ A few quick showroom scripts to run and checkout: :width: 100% :alt: Dexterous hands in Isaac Lab -- Spawn different deformable (soft) bodies and let them fall from a height: + +- Define multiple markers that are useful for visualizations: .. tab-set:: :sync-group: os @@ -87,18 +155,19 @@ A few quick showroom scripts to run and checkout: .. code:: bash - ./isaaclab.sh -p scripts/demos/deformables.py + ./isaaclab.sh -p scripts/demos/markers.py .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows .. code:: batch - isaaclab.bat -p scripts\demos\deformables.py + isaaclab.bat -p scripts\demos\markers.py - .. image:: ../_static/demos/deformables.jpg + .. image:: ../_static/demos/markers.jpg :width: 100% - :alt: Deformable primitive-shaped objects in Isaac Lab + :alt: Markers in Isaac Lab + - Use the interactive scene and spawn varying assets in individual environments: @@ -123,6 +192,101 @@ A few quick showroom scripts to run and checkout: :width: 100% :alt: Multiple assets managed through the same simulation handles + +- Use the RigidObjectCollection spawn and view manipulation to demonstrate bin-packing example: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/demos/bin_packing.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts\demos\bin_packing.py + + .. image:: ../_static/demos/bin_packing.jpg + :width: 100% + :alt: Spawning random number of random asset per env_id using combination of MultiAssetSpawner and RigidObjectCollection + + + +- Use the interactive scene and spawn a simple parallel robot for pick and place: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/demos/pick_and_place.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts\demos\pick_and_place.py + + .. image:: ../_static/demos/pick_and_place.jpg + :width: 100% + :alt: User controlled pick and place with a parallel robot + + This is an interactive demo that can be run using the mouse and keyboard. + Your goal is pick up the purple cube and to drop it on the red sphere! + Use the following controls to interact with the simulation: + + * Hold the ``A`` key to have the gripper track the cube position. + * Hold the ``D`` key to have the gripper track the target position + * Press the ``W`` or ``S`` keys to move the gantry UP or DOWN respectively + * Press ``Q`` or ``E`` to OPEN or CLOSE the gripper respectively + + + +- Teleoperate a Franka Panda robot using Haply haptic device with force feedback: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/demos/haply_teleoperation.py --websocket_uri ws://localhost:10001 --pos_sensitivity 1.65 + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts\demos\haply_teleoperation.py --websocket_uri ws://localhost:10001 --pos_sensitivity 1.65 + + .. image:: ../_static/demos/haply_teleop_franka.jpg + :width: 100% + :alt: Haply teleoperation with force feedback + + This demo requires Haply Inverse3 and VerseGrip devices. + The goal of this demo is to pick up the cube or touch it with the end-effector. + The Haply devices provide: + + * 3 dimensional position tracking for end-effector control + * Directional force feedback for contact sensing + * Button inputs for gripper and end-effector rotation control + + See :ref:`haply-teleoperation` for detailed setup instructions. + + + - Create and spawn procedurally generated terrains with different configurations: .. tab-set:: @@ -146,7 +310,9 @@ A few quick showroom scripts to run and checkout: :width: 100% :alt: Procedural Terrains in Isaac Lab -- Define multiple markers that are useful for visualizations: + + +- Spawn a quadcopter in the default environment: .. tab-set:: :sync-group: os @@ -156,20 +322,21 @@ A few quick showroom scripts to run and checkout: .. code:: bash - ./isaaclab.sh -p scripts/demos/markers.py + ./isaaclab.sh -p scripts/demos/quadcopter.py .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows .. code:: batch - isaaclab.bat -p scripts\demos\markers.py + isaaclab.bat -p scripts\demos\quadcopter.py - .. image:: ../_static/demos/markers.jpg + .. image:: ../_static/demos/quadcopter.jpg :width: 100% - :alt: Markers in Isaac Lab + :alt: Quadcopter in Isaac Lab -- Interactive inference of trained H1 rough terrain locomotion policy: + +- Spawn different quadrupeds and make robots stand using position commands: .. tab-set:: :sync-group: os @@ -179,15 +346,39 @@ A few quick showroom scripts to run and checkout: .. code:: bash - ./isaaclab.sh -p scripts/demos/h1_locomotion.py + ./isaaclab.sh -p scripts/demos/quadrupeds.py .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows .. code:: batch - isaaclab.bat -p scripts\demos\h1_locomotion.py + isaaclab.bat -p scripts\demos\quadrupeds.py - .. image:: ../_static/demos/h1_locomotion.jpg + .. image:: ../_static/demos/quadrupeds.jpg :width: 100% - :alt: H1 locomotion in Isaac Lab + :alt: Quadrupeds in Isaac Lab + + +- Spawn a multi-mesh ray caster that uses Warp kernels for raycasting + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type objects + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts\demos\sensors\multi_mesh_raycaster.py --num_envs 16 --asset_type objects + + .. image:: ../_static/demos/multi-mesh-raycast.jpg + :width: 100% + :alt: Multi-mesh raycaster in Isaac Lab diff --git a/docs/source/overview/teleop_imitation.rst b/docs/source/overview/teleop_imitation.rst deleted file mode 100644 index a1f0db04bbe7..000000000000 --- a/docs/source/overview/teleop_imitation.rst +++ /dev/null @@ -1,288 +0,0 @@ -Teleoperation and Imitation Learning -==================================== - - -Teleoperation -~~~~~~~~~~~~~ - -We provide interfaces for providing commands in SE(2) and SE(3) space -for robot control. In case of SE(2) teleoperation, the returned command -is the linear x-y velocity and yaw rate, while in SE(3), the returned -command is a 6-D vector representing the change in pose. - -To play inverse kinematics (IK) control with a keyboard device: - -.. code:: bash - - ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device keyboard - -For smoother operation and off-axis operation, we recommend using a SpaceMouse as the input device. Providing smoother demonstrations will make it easier for the policy to clone the behavior. To use a SpaceMouse, simply change the teleop device accordingly: - -.. code:: bash - - ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device spacemouse - -.. note:: - - If the SpaceMouse is not detected, you may need to grant additional user permissions by running ``sudo chmod 666 /dev/hidraw<#>`` where ``<#>`` corresponds to the device index - of the connected SpaceMouse. - - To determine the device index, list all ``hidraw`` devices by running ``ls -l /dev/hidraw*``. - Identify the device corresponding to the SpaceMouse by running ``cat /sys/class/hidraw/hidraw<#>/device/uevent`` on each of the devices listed - from the prior step. - - Only compatible with the SpaceMouse Wireless and SpaceMouse Compact models from 3Dconnexion. - -The script prints the teleoperation events configured. For keyboard, -these are as follows: - -.. code:: text - - Keyboard Controller for SE(3): Se3Keyboard - Reset all commands: R - Toggle gripper (open/close): K - Move arm along x-axis: W/S - Move arm along y-axis: A/D - Move arm along z-axis: Q/E - Rotate arm along x-axis: Z/X - Rotate arm along y-axis: T/G - Rotate arm along z-axis: C/V - -For SpaceMouse, these are as follows: - -.. code:: text - - SpaceMouse Controller for SE(3): Se3SpaceMouse - Reset all commands: Right click - Toggle gripper (open/close): Click the left button on the SpaceMouse - Move arm along x/y-axis: Tilt the SpaceMouse - Move arm along z-axis: Push or pull the SpaceMouse - Rotate arm: Twist the SpaceMouse - -The next section describes how teleoperation devices can be used for data collection for imitation learning. - - -Imitation Learning -~~~~~~~~~~~~~~~~~~ - -Using the teleoperation devices, it is also possible to collect data for -learning from demonstrations (LfD). For this, we provide scripts to collect data into the open HDF5 format. - -Collecting demonstrations -^^^^^^^^^^^^^^^^^^^^^^^^^ - -To collect demonstrations with teleoperation for the environment ``Isaac-Stack-Cube-Franka-IK-Rel-v0``, use the following commands: - -.. code:: bash - - # step a: create folder for datasets - mkdir -p datasets - # step b: collect data with a selected teleoperation device. Replace with your preferred input device. - # Available options: spacemouse, keyboard - ./isaaclab.sh -p scripts/tools/record_demos.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --teleop_device --dataset_file ./datasets/dataset.hdf5 --num_demos 10 - # step a: replay the collected dataset - ./isaaclab.sh -p scripts/tools/replay_demos.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --dataset_file ./datasets/dataset.hdf5 - - -.. note:: - - The order of the stacked cubes should be blue (bottom), red (middle), green (top). - -About 10 successful demonstrations are required in order for the following steps to succeed. - -Here are some tips to perform demonstrations that lead to successful policy training: - -* Keep demonstrations short. Shorter demonstrations mean fewer decisions for the policy, making training easier. -* Take a direct path. Do not follow along arbitrary axis, but move straight toward the goal. -* Do not pause. Perform smooth, continuous motions instead. It is not obvious for a policy why and when to pause, hence continuous motions are easier to learn. - -If, while performing a demonstration, a mistake is made, or the current demonstration should not be recorded for some other reason, press the ``R`` key to discard the current demonstration, and reset to a new starting position. - -.. note:: - Non-determinism may be observed during replay as physics in IsaacLab are not determimnistically reproducible when using ``env.reset``. - -Pre-recorded demonstrations -^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -We provide a pre-recorded ``dataset.hdf5`` containing 10 human demonstrations for ``Isaac-Stack-Cube-Franka-IK-Rel-v0`` `here `_. -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 -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Additional demonstrations can be generated using Isaac Lab Mimic. - -Isaac Lab Mimic is a feature in Isaac Lab that allows generation of additional demonstrations automatically, allowing a policy to learn successfully even from just a handful of manual demonstrations. - -In order to use Isaac Lab Mimic with the recorded dataset, first annotate the subtasks in the recording: - -.. code:: bash - - ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py --input_file ./datasets/dataset.hdf5 --output_file ./datasets/annotated_dataset.hdf5 --task Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0 --auto - -Then, use Isaac Lab Mimic to generate some additional demonstrations: - -.. code:: bash - - ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py --input_file ./datasets/annotated_dataset.hdf5 --output_file ./datasets/generated_dataset_small.hdf5 --num_envs 10 --generation_num_trials 10 - -.. note:: - - The output_file of the ``annotate_demos.py`` script is the input_file to the ``generate_dataset.py`` script - -.. note:: - - Isaac Lab is designed to work with manipulators with grippers. The gripper commands in the demonstrations are extracted separately and temporally replayed during the generation of additional demonstrations. - -Inspect the output of generated data (filename: ``generated_dataset_small.hdf5``), and if satisfactory, generate the full dataset: - -.. code:: bash - - ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py --input_file ./datasets/annotated_dataset.hdf5 --output_file ./datasets/generated_dataset.hdf5 --num_envs 10 --generation_num_trials 1000 --headless - -The number of demonstrations can be increased or decreased, 1000 demonstrations have been shown to provide good training results for this task. - -Additionally, the number of environments in the ``--num_envs`` parameter can be adjusted to speed up data generation. The suggested number of 10 can be executed even on a laptop GPU. On a more powerful desktop machine, set it to 100 or higher for significant speedup of this step. - -Robomimic setup -^^^^^^^^^^^^^^^ - -As an example, we will train a BC agent implemented in `Robomimic `__ to train a policy. Any other framework or training method could be used. - -To install the robomimic framework, use the following commands: - -.. code:: bash - - # install the dependencies - sudo apt install cmake build-essential - # install python module (for robomimic) - ./isaaclab.sh -i robomimic - -Training an agent -^^^^^^^^^^^^^^^^^ - -We can now train a BC agent for ``Isaac-Stack-Cube-Franka-IK-Rel-v0`` using the Mimic generated data: - -.. code:: bash - - ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --algo bc --dataset ./datasets/generated_dataset.hdf5 - -By default, the training script will save a model checkpoint every 100 epochs. The trained models and logs will be saved to logs/robomimic/Isaac-Stack-Cube-Franka-IK-Rel-v0/bc - -Visualizing results -^^^^^^^^^^^^^^^^^^^ - -By inferencing using the generated model, we can visualize the results of the policy in the same environment: - -.. code:: bash - - ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_rollouts 50 --checkpoint /PATH/TO/desired_model_checkpoint.pth - - -Common Pitfalls when Generating Data -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -**Demonstrations are too long:** - -* Longer time horizon is harder to learn for a policy -* Start close to the first object and minimize motions - -**Demonstrations are not smooth:** - -* Irregular motion is hard for policy to decipher -* Better teleop devices result in better data (i.e. SpaceMouse is better than Keyboard) - -**Pauses in demonstrations:** - -* Pauses are difficult to learn -* Keep the human motions smooth and fluid - -**Excessive number of subtasks:** - -* Minimize the number of defined subtasks for completing a given task -* Less subtacks results in less stitching of trajectories, yielding higher data generation success rate - -**Lack of action noise:** - -* Action noise makes policies more robust - -**Recording cropped too tight:** - -* If recording stops on the frame the success term triggers, it may not re-trigger during replay -* Allow for some buffer at the end of recording - -**Non-deterministic replay:** - -* Physics in IsaacLab are not deterministically reproducible when using ``env.reset`` so demonstrations may fail on replay -* Collect more human demos than needed, use the ones that succeed during annotation -* All data in Isaac Lab Mimic generated HDF5 file represent a successful demo and can be used for training (even if non-determinism causes failure when replayed) - - -Creating Your Own Isaac Lab Mimic Compatible Environments -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -How it works -^^^^^^^^^^^^ - -Isaac Lab Mimic works by splitting the input demonstrations into subtasks. Subtasks are user-defined segments in the demonstrations that are common to all demonstrations. Examples for subtasks are "grasp an object", "move end effector to some pre-defined position", "release object" etc.. Note that most subtasks are defined with respect to some object that the robot interacts with. - -Subtasks need to be defined, and then annotated for each input demonstration. Annotation can either happen algorithmically by defining heuristics for subtask detection, as was done in the example above, or it can be done manually. - -With subtasks defined and annotated, Isaac Lab Mimic utilizes a small number of helper methods to then transform the subtask segments, and generate new demonstrations by stitching them together to match the new task at hand. - -For each thusly generated candidate demonstration, Isaac Lab Mimic uses a boolean success criteria to determine whether the demonstration succeeded in performing the task, and if so, add it to the output dataset. Success rate of candidate demonstrations can be as high as 70% in simple cases, and as low as <1%, depending on the difficulty of the task, and the complexity of the robot itself. - -Configuration and subtask definition -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Subtasks, among other configuration settings for Isaac Lab Mimic, are defined in a Mimic compatible environment configuration class that is created by extending the existing environment config with additional Mimic required parameters. - -All Mimic required config parameters are specified in the :class:`~isaaclab.envs.MimicEnvCfg` class. - -The config class :class:`~isaaclab_mimic.envs.FrankaCubeStackIKRelMimicEnvCfg` serves as an example of creating a Mimic compatible environment config class for the Franka stacking task that was used in the examples above. - -The ``DataGenConfig`` member contains various parameters that influence how data is generated. It is initially sufficient to just set the ``name`` parameter, and revise the rest later. - -Subtasks are a list of ``SubTaskConfig`` objects, of which the most important members are: - -* ``object_ref`` is the object that is being interacted with. This will be used to adjust motions relative to this object during data generation. Can be ``None`` if the current subtask does not involve any object. -* ``subtask_term_signal`` is the ID of the signal indicating whether the subtask is active or not. - -Subtask annotation -^^^^^^^^^^^^^^^^^^ - -Once the subtasks are defined, they need to be annotated in the source data. There are two methods to annotate source demonstrations for subtask boundaries: Manual annotation or using heuristics. - -It is often easiest to perform manual annotations, since the number of input demonstrations is usually very small. To perform manual annotations, use the ``annotate_demos.py`` script without the ``--auto`` flag. Then press ``B`` to pause, ``N`` to continue, and ``S`` to annotate a subtask boundary. - -For more accurate boundaries, or to speed up repeated processing of a given task for experiments, heuristics can be implemented to perform the same task. Heuristics are observations in the environment. An example how to add subtask terms can be found in ``source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py``, where they are added as an observation group called ``SubtaskCfg``. This example is using prebuilt heuristics, but custom heuristics are easily implemented. - - -Helpers for demonstration generation -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Helpers needed for Isaac Lab Mimic are defined in the environment. All tasks that are to be used with Isaac Lab Mimic are derived from the :class:`~isaaclab.envs.ManagerBasedRLMimicEnv` base class, and must implement the following functions: - -* ``get_robot_eef_pose``: Returns the current robot end effector pose in the same frame as used by the robot end effector controller. - -* ``target_eef_pose_to_action``: Takes a target pose and a gripper action for the end effector controller and returns an action which achieves the target pose. - -* ``action_to_target_eef_pose``: Takes an action and returns a target pose for the end effector controller. - -* ``actions_to_gripper_actions``: Takes a sequence of actions and returns the gripper actuation part of the actions. - -* ``get_object_poses``: Returns the pose of each object in the scene that is used for data generation. - -* ``get_subtask_term_signals``: Returns a dictionary of binary flags for each subtask in a task. The flag of true is set when the subtask has been completed and false otherwise. - -The class :class:`~isaaclab_mimic.envs.FrankaCubeStackIKRelMimicEnv` shows an example of creating a Mimic compatible environment from an existing Isaac Lab environment. - -Registering the environment -^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Once both Mimic compatible environment and environment config classes have been created, a new Mimic compatible environment can be registered using ``gym.register``. For the Franka stacking task in the examples above, the Mimic environment is registered as ``Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0``. - -The registered environment is now ready to be used with Isaac Lab Mimic. diff --git a/docs/source/policy_deployment/00_hover/hover_policy.rst b/docs/source/policy_deployment/00_hover/hover_policy.rst new file mode 100644 index 000000000000..b7efd80a15e9 --- /dev/null +++ b/docs/source/policy_deployment/00_hover/hover_policy.rst @@ -0,0 +1,194 @@ +Training & Deploying HOVER Policy +================================= + +This tutorial shows you an example of how to train and deploy HOVER which is a whole-body control (WBC) policy for humanoid robots in the Isaac Lab simulation environment. +It uses the `HOVER`_ repository, which provides an Isaac Lab extension for training neural whole-body control policy for humanoids, as described in the `HOVER Paper`_ and `OMNIH2O Paper`_ papers. +For video demonstrations and more details about the project, please visit the `HOVER Project Website`_ and the `OMNIH2O Project Website`_. + +.. figure:: ../../_static/policy_deployment/00_hover/hover_training_robots.png + :align: center + :figwidth: 100% + :alt: visualization of training the policy + +Installation +------------ + +.. note:: + + This tutorial is for linux only. + + HOVER supports Isaac Lab 2.0 and Isaac Sim 4.5. Please ensure you have the correct version of Isaac Lab and Isaac Sim installed to run the HOVER workflow. + + +1. Install Isaac Lab following the instructions in the `Isaac Lab Installation Guide`_. + +2. Define the following environment variable to specify the path to your Isaac Lab installation: + +.. code-block:: bash + + # Set the ISAACLAB_PATH environment variable to point to your Isaac Lab installation directory + export ISAACLAB_PATH= + +3. Clone the `HOVER`_ repository and its submodules in your workspace. + +.. code-block:: bash + + git clone --recurse-submodules https://github.com/NVlabs/HOVER.git + +4. Install the dependencies. + +.. code-block:: bash + + cd HOVER + ./install_deps.sh + + +Training the Policy +------------------- + +Dataset +~~~~~~~ +Refer to the `HOVER Dataset`_ repository for the steps to obtain and process data for training the policy. + + +Training the teacher policy +~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Execute the following command from the ``HOVER`` directory to train the teacher policy. + +.. code-block:: bash + + ${ISAACLAB_PATH:?}/isaaclab.sh -p scripts/rsl_rl/train_teacher_policy.py \ + --num_envs 1024 \ + --reference_motion_path neural_wbc/data/data/motions/stable_punch.pkl \ + --headless + +The teacher policy is trained for 10000000 iterations, or until the user interrupts the training. +The resulting checkpoint is stored in ``neural_wbc/data/data/policy/h1:teacher/`` and the filename is ``model_.pt``. + +Training the student policy +~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Execute the following command from the ``HOVER`` directory to train the student policy using teacher policy checkpoint. + +.. code-block:: bash + + ${ISAACLAB_PATH:?}/isaaclab.sh -p scripts/rsl_rl/train_student_policy.py \ + --num_envs 1024 \ + --reference_motion_path neural_wbc/data/data/motions/stable_punch.pkl \ + --teacher_policy.resume_path neural_wbc/data/data/policy/h1:teacher \ + --teacher_policy.checkpoint model_.pt \ + --headless + +This assumes that you have already trained the teacher policy as there is no provided teacher policy in the repo. + +Please refer to these sections on the HOVER repository for more details about training configurations: + - `General Remarks for Training`_ + - `Generalist vs Specialist Policy`_ + +Testing the trained policy +-------------------------- + +Play teacher policy +~~~~~~~~~~~~~~~~~~~ +Execute the following command from the ``HOVER`` directory to play the trained teacher policy checkpoint. + +.. code-block:: bash + + ${ISAACLAB_PATH:?}/isaaclab.sh -p scripts/rsl_rl/play.py \ + --num_envs 10 \ + --reference_motion_path neural_wbc/data/data/motions/stable_punch.pkl \ + --teacher_policy.resume_path neural_wbc/data/data/policy/h1:teacher \ + --teacher_policy.checkpoint model_.pt + +Play student policy +~~~~~~~~~~~~~~~~~~~ +Execute the following command from the ``HOVER`` directory to play the trained student policy checkpoint. + +.. code-block:: bash + + ${ISAACLAB_PATH:?}/isaaclab.sh -p scripts/rsl_rl/play.py \ + --num_envs 10 \ + --reference_motion_path neural_wbc/data/data/motions/stable_punch.pkl \ + --student_player \ + --student_path neural_wbc/data/data/policy/h1:student \ + --student_checkpoint model_.pt + + +Evaluate the trained policy +--------------------------- +Evaluate the trained policy checkpoint in the Isaac Lab environment. +The evaluation iterates through all the reference motions included in the dataset specified by the ``--reference_motion_path`` option and exits when all motions are evaluated. Randomization is turned off during evaluation. + +Refer to the `HOVER Evaluation`_ repository for more details about the evaluation pipeline and the metrics used. + +The evaluation script, ``scripts/rsl_rl/eval.py``, uses the same arguments as the play script, ``scripts/rsl_rl/play.py``. You can use it for both teacher and student policies. + +.. code-block:: bash + + ${ISAACLAB_PATH}/isaaclab.sh -p scripts/rsl_rl/eval.py \ + --num_envs 10 \ + --teacher_policy.resume_path neural_wbc/data/data/policy/h1:teacher \ + --teacher_policy.checkpoint model_.pt + + +Validation of the policy +------------------------ +The trained policy in Isaac Lab can be validated in another simulation environment or on the real robot. + +.. figure:: ../../_static/policy_deployment/00_hover/hover_stable_wave.png + :align: center + :width: 100% + + Stable Wave - Mujoco (left) & Real Robot (right) + +Sim-to-Sim Validation +~~~~~~~~~~~~~~~~~~~~~ +Use the provided `Mujoco Environment`_ for conducting sim-to-sim validation of the trained policy. To run the evaluation of Sim2Sim, + +.. code-block:: bash + + ${ISAACLAB_PATH:?}/isaaclab.sh -p neural_wbc/inference_env/scripts/eval.py \ + --num_envs 1 \ + --headless \ + --student_path neural_wbc/data/data/policy/h1:student/ \ + --student_checkpoint model_.pt + +Please be aware that the mujoco_wrapper only supports one environment at a time. For reference, it will take up to 5h to evaluate 8k reference motions. The inference_env is designed for maximum versatility. + + +Sim-to-Real Deployment +~~~~~~~~~~~~~~~~~~~~~~ +For sim-to-real deployment, we provide a `Hardware Environment`_ for `Unitree H1 Robot`_. +Detailed steps of setting up a Sim-to-Real deployment workflow is explained at `README of Sim2Real deployment`_. + +To deploy the trained policy on the H1 robot, + +.. code-block:: bash + + ${ISAACLAB_PATH:?}/isaaclab.sh -p neural_wbc/inference_env/scripts/s2r_player.py \ + --student_path neural_wbc/data/data/policy/h1:student/ \ + --student_checkpoint model_.pt \ + --reference_motion_path neural_wbc/data/data/motions/.pkl \ + --robot unitree_h1 \ + --max_iterations 5000 \ + --num_envs 1 \ + --headless + +.. note:: + + The sim-to-real deployment wrapper currently only supports the Unitree H1 robot. It can be extended to other robots by implementing the corresponding hardware wrapper interface. + + +.. _Isaac Lab Installation Guide: https://isaac-sim.github.io/IsaacLab/v2.0.0/source/setup/installation/index.html +.. _HOVER: https://github.com/NVlabs/HOVER +.. _HOVER Dataset: https://github.com/NVlabs/HOVER/?tab=readme-ov-file#data-processing +.. _HOVER Evaluation: https://github.com/NVlabs/HOVER/?tab=readme-ov-file#evaluation +.. _General Remarks for Training: https://github.com/NVlabs/HOVER/?tab=readme-ov-file#general-remarks-for-training +.. _Generalist vs Specialist Policy: https://github.com/NVlabs/HOVER/?tab=readme-ov-file#generalist-vs-specialist-policy +.. _HOVER Paper: https://arxiv.org/abs/2410.21229 +.. _HOVER Project Website: https://omni.human2humanoid.com/ +.. _OMNIH2O Paper: https://arxiv.org/abs/2410.21229 +.. _OMNIH2O Project Website: https://hover-versatile-humanoid.github.io/ +.. _README of Sim2Real deployment: https://github.com/NVlabs/HOVER/blob/main/neural_wbc/hw_wrappers/README.md +.. _Hardware Environment: https://github.com/NVlabs/HOVER/blob/main/neural_wbc/hw_wrappers/README.md +.. _Mujoco Environment: https://github.com/NVlabs/HOVER/tree/main/neural_wbc/mujoco_wrapper +.. _Unitree H1 Robot: https://unitree.com/h1 diff --git a/docs/source/policy_deployment/01_io_descriptors/io_descriptors_101.rst b/docs/source/policy_deployment/01_io_descriptors/io_descriptors_101.rst new file mode 100644 index 000000000000..d31de818399a --- /dev/null +++ b/docs/source/policy_deployment/01_io_descriptors/io_descriptors_101.rst @@ -0,0 +1,281 @@ +IO Descriptors 101 +================== + +.. currentmodule:: isaaclab + +In this tutorial, we will learn about IO descriptors, what they are, how to export them, and how to add them to +your environments. We will use the Anymal-D robot as an example to demonstrate how to export IO descriptors from +an environment, and use our own terms to demonstrate how to attach IO descriptors to custom action and observation terms. + + +What are IO Descriptors? +------------------------ + +Before we dive into IO descriptors, let's first understand what they are and how they can be useful. + +IO descriptors are a way to describe the inputs and outputs of a policy trained using the ManagerBasedRLEnv in Isaac +Lab. In other words, they describe the action and observation terms of a policy. This description is used to generate +a YAML file that can be loaded in an external tool to run the policies without having to manually input the +configuration of the action and observation terms. + +In addition to this the IO Descriptors provide the following information: +- The parameters of all the joints in the articulation. +- Some simulation parameters including the simulation time step, and the policy time step. +- For some action and observation terms, it provides the joint names or body names in the same order as they appear in the action/observation terms. +- For both the observation and action terms, it provides the terms in the exact same order as they appear in the managers. Making it easy to reconstruct them from the YAML file. + +Here is an example of what the action part of the YAML generated from the IO descriptors looks like for the Anymal-D robot: + +.. literalinclude:: ../../_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_anymal_d_v0_IO_descriptors.yaml + :language: yaml + :lines: 1-39 + +Here is an example of what a portion of the observation part of the YAML generated from the IO descriptors looks like for the Anymal-D robot: + +.. literalinclude:: ../../_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_anymal_d_v0_IO_descriptors.yaml + :language: yaml + :lines: 158-199 + +.. literalinclude:: ../../_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_anymal_d_v0_IO_descriptors.yaml + :language: yaml + :lines: 236-279 + +Something to note here is that both the action and observation terms are returned as list of dictionaries, and not a dictionary of dictionaries. +This is done to ensure the order of the terms is preserved. Hence, to retrieve the action or observation term, the users need to look for the +``name`` key in the dictionaries. + +For example, in the following snippet, we are looking at the ``projected_gravity`` observation term. The ``name`` key is used to identify the term. +The ``full_path`` key is used to provide an explicit path to the function in Isaac Lab's source code that is used to compute this term. Some flags +like ``mdp_type`` and ``observation_type`` are also provided, these don't have any functional impact. They are here to inform the user that this is the +category this term belongs to. + +.. literalinclude:: ../../_static/policy_deployment/01_io_descriptors/isaac_velocity_flat_anymal_d_v0_IO_descriptors.yaml + :language: yaml + :lines: 200-219 + :emphasize-lines: 9, 11 + + +Exporting IO Descriptors from an Environment +-------------------------------------------- + +In this section, we will cover how to export IO descriptors from an environment. +Keep in mind that this feature is only available to the manager based RL environments. + +If a policy has already been trained using a given configuration, then the IO descriptors can be exported using: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/environments/export_io_descriptors.py --task --output_dir + +For example, if we want to export the IO descriptors for the Anymal-D robot, we can run: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/environments/export_io_descriptors.py --task Isaac-Velocity-Flat-Anymal-D-v0 --output_dir ./io_descriptors + +When training a policy, it is also possible to request the IO descriptors to be exported at the beginning of the training. +This can be done by setting the ``export_io_descriptors`` flag in the command line. + +.. code-block:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --export_io_descriptors + ./isaaclab.sh -p scripts/reinforcement_learning/sb3/train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --export_io_descriptors + ./isaaclab.sh -p scripts/reinforcement_learning/rl_games/train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --export_io_descriptors + ./isaaclab.sh -p scripts/reinforcement_learning/skrl/train.py --task Isaac-Velocity-Flat-Anymal-D-v0 --export_io_descriptors + + +Attaching IO Descriptors to Custom Observation Terms +---------------------------------------------------- + +In this section, we will cover how to attach IO descriptors to custom observation terms. + +Let's take a look at how we can attach an IO descriptor to a simple observation term: + +.. code-block:: python + + @generic_io_descriptor( + units="m/s", axes=["X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] + ) + 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 + +Here, we are defining a custom observation term called ``base_lin_vel`` that computes the root linear velocity of the robot. +We are also attaching an IO descriptor to this term. The IO descriptor is defined using the ``@generic_io_descriptor`` decorator. + +The ``@generic_io_descriptor`` decorator is a special decorator that is used to attach an IO descriptor to a custom observation term. +It takes arbitrary arguments that are used to describe the observation term, in this case we provide extra information that could be +useful for the end user: + +- ``units``: The units of the observation term. +- ``axes``: The axes of the observation term. +- ``observation_type``: The type of the observation term. + +You'll also notice that there is an ``on_inspect`` argument that is provided. This is a list of functions that are used to inspect the observation term. +In this case, we are using the ``record_shape`` and ``record_dtype`` functions to record the shape and dtype of the output of the observation term. + +These functions are defined like so: + +.. code-block:: python + + def record_shape(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs) -> None: + """Record the shape of the output tensor. + + Args: + output: The output tensor. + descriptor: The descriptor to record the shape to. + **kwargs: Additional keyword arguments. + """ + descriptor.shape = (output.shape[-1],) + + + def record_dtype(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs) -> None: + """Record the dtype of the output tensor. + + Args: + output: The output tensor. + descriptor: The descriptor to record the dtype to. + **kwargs: Additional keyword arguments. + """ + descriptor.dtype = str(output.dtype) + +They always take the output tensor of the observation term as the first argument, and the descriptor as the second argument. +In the ``kwargs`` all the inputs of the observation term are provided. In addition to the ``on_inspect`` functions, the decorator +will also call call some functions in the background to collect the ``name``, the ``description``, and the ``full_path`` of the +observation term. Note that adding this decorator does not change the signature of the observation term, so it can be used safely +with the observation manager! + +Let us now take a look at a more complex example: getting the relative joint positions of the robot. + +.. code-block:: python + + @generic_io_descriptor( + observation_type="JointState", + on_inspect=[record_joint_names, record_dtype, record_shape, record_joint_pos_offsets], + units="rad", + ) + 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] + +Similarly to the previous example, we are adding an IO descriptor to a custom observation term with a set of functions that probe the observation term. + +To get the name of the joints we can write the following function: + +.. code-block:: python + + def record_joint_names(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs) -> None: + """Record the joint names of the output tensor. + + Expects the `asset_cfg` keyword argument to be set. + + Args: + output: The output tensor. + descriptor: The descriptor to record the joint names to. + **kwargs: Additional keyword arguments. + """ + asset: Articulation = kwargs["env"].scene[kwargs["asset_cfg"].name] + joint_ids = kwargs["asset_cfg"].joint_ids + if joint_ids == slice(None, None, None): + joint_ids = list(range(len(asset.joint_names))) + descriptor.joint_names = [asset.joint_names[i] for i in joint_ids] + +Note that we can access all the inputs of the observation term in the ``kwargs`` dictionary. Hence we can access the ``asset_cfg``, which contains the +configuration of the articulation that the observation term is computed on. + +To get the offsets, we can write the following function: + +.. code-block:: python + + def record_joint_pos_offsets(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs): + """Record the joint position offsets of the output tensor. + + Expects the `asset_cfg` keyword argument to be set. + + Args: + output: The output tensor. + descriptor: The descriptor to record the joint position offsets to. + **kwargs: Additional keyword arguments. + """ + asset: Articulation = kwargs["env"].scene[kwargs["asset_cfg"].name] + ids = kwargs["asset_cfg"].joint_ids + # Get the offsets of the joints for the first robot in the scene. + # This assumes that all robots have the same joint offsets. + descriptor.joint_pos_offsets = asset.data.default_joint_pos[:, ids][0] + +With this in mind, you should now be able to attach an IO descriptor to your own custom observation terms! However, before +we close this tutorial, let's take a look at how we can attach an IO descriptor to a custom action term. + + +Attaching IO Descriptors to Custom Action Terms +----------------------------------------------- + +In this section, we will cover how to attach IO descriptors to custom action terms. Action terms are classes that +inherit from the :class:`managers.ActionTerm` class. To add an IO descriptor to an action term, we need to expand +upon its :meth:`~managers.ActionTerm.IO_descriptor` property. + +By default, the :meth:`~managers.ActionTerm.IO_descriptor` property returns the base descriptor and fills the following fields: +- ``name``: The name of the action term. +- ``full_path``: The full path of the action term. +- ``description``: The description of the action term. +- ``export``: Whether to export the action term. + +.. code-block:: python + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor for the action term.""" + self._IO_descriptor.name = re.sub(r"([a-z])([A-Z])", r"\1_\2", self.__class__.__name__).lower() + self._IO_descriptor.full_path = f"{self.__class__.__module__}.{self.__class__.__name__}" + self._IO_descriptor.description = " ".join(self.__class__.__doc__.split()) + self._IO_descriptor.export = self.export_IO_descriptor + return self._IO_descriptor + +To add more information to the descriptor, we need to override the :meth:`~managers.ActionTerm.IO_descriptor` property. +Let's take a look at an example on how to add the joint names, scale, offset, and clip to the descriptor. + +.. code-block:: python + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the joint action. + It adds the following information to the base descriptor: + - joint_names: The names of the joints. + - scale: The scale of the action term. + - offset: The offset of the action term. + - clip: The clip of the action term. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "JointAction" + self._IO_descriptor.joint_names = self._joint_names + self._IO_descriptor.scale = self._scale + # This seems to be always [4xNum_joints] IDK why. Need to check. + if isinstance(self._offset, torch.Tensor): + self._IO_descriptor.offset = self._offset[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.offset = self._offset + # FIXME: This is not correct. Add list support. + if self.cfg.clip is not None: + if isinstance(self._clip, torch.Tensor): + self._IO_descriptor.clip = self._clip[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.clip = self._clip + else: + self._IO_descriptor.clip = None + return self._IO_descriptor + +This is it! You should now be able to attach an IO descriptor to your own custom action terms which concludes this tutorial. diff --git a/docs/source/policy_deployment/02_gear_assembly/gear_assembly_policy.rst b/docs/source/policy_deployment/02_gear_assembly/gear_assembly_policy.rst new file mode 100644 index 000000000000..885b7fb6733a --- /dev/null +++ b/docs/source/policy_deployment/02_gear_assembly/gear_assembly_policy.rst @@ -0,0 +1,605 @@ +.. _walkthrough_sim_to_real: + +Training a Gear Insertion Policy and ROS Deployment +==================================================== + +This tutorial walks you through how to train a gear insertion assembly reinforcement learning (RL) policy that transfers from simulation to a real robot. The workflow consists of two main stages: + +1. **Simulation Training in Isaac Lab**: Train the policy in a high-fidelity physics simulation with domain randomization +2. **Real Robot Deployment with Isaac ROS**: Deploy the trained policy on real hardware using Isaac ROS and a custom ROS inference node + +This walkthrough covers the key principles and best practices for sim-to-real transfer using Isaac Lab, illustrated with a real-world example: + +- the Gear Assembly task for the UR10e robot with the Robotiq 2F-140 gripper or 2F-85 gripper + +**Task Details:** + +The gear assembly policy operates as follows: + +1. **Initial State**: The policy assumes the gear is already grasped by the gripper at the start of the episode +2. **Input Observations**: The policy receives the pose of the gear shaft (position and orientation) in which the gear should be inserted, obtained from a separate perception pipeline +3. **Policy Output**: The policy outputs delta joint positions (incremental changes to joint angles) to control the robot arm and perform the insertion +4. **Generalization**: The trained policy generalizes across 3 different gear sizes without requiring retraining for each size + + +.. figure:: ../../_static/policy_deployment/02_gear_assembly/gear_assembly_sim_real.webm + :align: center + :figwidth: 100% + :alt: Comparison of gear assembly in simulation versus real hardware + + Sim-to-real transfer: Gear assembly policy trained in Isaac Lab (left) successfully deployed on real UR10e robot (right). + +This environment has been successfully deployed on real UR10e robots without an IsaacLab dependency. + +**Scope of This Tutorial:** + +This tutorial focuses exclusively on the **training part** of the sim-to-real transfer workflow in Isaac Lab. For the complete deployment workflow on the real robot, including the exact steps to set up the vision pipeline, robot interface and the ROS inference node to run your trained policy on real hardware, please refer to the `Isaac ROS Documentation `_. + +Overview +-------- + +Successful sim-to-real transfer requires addressing three fundamental aspects: + +1. **Input Consistency**: Ensuring the observations your policy receives in simulation match those available on the real robot +2. **System Response Consistency**: Ensuring the robot and environment respond to actions in simulation the same way they do in reality +3. **Output Consistency**: Ensuring any post-processing applied to policy outputs in Isaac Lab is also applied during real-world inference + +When all three aspects are properly addressed, policies trained purely in simulation can achieve robust performance on real hardware without any real-world training data. + +**Debugging Tip**: When your policy fails on the real robot, the best approach to debug is to set up the real robot with the same initial observations as in simulation, then compare how the controller/system respond. This isolates whether the problem is from observation mismatch (Input Consistency) or physics/controller mismatch (System Response Consistency). + +Part 1: Input Consistency +-------------------------- + +The observations your policy receives must be consistent between simulation and reality. This means: + +1. The observation space should only include information available from real sensors +2. Sensor noise and delays should be modeled appropriately + +Using Real-Robot-Available Observations +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Your simulation environment should only use observations that are available on the real robot and not use "privileged" information that wouldn't be available in deployment. + + +Observation Specification: Isaac-Deploy-GearAssembly-UR10e-2F140-v0 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The Gear Assembly environment uses both proprioceptive and exteroceptive (vision) observations: + +.. list-table:: Gear Assembly Environment Observations + :widths: 25 25 25 25 + :header-rows: 1 + + * - Observation + - Dimension + - Real-World Source + - Noise in Training + * - ``joint_pos`` + - 6 (UR10e arm joints) + - UR10e controller + - None (proprioceptive) + * - ``joint_vel`` + - 6 (UR10e arm joints) + - UR10e controller + - None (proprioceptive) + * - ``gear_shaft_pos`` + - 3 (x, y, z position) + - FoundationPose + RealSense depth + - ยฑ0.005 m (5mm, estimated error from FoundationPose + RealSense depth pipeline) + * - ``gear_shaft_quat`` + - 4 (quaternion orientation) + - FoundationPose + RealSense depth + - ยฑ0.01 per component (~5ยฐ angular error, estimated error from FoundationPose + RealSense depth pipeline) + +**Implementation:** + +.. code-block:: python + + from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # Robot joint states - NO noise for proprioceptive observations + joint_pos = ObsTerm( + func=mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["shoulder_pan_joint", ...])}, + ) + + joint_vel = ObsTerm( + func=mdp.joint_vel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["shoulder_pan_joint", ...])}, + ) + + # Gear shaft pose from FoundationPose perception + # ADD noise for exteroceptive (vision-based) observations + # Calibrated to match FoundationPose + RealSense D435 error + # Typical error: 3-8mm position, 3-7ยฐ orientation + gear_shaft_pos = ObsTerm( + func=mdp.gear_shaft_pos_w, + params={"asset_cfg": SceneEntityCfg("factory_gear_base")}, + noise=Unoise(n_min=-0.005, n_max=0.005), # ยฑ5mm + ) + + # Quaternion noise: small uniform noise on each component + # Results in ~5ยฐ orientation error + gear_shaft_quat = ObsTerm( + func=mdp.gear_shaft_quat_w, + params={"asset_cfg": SceneEntityCfg("factory_gear_base")}, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + + def __post_init__(self): + self.enable_corruption = True # Enable for perception observations only + self.concatenate_terms = True + +**Why No Noise for Proprioceptive Observations?** + +Empirically, we found that policies trained without noise on proprioceptive observations (joint positions and velocities) transfer well to real hardware. The UR10e controller provides sufficiently accurate joint state feedback that modeling sensor noise doesn't improve sim-to-real transfer for these tasks. + + +Part 2: System Response Consistency +------------------------------------ + +Once your observations are consistent, you need to ensure the simulated robot and environment respond to actions the same way the real system does. In this use case, this involves three main aspects: + +1. Physics simulation parameters (friction, contact properties) +2. Actuator modeling (PD controller gains, effort limits) +3. Domain randomization + +Physics Parameter Tuning +~~~~~~~~~~~~~~~~~~~~~~~~~ + +Accurate physics simulation is critical for contact-rich tasks. Key parameters include: + +- Friction coefficients (static and dynamic) +- Contact solver parameters +- Material properties +- Rigid body properties + +**Example: Gear Assembly Physics Configuration** + +The Gear Assembly task requires accurate contact modeling for insertion. Here's how friction is configured: + +.. code-block:: python + + # From joint_pos_env_cfg.py in Isaac-Deploy-GearAssembly-UR10e-2F140-v0 + + @configclass + class EventCfg: + """Configuration for events including physics randomization.""" + + # Randomize friction for gear objects + small_gear_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("factory_gear_small", body_names=".*"), + "static_friction_range": (0.75, 0.75), # Calibrated to real gear material + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), # No bounce + "num_buckets": 16, + }, + ) + + # Similar configuration for gripper fingers + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*finger"), + "static_friction_range": (0.75, 0.75), # Calibrated to real gripper + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + +These friction values (0.75) were determined through iterative visual comparison: + +1. Record videos of the gear being grasped and manipulated on real hardware +2. Start training in simulation and observe the live simulation viewer +3. Look for physics issues (penetration, unrealistic slipping, poor contact) +4. Adjust friction coefficients and solver parameters and retry +5. Compare the gear's behavior in the gripper visually between sim and real +6. Repeat adjustments until behavior matches (no need to wait for full policy training) +7. Once physics looks good, train in headless mode with video recording: + + .. code-block:: bash + + python scripts/reinforcement_learning/rsl_rl/train.py \ + --task Isaac-Deploy-GearAssembly-UR10e-2F140-v0 \ + --headless \ + --video --video_length 800 --video_interval 5000 + +8. Review the recorded videos and compare with real hardware videos to verify physics behavior + +**Contact Solver Configuration** + +Contact-rich manipulation requires careful solver tuning. These parameters were calibrated through the same iterative visual comparison process as the friction coefficients: + +.. code-block:: python + + # Robot rigid body properties + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, # Robot is mounted, no gravity + max_depenetration_velocity=5.0, # Control interpenetration resolution + linear_damping=0.0, # No artificial damping + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, # Important for accurate dynamics + solver_position_iteration_count=4, # Balance accuracy vs performance + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, # Allow large contact forces + ), + +**Important**: The ``solver_position_iteration_count`` is a critical parameter for contact-rich tasks. Increasing this value improves collision simulation stability and reduces penetration issues, but it also increases simulation and training time. For the gear assembly task, we use ``solver_position_iteration_count=4`` as a balance between physics accuracy and computational performance. If you observe penetration or unstable contacts, try increasing to 8 or 16, but expect slower training. + +.. code-block:: python + + # Articulation properties + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + + # Contact properties + collision_props=sim_utils.CollisionPropertiesCfg( + contact_offset=0.005, # 5mm contact detection distance + rest_offset=0.0, # Objects touch at 0 distance + ), + +Actuator Modeling +~~~~~~~~~~~~~~~~~ + +Accurate actuator modeling ensures the simulated robot moves like the real one. This includes: + +- PD controller gains (stiffness and damping) +- Effort and velocity limits +- Joint friction + +**Controller Choice: Impedance Control** + +For the UR10e deployment, we use an impedance controller interface. Using a simpler controller like impedance control reduces the chances of variation between simulation and reality compared to more complex controllers (e.g., operational space control, hybrid force-position control). Simpler controllers: + +- Have fewer parameters that can mismatch between sim and real +- Are easier to model accurately in simulation +- Have more predictable behavior that's easier to replicate +- Reduce the controller complexity as a source of sim-real gap + +**Example: UR10e Actuator Configuration** + +.. code-block:: python + + # Default UR10e actuator configuration + actuators = { + "arm": ImplicitActuatorCfg( + joint_names_expr=["shoulder_pan_joint", "shoulder_lift_joint", + "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"], + effort_limit=87.0, # From UR10e specifications + velocity_limit=2.0, # From UR10e specifications + stiffness=800.0, # Calibrated to match real behavior + damping=40.0, # Calibrated to match real behavior + ), + } + +**Domain Randomization of Actuator Parameters** + +To account for variations in real robot behavior, randomize actuator gains during training: + +.. code-block:: python + + # From EventCfg in the Gear Assembly environment + robot_joint_stiffness_and_damping = EventTerm( + func=mdp.randomize_actuator_gains, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=["shoulder_.*", "elbow_.*", "wrist_.*"]), + "stiffness_distribution_params": (0.75, 1.5), # 75% to 150% of nominal + "damping_distribution_params": (0.3, 3.0), # 30% to 300% of nominal + "operation": "scale", + "distribution": "log_uniform", + }, + ) + + +**Joint Friction Randomization** + +Real robots have friction in their joints that varies with position, velocity, and temperature. For the UR10e with impedance controller interface, we observed significant stiction (static friction) causing the controller to not reach target joint positions. + +**Characterizing Real Robot Behavior:** + +To quantify this behavior, we plotted the step response of the impedance controller on the real robot and observed contact offsets of approximately 0.25 degrees from the commanded setpoint. This steady-state error is caused by joint friction opposing the controller's commanded motion. Based on these measurements, we added joint friction modeling in simulation to replicate this behavior: + +.. code-block:: python + + joint_friction = EventTerm( + func=mdp.randomize_joint_parameters, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=["shoulder_.*", "elbow_.*", "wrist_.*"]), + "friction_distribution_params": (0.3, 0.7), # Add 0.3 to 0.7 Nm friction + "operation": "add", + "distribution": "uniform", + }, + ) + +**Why Joint Friction Matters**: Without modeling joint friction in simulation, the policy learns to expect that commanded joint positions are always reached. On the real robot, stiction prevents small movements and causes steady-state errors. By adding friction during training, the policy learns to account for these effects and commands appropriately larger motions to overcome friction. + +**Compensating for Stiction with Action Scaling:** + +To help the policy overcome stiction on the real robot, we also increased the output action scaling. The Isaac ROS documentation notes that a higher action scale (0.0325 vs 0.025) is needed to overcome the higher static friction (stiction) compared to the 2F-85 gripper. This increased scaling ensures the policy commands are large enough to overcome the friction forces observed in the step response analysis. + +Action Space Design +~~~~~~~~~~~~~~~~~~~ + +Your action space should match what the real robot controller can execute. For this task we found that **incremental joint position control** is the most reliable approach. + +**Example: Gear Assembly Action Configuration** + +.. code-block:: python + + # For contact-rich manipulation, smaller action scale for more precise control + self.joint_action_scale = 0.025 # ยฑ2.5 degrees per step + + self.actions.arm_action = mdp.RelativeJointPositionActionCfg( + asset_name="robot", + joint_names=["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"], + scale=self.joint_action_scale, + use_zero_offset=True, + ) + +The action scale is a critical hyperparameter that should be tuned based on: + +- Task precision requirements (smaller for contact-rich tasks) +- Control frequency (higher frequency allows larger steps) + +Domain Randomization Strategy +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Domain randomization should cover the range of conditions in which you want the real robot to perform. Increasing randomization ranges makes it harder for the policy to learn, but allows for larger variations in inputs and system parameters. The key is to balance training difficulty with robustness: randomize enough to cover real-world variations, but not so much that the policy cannot learn effectively. + +**Pose Randomization** + +For manipulation tasks, randomize object poses to ensure the policy works across the workspace: + +.. code-block:: python + + # From Gear Assembly environment + randomize_gears_and_base_pose = EventTerm( + func=gear_assembly_events.randomize_gears_and_base_pose, + mode="reset", + params={ + "pose_range": { + "x": [-0.1, 0.1], # ยฑ10cm + "y": [-0.25, 0.25], # ยฑ25cm + "z": [-0.1, 0.1], # ยฑ10cm + "roll": [-math.pi/90, math.pi/90], # ยฑ2 degrees + "pitch": [-math.pi/90, math.pi/90], # ยฑ2 degrees + "yaw": [-math.pi/6, math.pi/6], # ยฑ30 degrees + }, + "gear_pos_range": { + "x": [-0.02, 0.02], # ยฑ2cm relative to base + "y": [-0.02, 0.02], + "z": [0.0575, 0.0775], # 5.75-7.75cm above base + }, + "rot_randomization_range": { + "roll": [-math.pi/36, math.pi/36], # ยฑ5 degrees + "pitch": [-math.pi/36, math.pi/36], + "yaw": [-math.pi/36, math.pi/36], + }, + }, + ) + +**Initial State Randomization** + +Randomizing the robot's initial configuration helps the policy handle different starting conditions: + +.. code-block:: python + + set_robot_to_grasp_pose = EventTerm( + func=gear_assembly_events.set_robot_to_grasp_pose, + mode="reset", + params={ + "robot_asset_cfg": SceneEntityCfg("robot"), + "rot_offset": [0.0, math.sqrt(2)/2, math.sqrt(2)/2, 0.0], # Base gripper orientation + "pos_randomization_range": { + "x": [-0.0, 0.0], + "y": [-0.005, 0.005], # ยฑ5mm variation + "z": [-0.003, 0.003], # ยฑ3mm variation + }, + "gripper_type": "2f_140", + }, + ) + +Part 3: Training the Policy in Isaac Lab +----------------------------------------- + +Now that we've covered the key principles for sim-to-real transfer, let's train the gear assembly policy in Isaac Lab. + +Step 1: Visualize the Environment +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +First, launch the training with a small number of environments and visualization enabled to verify that the environment is set up correctly: + +.. code-block:: bash + + # Launch training with visualization + python scripts/reinforcement_learning/rsl_rl/train.py \ + --task Isaac-Deploy-GearAssembly-UR10e-2F140-v0 \ + --num_envs 4 + +.. note:: + + For the Robotiq 2F-85 gripper, use ``--task Isaac-Deploy-GearAssembly-UR10e-2F85-v0`` instead. + +This will open the Isaac Sim viewer where you can observe the training process in real-time. + +.. figure:: ../../_static/policy_deployment/02_gear_assembly/sim_real_gear_assembly_train.jpg + :align: center + :figwidth: 100% + :alt: Gear assembly training visualization in Isaac Lab + + Training visualization showing multiple parallel environments with robots grasping gears. + +**What to Expect:** + +In the early stages of training, you should see the robots moving around with the gears grasped by the grippers, but they won't be successfully inserting the gears yet. This is expected behavior as the policy is still learning. The robots will move the grasped gear in various directions. Once you've verified the environment looks correct, stop the training (Ctrl+C) and proceed to full-scale training. + +Step 2: Full-Scale Training with Video Recording +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Now launch the full training run with more parallel environments in headless mode for faster training. We'll also enable video recording to monitor progress: + +.. code-block:: bash + + # Full training with video recording + python scripts/reinforcement_learning/rsl_rl/train.py \ + --task Isaac-Deploy-GearAssembly-UR10e-2F140-v0 \ + --headless \ + --num_envs 256 \ + --video --video_length 800 --video_interval 5000 + +This command will: + +- Run 256 parallel environments for efficient training +- Run in headless mode (no visualization) for maximum performance +- Record videos every 5000 steps to monitor training progress +- Save videos with 800 frames each + +Training typically takes ~12-24 hours for a robust insertion policy. The videos will be saved in the ``logs`` directory and can be reviewed to assess policy performance during training. + +.. note:: + + **GPU Memory Considerations**: The default configuration uses 256 parallel environments, which should work on most modern GPUs (e.g., RTX 3090, RTX 4090, A100). For better sim-to-real transfer performance, you can increase ``solver_position_iteration_count`` from 4 to 196 in ``gear_assembly_env_cfg.py`` and ``joint_pos_env_cfg.py`` for more realistic contact simulation, but this requires a larger GPU (e.g., RTX PRO 6000 with 40GB+ VRAM). Higher solver iteration counts reduce penetration and improve contact stability but significantly increase GPU memory usage. + + +**Monitoring Training Progress with TensorBoard:** + +You can monitor training metrics in real-time using TensorBoard. Open a new terminal and run: + +.. code-block:: bash + + ./isaaclab.sh -p -m tensorboard.main --logdir + +Replace ```` with the path to your training logs (e.g., ``logs/rsl_rl/gear_assembly_ur10e/2025-11-19_19-31-01``). TensorBoard will display plots showing rewards, episode lengths, and other metrics. Verify that the rewards are increasing over iterations to ensure the policy is learning successfully. + + +Step 3: Deploy on Real Robot +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Once training is complete, follow the `Isaac ROS inference documentation `_ to deploy your policy. + +The Isaac ROS deployment pipeline directly uses the trained model checkpoint (``.pt`` file) along with the ``agent.yaml`` and ``env.yaml`` configuration files generated during training. No additional export step is required. + +The deployment pipeline uses Isaac ROS and a custom ROS inference node to run the policy on real hardware. The pipeline includes: + +1. **Perception**: Camera-based pose estimation (FoundationPose, Segment Anything) +2. **Motion Planning**: cuMotion for collision-free trajectories +3. **Policy Inference**: Your trained policy running at control frequency in a custom ROS inference node +4. **Robot Control**: Low-level controller executing commands + + +Troubleshooting +--------------- + +This section covers common errors you may encounter during training and their solutions. + +PhysX Collision Stack Overflow +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +**Error Message:** + +.. code-block:: text + + PhysX error: PxGpuDynamicsMemoryConfig::collisionStackSize buffer overflow detected, + please increase its size to at least 269452544 in the scene desc! + Contacts have been dropped. + +**Cause:** This error occurs when the GPU collision detection buffer is too small for the number of contacts being simulated. This is common in contact-rich environments like gear assembly. + +**Solution:** Increase the ``gpu_collision_stack_size`` parameter in ``gear_assembly_env_cfg.py``: + +.. code-block:: python + + # In GearAssemblyEnvCfg class + sim: SimulationCfg = SimulationCfg( + physx=PhysxCfg( + gpu_collision_stack_size=2**31, # Increase this value if you see overflow errors + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + ), + ) + +The error message will suggest a minimum size. Set ``gpu_collision_stack_size`` to at least the recommended value (e.g., if the error says "at least 269452544", set it to ``2**28`` or ``2**29``). Note that increasing this value increases GPU memory usage. + +CUDA Out of Memory +~~~~~~~~~~~~~~~~~~ + +**Error Message:** + +.. code-block:: text + + torch.OutOfMemoryError: CUDA out of memory. + +**Cause:** The GPU does not have enough memory to run the requested number of parallel environments with the current simulation parameters. + +**Solutions (in order of preference):** + +1. **Reduce the number of parallel environments:** + + .. code-block:: bash + + python scripts/reinforcement_learning/rsl_rl/train.py \ + --task Isaac-Deploy-GearAssembly-UR10e-2F140-v0 \ + --headless \ + --num_envs 128 # Reduce from 256 to 128, 64, etc. + + **Trade-off:** Using fewer environments will reduce sample diversity per training iteration and may slow down training convergence. You may need to train for more iterations to achieve the same performance. However, the final policy quality should be similar. + +2. **If using increased solver iteration counts** (values higher than the default 4): + + In both ``gear_assembly_env_cfg.py`` and ``joint_pos_env_cfg.py``, reduce ``solver_position_iteration_count`` back to the default value of 4, or use intermediate values like 8 or 16: + + .. code-block:: python + + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, # Use default value + # ... other parameters + ), + + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + solver_position_iteration_count=4, # Use default value + # ... other parameters + ), + + **Trade-off:** Lower solver iteration counts may result in less realistic contact dynamics and more penetration issues. The default value of 4 provides a good balance for most use cases. + +3. **Disable video recording during training:** + + Remove the ``--video`` flags to save GPU memory: + + .. code-block:: bash + + python scripts/reinforcement_learning/rsl_rl/train.py \ + --task Isaac-Deploy-GearAssembly-UR10e-2F140-v0 \ + --headless \ + --num_envs 256 + + You can always evaluate the trained policy later with visualization. + + +Further Resources +----------------- + +- `IndustReal: Transferring Contact-Rich Assembly Tasks from Simulation to Reality `_ +- `FORGE: Force-Guided Exploration for Robust Contact-Rich Manipulation under Uncertainty `_ +- Sim-to-Real Policy Transfer for Whole Body Controllers: :ref:`sim2real` - Shows how to train and deploy a whole body controller for legged robots using Isaac Lab with the Newton backend +- `Isaac ROS Manipulation Documentation `_ +- `Isaac ROS Gear Assembly Tutorial `_ +- RL Training Tutorial: :ref:`tutorial-run-rl-training` diff --git a/docs/source/policy_deployment/index.rst b/docs/source/policy_deployment/index.rst new file mode 100644 index 000000000000..3ee100f22174 --- /dev/null +++ b/docs/source/policy_deployment/index.rst @@ -0,0 +1,13 @@ +Sim2Real Deployment of Policies Trained in Isaac Lab +==================================================== + +Welcome to the Policy Deployment Guide! This section provides examples of training policies in Isaac Lab and deploying them to both simulation and real robots. + +Below, youโ€™ll find detailed examples of various policies for training and deploying them, along with essential configuration details. + +.. toctree:: + :maxdepth: 1 + + 00_hover/hover_policy + 01_io_descriptors/io_descriptors_101 + 02_gear_assembly/gear_assembly_policy diff --git a/docs/source/refs/additional_resources.rst b/docs/source/refs/additional_resources.rst index 7c0ca785fda0..16913b36d2ee 100644 --- a/docs/source/refs/additional_resources.rst +++ b/docs/source/refs/additional_resources.rst @@ -27,6 +27,8 @@ Simulation Features At the heart of Isaac Lab is Isaac Sim, which is itself a feature rich tool that is useful for robotics in general, and not only for RL. The stronger your understanding of the simulation, the readily you will be able to exploit its capabilities for your own projects and applications. These resources are dedicated to informing you about the other features of the simulation that may be useful to you given your specific interest in Isaac Lab! +* `Simulation Performance Guide `_ is a best practice guide for obtaining the best simulation performance from OmniPhysics. + * `Deploying Policies in Isaac Sim `_ is an Isaac Sim tutorial on how to use trained policies within the simulation. * `Supercharge Robotics Workflows with AI and Simulation Using NVIDIA Isaac Sim 4.0 and NVIDIA Isaac Lab `_ is a blog post covering the newest features of Isaac Sim 4.0, including ``pip install``, a more advanced physics engine, updated sensor simulations, and more! diff --git a/docs/source/refs/contributing.rst b/docs/source/refs/contributing.rst index 6fb06deb29d9..411742fd19e8 100644 --- a/docs/source/refs/contributing.rst +++ b/docs/source/refs/contributing.rst @@ -54,6 +54,9 @@ Please ensure that your code is well-formatted, documented and passes all the te Large pull requests are difficult to review and may take a long time to merge. +More details on the code style and testing can be found in the `Coding Style`_ and `Unit Testing`_ sections. + + Contributing Documentation -------------------------- @@ -112,11 +115,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:: @@ -237,6 +237,62 @@ For documentation, we adopt the `Google Style Guide `__ for generating the documentation. Please make sure that your code is well-documented and follows the guidelines. +Code Structure +^^^^^^^^^^^^^^ + +We follow a specific structure for the codebase. This helps in maintaining the codebase and makes it easier to +understand. + +In a Python file, we follow the following structure: + +.. code:: python + + # Imports: These are sorted by the pre-commit hooks. + # Constants + # Functions (public) + # Classes (public) + # _Functions (private) + # _Classes (private) + +Imports are sorted by the pre-commit hooks. Unless there is a good reason to do otherwise, please do not +import the modules inside functions or classes. To deal with circular imports, we use the +:obj:`typing.TYPE_CHECKING` variable. Please refer to the `Circular Imports`_ section for more details. + +Python does not have a concept of private and public classes and functions. However, we follow the +convention of prefixing the private functions and classes with an underscore. +The public functions and classes are the ones that are intended to be used by the users. The private +functions and classes are the ones that are intended to be used internally in that file. +Irrespective of the public or private nature of the functions and classes, we follow the Style Guide +for the code and make sure that the code and documentation are consistent. + +Similarly, within Python classes, we follow the following structure: + +.. code:: python + + # Constants + # Class variables (public or private): Must have the type hint ClassVar[type] + # Dunder methods: __init__, __del__ + # Representation: __repr__, __str__ + # Properties: @property + # Instance methods (public) + # Class methods (public) + # Static methods (public) + # _Instance methods (private) + # _Class methods (private) + # _Static methods (private) + +The rule of thumb is that the functions within the classes are ordered in the way a user would +expect to use them. For instance, if the class contains the method :meth:`initialize`, :meth:`reset`, +:meth:`update`, and :meth:`close`, then they should be listed in the order of their usage. +The same applies for private functions in the class. Their order is based on the order of call inside the +class. + +.. dropdown:: Code skeleton + :icon: code + + .. literalinclude:: snippets/code_skeleton.py + :language: python + Circular Imports ^^^^^^^^^^^^^^^^ @@ -414,27 +470,70 @@ We summarize the key points below: Unit Testing -^^^^^^^^^^^^ +------------ -We use `unittest `__ for unit testing. +We use `pytest `__ for unit testing. Good tests not only cover the basic functionality of the code but also the edge cases. They should be able to catch regressions and ensure that the code is working as expected. Please make sure that you add tests for your changes. +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + # Run all tests + ./isaaclab.sh --test # or "./isaaclab.sh -t" + + # Run all tests in a particular file + ./isaaclab.sh -p -m pytest source/isaaclab/test/deps/test_torch.py + + # Run a particular test + ./isaaclab.sh -p -m pytest source/isaaclab/test/deps/test_torch.py::test_array_slicing + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: bash + + # Run all tests + isaaclab.bat --test # or "isaaclab.bat -t" + + # Run all tests in a particular file + isaaclab.bat -p -m pytest source/isaaclab/test/deps/test_torch.py + + # Run a particular test + isaaclab.bat -p -m pytest source/isaaclab/test/deps/test_torch.py::test_array_slicing + + Tools -^^^^^ +----- We use the following tools for maintaining code quality: * `pre-commit `__: Runs a list of formatters and linters over the codebase. -* `black `__: The uncompromising code formatter. -* `flake8 `__: A wrapper around PyFlakes, pycodestyle and - McCabe complexity checker. +* `ruff `__: An extremely fast Python linter and formatter. Please check `here `__ for instructions to set these up. To run over the entire repository, please execute the following command in the terminal: -.. code:: bash +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + ./isaaclab.sh --format # or "./isaaclab.sh -f" + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: bash - ./isaaclab.sh --format # or "./isaaclab.sh -f" + isaaclab.bat --format # or "isaaclab.bat -f" diff --git a/docs/source/refs/issues.rst b/docs/source/refs/issues.rst index 163b1fe25360..c4bb56182e51 100644 --- a/docs/source/refs/issues.rst +++ b/docs/source/refs/issues.rst @@ -91,3 +91,24 @@ This is due to the termination occurring in the middle of a physics event call a should not affect the functionality of Isaac Lab. It is safe to ignore the error message and continue with terminating the process. On Windows systems, please use ``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 +----------------------- + +In Isaac Sim 5.0, we have observed some workflows exiting with an ``OSError`` indicating +``version 'GLIBCXX_3.4.30' not found`` when running from a conda environment. +The issue apperas to be stemming from importing torch or torch-related packages, such as tensorboard, +prior to launching ``AppLauncher``. As a workaround, ensure that all torch imports happen after +the ``AppLauncher`` instance has been created, which should resolve the error. diff --git a/docs/source/refs/license.rst b/docs/source/refs/license.rst index 4d907efa15e2..4e1a29658733 100644 --- a/docs/source/refs/license.rst +++ b/docs/source/refs/license.rst @@ -3,15 +3,14 @@ License ======== -NVIDIA Isaac Sim is available freely under `individual license -`_. For more information -about its license terms, please check `here `_. +NVIDIA Isaac Sim is licensed under Apache 2.0. For more information +about its license terms, please check `here `_. The license files for all its dependencies and included assets are available in its `documentation `_. The Isaac Lab framework is open-sourced under the -`BSD-3-Clause license `_. +`BSD-3-Clause license `_, with some dependencies licensed under other terms. .. code-block:: text diff --git a/docs/source/refs/reference_architecture/index.rst b/docs/source/refs/reference_architecture/index.rst index 342961445061..c875c964e26c 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 5ed1c6f117ec..8ba703ddcde2 100644 --- a/docs/source/refs/release_notes.rst +++ b/docs/source/refs/release_notes.rst @@ -4,6 +4,1063 @@ Release Notes The release notes are now available in the `Isaac Lab GitHub repository `_. We summarize the release notes here for convenience. +v2.3.2 +====== + +What's Changed +-------------- + +This release focuses on stability, infrastructure improvements, workflow refinements, and incremental feature expansions, along with some significant new features, including **Multirotor and thruster support for drones**, **Multi-mesh RayCaster**, **Visual-based tactile sensor**, **Haply device integration**, and new **OpenArm environments**. It includes improvements to training workflows, teleoperation and Mimic pipelines, Ray integration, simulation utilities, and developer tooling, along with a large number of robustness and quality-of-life fixes. + +This will be our final release on the current **main** branch as we shift our development focus towards the **develop** branch. We anticipate large restructuring changes to happen on **develop**. While we hope to continue taking in contributions from the community, we will focus more time on our development towards Isaac Lab 3.0. For existing PRs, please re-target the target branch to **develop** to stay up-to-date with the latest changes. + +New Features +------------ + +Core & Simulation +~~~~~~~~~~~~~~~~~ + +* Adds Raycaster with tracking support for dynamic meshes by @renezurbruegg in https://github.com/isaac-sim/IsaacLab/pull/3298 +* Adds visual-based tactile sensor with shape sensing example by @JuanaDd in https://github.com/isaac-sim/IsaacLab/pull/3420 +* Adds wrench composers allowing the composition of multiple wrenches on the same bodies by @AntoineRichard in https://github.com/isaac-sim/IsaacLab/pull/3287 +* Adds multirotor/thruster actuator, multirotor asset and manager-based ARL drone task https://github.com/isaac-sim/IsaacLab/pull/3760 by @mihirk284 @grzemal @Zwoelf12 +* Adds automatic transform discovery for IMU sensors to find valid parent bodies by @bmccann-bdai in https://github.com/isaac-sim/IsaacLab/pull/3864 +* Adds friction force reporting to ContactSensor by @gattra-rai in https://github.com/isaac-sim/IsaacLab/pull/3563 +* Adds MJCF spawner for importing MJCF-based assets by @KyleM73 in https://github.com/isaac-sim/IsaacLab/pull/1672 + +Learning & Environments +~~~~~~~~~~~~~~~~~~~~~~~ + +* Adds OpenArm environments by @JinnnK in https://github.com/isaac-sim/IsaacLab/pull/4089 + +Mimic & Teleoperation +~~~~~~~~~~~~~~~~~~~~~ + +* Adds Haply device API with force feedback and teleoperation demo by @mingxueg-nv in https://github.com/isaac-sim/IsaacLab/pull/3873 +* Refactors retargeters and adds Quest retargeters for G1 tasks by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3950 +* Adds Arena G1 locomanipulation retargeters by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/4140 +* Adds APIs to Isaac Lab Mimic for loco-manipulation data generation by @peterd-NV in https://github.com/isaac-sim/IsaacLab/pull/3992 + +Improvements +------------ + +Core & Simulation +~~~~~~~~~~~~~~~~~ + +* Adds preserve-order flag to JointPositionToLimitsAction by @renezurbruegg in https://github.com/isaac-sim/IsaacLab/pull/3716 +* Adds parsing of instanced meshes to prim fetching utilities by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/3367 +* Adds configurable logdir parameter to environments by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3391 +* Exposes PhysX flag solveArticulationContactLast via PhysxCfg by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3502 +* Removes pickle dependency for config load and dump by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3709 +* Improves recorder manager to support custom demo indices by @rebeccazhang0707 in https://github.com/isaac-sim/IsaacLab/pull/3552 +* Normalizes Python logging by replacing remaining omni.log usage by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/3912 +* Replaces Isaac Sim stage_utils, prim_utils, and nucleus_utils with Isaac Lab implementations by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/3921, https://github.com/isaac-sim/IsaacLab/pull/3923, https://github.com/isaac-sim/IsaacLab/pull/3924 +* Breaks actuator configuration into multiple files to avoid circular imports by @bmccann-bdai in https://github.com/isaac-sim/IsaacLab/pull/3994 +* Moves logging configuration into shared utilities by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4298 +* Caches Isaac Sim package version for faster lookup by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4299 +* Simplifies imports of stage and prim utilities by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4286 +* Randomizes viscous and dynamic joint friction consistent with Isaac Sim 5.0 by @GiulioRomualdi in https://github.com/isaac-sim/IsaacLab/pull/3318 +* Prevents randomization of rigid body mass to zero or negative values by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/4060 +* Improves image plotting normalization and colorization by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4302 +* Adds Fabric backend support to isaaclab.sim.views.XformPrimView by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/4374 + +Learning & Environments +~~~~~~~~~~~~~~~~~~~~~~~ + +* Enhances PBT usability with small workflow improvements by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3449 +* Supports vectorized environments for pick-and-place demo by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3996 +* Registers direct environments to Gymnasium using string-style imports by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3803 +* Updates Gymnasium dependency to version 1.2.1 by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/3696 +* Updates SB3 PPO configuration to reduce excessive training time by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3726 +* Adds support for validating replay success using task termination conditions by @yami007007 in https://github.com/isaac-sim/IsaacLab/pull/4170 +* Adds early stopping support for Ray-based training by @ozhanozen in https://github.com/isaac-sim/IsaacLab/pull/3276 +* Adds support for custom ProgressReporter implementations in Ray integration by @ozhanozen in https://github.com/isaac-sim/IsaacLab/pull/3269 +* Updates rsl_rl to version 3.1.2 to support state-dependent standard deviation by @ashwinvkNV in https://github.com/isaac-sim/IsaacLab/pull/3867 + +Infrastructure +~~~~~~~~~~~~~~ + +* Switches linting and import sorting to Ruff by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4329, https://github.com/isaac-sim/IsaacLab/pull/4377 +* Moves flake8 and pytest configuration into pyproject.toml by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4335, https://github.com/isaac-sim/IsaacLab/pull/4376 +* Removes dependency on XformPrim for create_prim by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4307 +* Updates copyright year to 2026 by @ashwinvkNV in https://github.com/isaac-sim/IsaacLab/pull/4311 +* Restricts .gitignore dataset rule to top-level directory only by @louislelay in https://github.com/isaac-sim/IsaacLab/pull/3400 +* Adds uv as an alternative to conda in isaaclab.sh by @KyleM73 in https://github.com/isaac-sim/IsaacLab/pull/3172 +* Fixes transformers dependency for theia issue and failing tests by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/4484 + +Bug Fixes +--------- + +Core & Simulation +~~~~~~~~~~~~~~~~~ + +* Fixes missing actuator indices variable in joint randomization by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3447 +* Fixes ViewportCameraController numpy array missing datatype by @T-K-233 in https://github.com/isaac-sim/IsaacLab/pull/3375 +* Fixes PDActuator docstring mismatch with implementation by @lorenwel in https://github.com/isaac-sim/IsaacLab/pull/3493 +* Fixes rail difficulty-based height computation in mesh terrains by @KyleM73 in https://github.com/isaac-sim/IsaacLab/pull/3254 +* Fixes contact threshold handling when activating contact sensors by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3498 +* Fixes indexing errors in joint parameter randomization by @GiulioRomualdi in https://github.com/isaac-sim/IsaacLab/pull/4051 +* Fixes noisy velocities near joint limits by @AntoineRichard in https://github.com/isaac-sim/IsaacLab/pull/3989 +* Fixes mesh converter not setting collision approximation attributes by @Soappyooo in https://github.com/isaac-sim/IsaacLab/pull/4082 +* Fixes returned normal tensor shape in TiledCamera by @Rabbit-Hu in https://github.com/isaac-sim/IsaacLab/pull/4241 +* Fixes advanced indexing shape mismatch in JointPositionToLimitsAction by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3865 +* Fixes teleoperation crash when using DirectRL environments by @emmanuel-ferdman in https://github.com/isaac-sim/IsaacLab/pull/4364 +* Fixes lidar pattern horizontal resolution bug by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/4452 + +Learning & Environments +~~~~~~~~~~~~~~~~~~~~~~~ + +* Fixes CUDA version parsing for AutoMate environments by @yijieg in https://github.com/isaac-sim/IsaacLab/pull/3795 + +Infrastructure & Tooling +~~~~~~~~~~~~~~~~~~~~~~~~ + +* Fixes CI behavior to correctly fail fork PRs when general tests fail by @nv-apoddubny in https://github.com/isaac-sim/IsaacLab/pull/3412 +* Fixes docker availability check in isaaclab.sh on systems without Docker by @klakhi in https://github.com/isaac-sim/IsaacLab/pull/4180 +* Forces CRLF line endings for .bat files to avoid Windows execution errors by @jiang131072 in https://github.com/isaac-sim/IsaacLab/pull/3624 +* Fixes environment test failures and disables unstable tests by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3413 +* Fixes vulnerability in eval usage for Ray resource parsing by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/4425 +* Fixes curobo dockerfile for CI runs by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/4462 + +Documentation +------------- + +* Improves contribution guidelines for Isaac Lab by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/3403 +* Abstracts common installation steps in documentation by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/3445 +* Updates SkillGen documentation with data generation commands and success rates by @njawale42 in https://github.com/isaac-sim/IsaacLab/pull/3702 +* Adds Newton Beta documentation updates and visualizer guidance by @kellyguo11 and @Milad-Rakhsha-NV in https://github.com/isaac-sim/IsaacLab/pull/3518, https://github.com/isaac-sim/IsaacLab/pull/3551 +* Adds automated checks for broken documentation links and fixes existing ones by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3888 +* Updates technical report link for Isaac Lab by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4074 +* Adds clarification on missing pip in uv virtual environments by @DBinK in https://github.com/isaac-sim/IsaacLab/pull/4055 +* Adds keyword filtering documentation for list_envs.py by @louislelay in https://github.com/isaac-sim/IsaacLab/pull/3384 +* Adds documentation for Multirotor feature by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/4400 +* Adds documentation for PVD and OVD comparison by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/4409 + +Migration Guide +--------------- + +External Force and Torque Application - Wrench Composers +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + + +The ``set_external_force_and_torque()`` method on articulations, rigid bodies, and rigid body collections has been deprecated in favor of a new composable wrench system. + +Related PR: https://github.com/isaac-sim/IsaacLab/pull/3287 + +**New Features:** +- **Permanent Wrench Composer**: Applies forces/torques that persist across simulation steps until explicitly changed +- **Instantaneous Wrench Composer**: Applies forces/torques for a single simulation step, then automatically resets +- **Composability**: Multiple forces and torques can now be added together on the same body +- **Mixed Frame Support**: Seamlessly compose local and global frame wrenches + +**Migration Guide:** + +**Old API (Deprecated):** + +.. code-block:: python + + # Old method - overwrites previous forces + asset.set_external_force_and_torque( + forces=torch.ones(1, 1, 3), + torques=torch.ones(1, 1, 3), + body_ids=[0], + env_ids=[0], + is_global=False, + ) + +**New API:** + +.. code-block:: python + + # Set initial permanent forces (replaces previous) + asset.permanent_wrench_composer.set_forces_and_torques( + forces=torch.ones(1, 1, 3), + env_ids=[0], + body_ids=[0], + ) + + # Compose additional forces on the same body + asset.permanent_wrench_composer.add_forces_and_torques( + forces=torch.ones(1, 1, 3), + env_ids=[0], + body_ids=[0], + is_global=True, # Mix local and global frames + ) + + # Add torques independently + asset.permanent_wrench_composer.add_forces_and_torques( + torques=torch.ones(1, 1, 3), + env_ids=[0], + body_ids=[0], + ) + + # Apply forces and torques together with custom application points + asset.permanent_wrench_composer.add_forces_and_torques( + forces=torch.ones(1, 1, 3), + torques=torch.ones(1, 1, 3), + positions=torch.ones(1, 1, 3), + env_ids=[0], + body_ids=[0], + ) + +**Instantaneous Wrenches (New):** + +.. code-block:: python + + # Apply forces for a single simulation step only + asset.instantaneous_wrench_composer.add_forces_and_torques( + forces=torch.ones(1, 1, 3), + env_ids=[0], + body_ids=[0], + ) + + # Multiple instantaneous wrenches compose automatically + asset.instantaneous_wrench_composer.add_forces_and_torques( + forces=torch.ones(1, 2, 3), # Add more forces + env_ids=[0], + body_ids=[0, 1], + ) + # These are automatically reset after write_data_to_sim() + +**Key Differences:** + +- ``set_forces_and_torques()`` replaces existing wrenches +- ``add_forces_and_torques()`` composes with existing wrenches +- Permanent and instantaneous wrenches compose automatically +- Instantaneous wrenches auto-clear after each simulation step + +**Use Cases:** +- **Drones**: Compose thrust forces with aerodynamic drag and wind disturbances +- **Boats**: Apply buoyancy forces with wave-induced motions + + +Formatting and Linting - Migration to Ruff +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The project has migrated from multiple tools (``flake8`` for linting, ``black`` for formatting, ``isort`` for import sorting) to a unified toolchain using ``ruff`` for all formatting and linting tasks. + +Related PRs: https://github.com/isaac-sim/IsaacLab/pull/4329, https://github.com/isaac-sim/IsaacLab/pull/4377, https://github.com/isaac-sim/IsaacLab/pull/4335, https://github.com/isaac-sim/IsaacLab/pull/4376 + + +**Why:** + +- Faster performance (10-100x speedup) +- Unified configuration in ``pyproject.toml`` +- More consistent formatting and linting rules +- Simplified developer workflow + +**Migration Steps:** + +1. **Update configuration files:** + + .. code-block:: bash + + # Copy the updated configuration from the main branch + # Files to update: pyproject.toml, .pre-commit-config.yaml + +2. **Apply new formatting:** + + .. code-block:: bash + + ./isaaclab.sh --format + +3. **Resolve merge conflicts:** + If you encounter merge conflicts after updating, they likely originate from formatting differences. After copying the new configuration files, rerun the formatting command and commit the changes. + +.. note:: + + Pre-commit hooks will automatically run ``ruff`` on staged files. Ensure your code is formatted + before committing to avoid CI failures. + + +USD Utilities - Unified ``isaaclab.sim.utils`` Module +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Isaac Lab now provides its own comprehensive USD utility module (``isaaclab.sim.utils``) instead of relying on scattered utilities from Isaac Sim's ``isaacsim.core.utils`` packages. + +Related PR: https://github.com/isaac-sim/IsaacLab/pull/4286 + +**Why:** + +- **Better Organization**: All USD operations grouped into logical submodules (stage, prims, queries, transforms, semantics) +- **Type Hints**: Full type annotations for better IDE support and code safety +- **Version Compatibility**: Handles differences between Isaac Sim versions automatically + +**Old API (Isaac Sim utilities):** + +.. code-block:: python + + import isaac.core.utils.stage as stage_utils + import isaac.core.utils.prims as prim_utils + + # Stage operations + stage_utils.create_new_stage() + current_stage = stage_utils.get_current_stage() + + # Prim operations + prim_utils.create_prim("/World/Cube", "Cube") + prim_utils.delete_prim("/World/OldObject") + +**New API (Isaac Lab utilities):** + +.. code-block:: python + + import isaaclab.sim as sim_utils + + # Stage operations + sim_utils.create_new_stage() + current_stage = sim_utils.get_current_stage() + + # Prim operations + sim_utils.create_prim("/World/Cube", "Cube", attributes={"size": 1.0}) + sim_utils.delete_prim("/World/OldObject") + +**Legacy Support:** + +For backward compatibility, legacy functions are still available in ``isaaclab.sim.utils.legacy``, but it's recommended to migrate to the new APIs or use USD directly. + +**Full Changelog**: https://github.com/isaac-sim/IsaacLab/compare/v2.2.1...v2.3.2 + +v2.3.1 +====== + +What's Changed +-------------- + +This is a small patch release with a few critical fixes that impacted user workflows. + +Key fixes include: +* The behavior of termination logging has changed in the manager-based workflow, where ``get_done_term`` now returns the current step value instead of the last episode value. +* Additionally, a breaking change in the URDF importer was introduced in Isaac Sim 5.1, where the merge joints flag is no longer supported. We have now introduced a patch in the importer to return the behavior. Moving forward, we plan to deprecate this flag in favor of preserving asset definitions from URDFs directly without performing additional processing during the import process. + +Bug Fixes +--------- + +* Updates URDF importer to 2.4.31 to continue support for merge-joints by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/4000 +* Separates per-step termination and last-episode termination bookkeeping by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3745 +* Uses effort_limit from USD if not specified in actuator cfg by @JuanaDd in https://github.com/isaac-sim/IsaacLab/pull/3522 +* Fixes type name for tendon properties in from_files config by @KyleM73 in https://github.com/isaac-sim/IsaacLab/pull/3941 +* Fixes duplicated text in pip installation docs by @shryt in https://github.com/isaac-sim/IsaacLab/pull/3969 +* Pins python version of pre-commmit.yaml workflow by @hhansen-bdai in https://github.com/isaac-sim/IsaacLab/pull/3929 + +Documentation +------------- + +* Updates the mimic teleop doc to link to the locomotion policy training by @huihuaNvidia2023 in https://github.com/isaac-sim/IsaacLab/pull/4053 + +**Full Changelog**: https://github.com/isaac-sim/IsaacLab/compare/v2.3.0...v2.3.1 + +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 +====== + +Overview +-------- + +This is a minor patch release with some improvements and bug fixes. + +Full Changelog: https://github.com/isaac-sim/IsaacLab/compare/v2.2.0...v2.2.1 + +New Features +------------ + +- Adds contact point location reporting to ContactSensor by @jtigue-bdai +- Adds environments actions/observations descriptors for export by @AntoineRichard +- Adds RSL-RL symmetry example for cartpole and ANYmal locomotion by @Mayankm96 + +Improvements +------------ + +Core API +~~~~~~~~ + +- Enhances Pink IK controller with null-space posture control and improvements by @michaellin6 +- Adds periodic logging when checking USD path on Nucleus server by @matthewtrepte +- Disallows string value written in sb3_ppo_cfg.yaml from being evaluated in process_sb3_cfg by @ooctipus + +Infrastructure +~~~~~~~~~~~~~~ + +* **Application Settings** + - Disables rate limit for headless and headless rendering app by @matthewtrepte, @kellyguo11 + - Disables ``rtx.indirrectDiffuse.enabled`` in render preset balanced and performance modes by @matthewtrepte + - Sets profiler backend to NVTX by default by @soowanpNV, @rwiltz +* **Dependencies** + - Adds hf-xet license by @hhansen-bdai + - Fixes new typing-inspection dependency license by @kellyguo11 +* **Testing & Benchmarking** + - Adds basic validation tests for scale-based randomization ranges by @louislelay + - Adds ``SensorBase`` tests by @jtigue-bdai +* **Repository Utilities** + - Adds improved readout from install_deps.py by @hhansen-bdai + - Fixes isaaclab.sh to detect isaacsim_version accurately 4.5 or >= 5.0 by @ooctipus + - Disables verbose printing in conftest.py by @ooctipus + - Updates pytest flags for isaacsim integration testing by @ben-johnston-nv + - Updates CodeOwners to be more fine-grained by @pascal-roth + - Fixes minor issues in CI by @nv-apoddubny + +Bug Fixes +--------- + +Core API +~~~~~~~~ + +* **Asset Interfaces** + - Fixes setting friction coefficients into PhysX in the articulation classes by @ossamaAhmed + - Sets joint_friction_coeff only for selected physx_env_ids by @ashwinvkNV +* **Manager Interfaces** + - Fixes observation space Dict for non-concatenated groups only keeping the last term by @CSCSX +* **MDP Terms** + - Fixes termination term effort limit check logic by @moribots + - Broadcasts environment ids inside ``mdp.randomize_rigid_body_com`` by @Foruck + - Fixes IndexError in reset_joints_by_scale and reset_joints_by_offset by @Creampelt + - Fixes ``terrain_out_of_bounds`` to return tensor instead of bool by @fan-ziqi + +Infrastructure +~~~~~~~~~~~~~~ + +- Fixes distributed training hanging issue by @kellyguo11 +- Disables generation of internal template when detecting isaaclab install via pip by @ooctipus +- Fixes typo in isaaclab.bat by @ooctipus +- Updates app pathing for user-provided rendering preset mode by @matthewtrepte + +Documentation +------------- + +- Adds documentation for Newton integration by @mpgussert +- Adapts FAQ section in docs with Isaac Sim open-sourcing by @Mayankm96 +- Changes checkpoint path in rsl-rl to an absolute path in documentation by @fan-ziqi +- Fixes MuJoCo link in docs by @fan-ziqi +- Adds client version direction to XR document by @lotusl-code +- Fixes broken link in doc by @kellyguo11 +- Fixes typo in list_envs.py script path by @fbeltrao +- Fixes Franka blueprint env ID in docs by @louislelay + +Breaking Changes +---------------- + +- Improves termination manager logging to report aggregated percentage of environments done due to each term by @ooctipus + + +v2.2.0 +====== + +Overview +-------- + +**Isaac Lab 2.2** brings major upgrades across simulation capabilities, tooling, and developer experience. It expands support for advanced physics features, new environments, and improved testing and documentation workflows. This release includes full compatibility with **Isaac Sim 5.0** as well as backwards compatibility with **Isaac Sim 4.5**. + +Key highlights of this release include: + +- **Enhanced Physics Support**: Updated `joint friction modeling using the latest PhysX APIs `_, added support for `spatial tendons `_, and improved surface gripper interactions. +- **New Environments for Imitation Learning**: Introduction of two new GR1 mimic environments, with domain randomization and visual robustness evaluation, and improved pick-and-place tasks. +- **New Contact-Rich Manipulation Tasks**: Integration of `FORGE `_ and `AutoMate `_ tasks for learning fine-grained contact interactions in simulation. +- **Teleoperation Improvements**: Teleoperation tools have been enhanced with configurable parameters and CloudXR runtime updates, including head tracking and hand tracking. +- **Performance & Usability Improvements**: Includes support for Stage in Memory and Cloning in Fabric for faster scene creation, new OVD recorder for large-scene GPU-based animation recording, and FSD (Fabric Scene Delegate) for improved rendering speed. +- **Improved Documentation**: The documentation has been extended and updated to cover new features, resolve common issues, and streamline setup, including updates to teleop system requirements, VS Code integration, and Python environment management. + +**Full Changelog**: https://github.com/isaac-sim/IsaacLab/compare/v2.1.1...v2.2.0 + + +Isaac Sim 5.0 Updates +--------------------- + +* Fixes rendering issues on Blackwell GPUs that previously resulted in overly noisy renders +* Updates Python version from 3.10 to 3.11 +* Updates PyTorch version to torch 2.7.0+cu128, which will include Blackwell support +* Drops official support for Ubuntu 20.04, we now officially support Ubuntu 22.04 and 24.04 Linux platforms +* Isaac Sim 5.0 no longer sets ``/app/player/useFixedTimeStepping=False`` by default. We now do this in Isaac Lab. +* :attr:`~isaaclab.sim.spawners.PhysicsMaterialCfg.improve_patch_friction` is now removed. The simulation will always behave as if this attribute is set to true. +* Native Livestreaming support has been removed. ``LIVESTREAM=1`` can now be used for WebRTC streaming over public networks and + ``LIVESTREAM=2`` for private and local networks with WebRTC streaming. +* Some assets in Isaac Sim have been reworked and restructured. Notably, the following asset paths were updated: + + * ``Robots/Ant/ant_instanceable.usd`` --> ``Robots/IsaacSim/Ant/ant_instanceable.usd`` + * ``Robots/Humanoid/humanoid_instanceable.usd`` --> ``Robots/IsaacSim/Humanoid/humanoid_instanceable.usd`` + * ``Robots/ANYbotics/anymal_instanceable.usd`` --> ``Robots/ANYbotics/anymal_c/anymal_c.usd`` + * ``Robots/ANYbotics/anymal_c.usd`` --> ``Robots/ANYbotics/anymal_c/anymal_c.usd`` + * ``Robots/Franka/franka.usd`` --> ``Robots/FrankaRobotics/FrankaPanda/franka.usd`` + * ``Robots/AllegroHand/allegro_hand_instanceable.usd`` --> ``Robots/WonikRobotics/AllegroHand/allegro_hand_instanceable.usd`` + * ``Robots/Crazyflie/cf2x.usd`` --> ``Robots/Bitcraze/Crazyflie/cf2x.usd`` + * ``Robots/RethinkRobotics/sawyer_instanceable.usd`` --> ``Robots/RethinkRobotics/Sawyer/sawyer_instanceable.usd`` + * ``Robots/ShadowHand/shadow_hand_instanceable.usd`` --> ``Robots/ShadowRobot/ShadowHand/shadow_hand_instanceable.usd`` + + +New Features +------------ + +* Adds FORGE tasks for contact-rich manipulation with force sensing to IsaacLab by @noseworm in #2968 +* Adds two new GR1 environments for IsaacLab Mimic by @peterd-NV +* Adds stack environment, scripts for Cosmos, and visual robustness evaluation by @shauryadNv +* Updates Joint Friction Parameters to Isaac Sim 5.0 PhysX APIs by @ossamaAhmed +* Adds support for spatial tendons by @ossamaAhmed +* Adds support and example for SurfaceGrippers by @AntoineRichard +* Adds support for stage in memory by @matthewtrepte +* Adds OVD animation recording feature by @matthewtrepte + +Improvements +------------ + +* Enables FSD for faster rendering by @nv-mm +* Sets rtx.indirectDiffuse.enabled to True for performance & balanced rendering presets by @matthewtrepte +* Changes runner for post-merge pipeline on self-hosted runners by @nv-apoddubny +* Fixes and improvements for CI pipeline by @nv-apoddubny +* Adds flaky annotation for tests by @kellyguo11 +* Updates Mimic test cases to pytest format by @peterd-NV +* Updates cosmos test files to use pytest by @shauryadNv +* Updates onnx and protobuf version due to vulnerabilities by @kellyguo11 +* Updates minimum skrl version to 1.4.3 by @Toni-SM +* Updates to Isaac Sim 5.0 by @kellyguo11 +* Updates docker CloudXR runtime version by @lotusl-code +* Removes xr rendering mode by @rwiltz +* Migrates OpenXRDevice from isaacsim.xr.openxr to omni.xr.kitxr by @rwiltz +* Implements teleop config parameters and device factory by @rwiltz +* Updates pick place env to use steering wheel asset by @peterd-NV +* Adds a CLI argument to set epochs for Robomimic training script by @peterd-NV + +Bug Fixes +--------- + +* Fixes operational space unit test to avoid pi rotation error by @ooctipus +* Fixes GLIBC errors with importing torch before AppLauncher by @kellyguo11 +* Fixes rendering preset by @matthewtrepte in cc0dab6cd50778507efc3c9c2d74a28919ab2092 +* Fixes callbacks with stage in memory and organize environment tests by @matthewtrepte +* Fixes XR and external camera bug with async rendering by @rwiltz +* Disables selection for rl_games when marl is selected for template generator by @ooctipus +* Adds check for .gitignore when generating template by @kellyguo11 +* Fixes camera obs errors in stack instance randomize envs by @peterd-NV +* Fixes parsing for play envs by @matthewtrepte +* Fixes issues with consecutive python exe calls in isaaclab.bat by @kellyguo11 +* Fixes spacemouse add callback function by @peterd-NV +* Fixes humanoid training with new velocity_limit_sim by @AntoineRichard + +Documentation +------------- + +* Adds note to mimic cosmos pipeline doc for eval by @shauryadNv +* Updates teleop docs for 2.2 release by @rwiltz +* Fixes outdated dofbot path in tutorial scripts by @mpgussert +* Updates docs for VS Code IntelliSense setup and JAX installation by @Toni-SM +* Updates Jax doc to overwrite version < 0.6.0 for torch by @kellyguo11 +* Adds docs for fabric cloning & stage in memory by @matthewtrepte +* Updates driver requirements to point to our official technical docs by @mpgussert +* Adds warning for ovd recording warning logs spam by @matthewtrepte +* Adds documentation to specify HOVER version and known GLIBCXX error by @kellyguo11 +* Updates teleop system requirements doc by @lotusl-code +* Add network requirements to cloudxr teleop doc by @lotusl-code + + +v2.1.1 +====== + +Overview +-------- + +This release has been in development over the past few months and includes a significant number of updates, +enhancements, and new features across the entire codebase. Given the volume of changes, we've grouped them +into relevant categories to improve readability. This version is compatible with +`NVIDIA Isaac Sim 4.5 `__. + +We appreciate the community's patience and contributions in ensuring quality and stability throughout. +We're aiming for more frequent patch releases moving forward to improve the developer experience. + +**Note:** This minor release does not include a Docker image or pip package. + +**Full Changelog:** https://github.com/isaac-sim/IsaacLab/compare/v2.1.0...v2.1.1 + +New Features +------------ + +* **Asset Interfaces** + * Adds ``position`` argument to set external forces and torques at different locations on the rigid body by @AntoineRichard + * Adds ``body_incoming_joint_wrench_b`` to ArticulationData field by @jtigue-bdai + * Allows selecting articulation root prim explicitly by @lgulich +* **Sensor Interfaces** + * Draws connection lines for FrameTransformer visualization by @Mayankm96 + * Uses visualization marker for connecting lines inside FrameTransformer by @bikcrum +* **MDP Terms** + * Adds ``body_pose_w`` and ``body_projected_gravity_b`` observations by @jtigue-bdai + * Adds joint effort observation by @jtigue-bdai + * Adds CoM randomization term to manager-based events by @shendredm + * Adds time-based mdp (observation) functions by @TheIndoorDad + * Adds curriculum mdp term to modify any environment parameters by @ooctipus +* **New Example Tasks** + * Adds assembly tasks from the Automate project by @yijieg + * Adds digit locomotion examples by @lgulich + +Improvements +------------ + +Core API +~~~~~~~~ + +* **Actuator Interfaces** + * Fixes implicit actuator limits configs for assets by @ooctipus + * Updates actuator configs for Franka arm by @reeceomahoney +* **Asset Interfaces** + * Optimizes getters of data inside asset classes by @Mayankm96 + * Adds method to set the visibility of the Asset's prims by @Mayankm96 +* **Sensor Interfaces** + * Updates to ray caster ray alignment and customizable drift sampling by @jsmith-bdai + * Extends ``ContactSensorData`` by ``force_matrix_w_history`` attribute by @bikcrum + * Adds IMU ``projected_gravity_b`` and optimizations by @jtigue-bdai +* **Manager Interfaces** + * Adds serialization to observation and action managers by @jsmith-bdai + * Adds concatenation dimension to ``ObservationManager`` by @pascal-roth + * Supports composite observation space with min/max by @ooctipus + * Changes counter update in ``CommandManager`` by @pascal-roth + * Integrates ``NoiseModel`` to manager-based workflows by @ozhanozen + * Updates ``NoiseModelWithAdditiveBias`` to apply per-feature bias by @ozhanozen + * Fixes :meth:`isaaclab.scene.reset_to` to accept ``None`` by @ooctipus + * Resets step reward buffer properly by @bikcrum +* **Terrain Generation** + * Custom ``TerrainGenerator`` support by @pascal-roth + * Adds terrain border options by @pascal-roth + * Platform height independent of object height by @jtigue-bdai + * Adds noise to ``MeshRepeatedObjectsTerrain`` by @jtigue-bdai +* **Simulation** + * Raises exceptions from SimContext init callbacks + * Applies ``semantic_tags`` to ground by @KumoLiu + * Sets ``enable_stabilization`` to false by default by @AntoineRichard + * Fixes deprecation for ``pxr.Semantics`` by @kellyguo11 +* **Math Utilities** + * Improves ``euler_xyz_from_quat`` by @ShaoshuSu + * Optimizes ``yaw_quat`` by @hapatel-bdai + * Changes ``quat_apply`` and ``quat_apply_inverse`` by @jtigue-bdai + * Changes ``quat_box_minus`` by @jtigue-bdai + * Adds ``quat_box_plus`` and ``rigid_body_twist_transform`` by @jtigue-bdai + * Adds math tests for transforms by @jtigue-bdai +* **General Utilities** + * Simplifies buffer validation for ``CircularBuffer`` by @Mayankm96 + * Modifies ``update_class_from_dict()`` by @ozhanozen + * Allows slicing from list values in dicts by @LinghengMeng @kellyguo11 + +Tasks API +~~~~~~~~~ + +* Adds support for ``module:task`` and gymnasium >=1.0 by @kellyguo11 +* Adds RL library error hints by @Toni-SM +* Enables hydra for ``play.py`` scripts by @ooctipus +* Fixes ray metric reporting and hangs by @ozhanozen +* Adds gradient clipping for distillation (RSL-RL) by @alessandroassirelli98 +* GRU-based RNNs ONNX export in RSL RL by @WT-MM +* Adds wandb support in rl_games by @ooctipus +* Optimizes SB3 wrapper by @araffin +* Enables SB3 checkpoint loading by @ooctipus +* Pre-processes SB3 env image obs-space for CNN pipeline by @ooctipus + +Infrastructure +~~~~~~~~~~~~~~ + +* **Dependencies** + * Updates torch to 2.7.0 with CUDA 12.8 by @kellyguo11 + * Updates gymnasium to 1.2.0 by @kellyguo11 + * Fixes numpy version to <2 by @ooctipus + * Adds license file for OSS by @kellyguo11 + * Sets robomimic to v0.4.0 by @masoudmoghani + * Upgrades pillow for Kit 107.3.1 by @ooctipus + * Removes protobuf upper pin by @kwlzn +* **Docker** + * Uses ``--gpus`` instead of Nvidia runtime by @yanziz-nvidia + * Adds docker name suffix parameter by @zoemcc + * Adds bash history support in docker by @AntoineRichard +* **Testing & Benchmarking** + * Switches unittest to pytest by @kellyguo11 @pascal-roth + * Adds training benchmark unit tests by @matthewtrepte + * Fixes env and IK test failures by @kellyguo11 +* **Repository Utilities** + * Adds URDF to USD batch conversion script by @hapatel-bdai + * Adds repository citation link by @kellyguo11 + * Adds pip install warning for internal templates by @ooctipus + +Bug Fixes +--------- + +Core API +~~~~~~~~ + +* **Actuator Interfaces** + * Fixes DCMotor clipping for negative power by @jtigue-bdai +* **Asset Interfaces** + * Fixes inconsistent data reads for body/link/com by @ooctipus +* **Sensor Interfaces** + * Fixes pose update in ``Camera`` and ``TiledCamera`` by @pascal-roth + * Fixes CPU fallback in camera.py by @renaudponcelet + * Fixes camera intrinsics logic by @jtigue-bdai +* **Manager Interfaces** + * Fixes ``ObservationManager`` buffer overwrite by @patrickhaoy + * Fixes term check in event manager by @miguelalonsojr + * Fixes ``Modifiers`` and history buffer bug by @ZiwenZhuang + * Fixes re-init check in ``ManagerBase`` by @Mayankm96 + * Fixes CPU collision filtering by @kellyguo11 + * Fixes imports in InteractiveScene/LiveVisualizer by @Mayankm96 + * Fixes image plot import in Live Visualizer by @pascal-roth +* **MDP Terms** + * Fixes CoM randomization shape mismatch by @shendredm + * Fixes visual prim handling in texture randomization by @KumoLiu + * Resets joint targets in ``reset_scene_to_default`` by @wghou + * Fixes joint limit terminations by @GiulioRomualdi + * Fixes joint reset scope in ``SceneEntityCfg`` by @ooctipus +* **Math Utilities** + * Fixes ``quat_inv()`` implementation by @ozhanozen + +Tasks API +~~~~~~~~~ + +* Fixes LSTM to ONNX export by @jtigue-bdai + +Example Tasks +~~~~~~~~~~~~~ + +* Removes contact termination redundancy by @louislelay +* Fixes memory leak in SDF by @leondavi +* Changes ``randomization`` to ``events`` in Digit envs by @fan-ziqi + +Documentation +------------- + +* Adds Isaac Sim version section to README by @kellyguo11 +* Adds physics performance guide by @kellyguo11 +* Adds jetbot tutorial to walkthrough docs by @mpgussert +* Changes quickstart install to conda by @mpgussert +* Fixes typo in library docs by @norbertcygiert +* Updates docs for conda, fabric, inference by @kellyguo11 +* Adds license/contributing updates with DCO by @kellyguo11 +* Updates pytest docs and help by @louislelay +* Adds actuator reference docs by @AntoineRichard +* Updates multi-GPU PyTorch setup docs by @Alex-Omar-Nvidia +* Removes deprecated env var in docs by @Kyu3224 + + +v2.1.0 +====== + +Overview +-------- + +This release introduces the official support for teleoperation using the Apple Vision Pro for collecting high-quality +and dexterous hand data, including the addition of bi-manual teleoperation and imitation learning workflows through Isaac Lab Mimic. + +We have also introduced new randomization methods for USD attributes, including the randomization of +scale, color, and textures. In this release, we updated RSL RL to v2.3.1, which introduces many additional features +including distributed training, student-teacher distillation, and recurrent student-teacher distillation. + +Additionally, we revamped the `Extension Template `_ +to include an automatic template generator tool from within the Isaac Lab repo. The extension template is +a powerful method for users to develop new projects in user-hosted repos, allowing for isolation from the core +Isaac Lab repo and changes. The previous IsaacLabExtensionTemplate repo showed a limited example pertaining only +to the Manager-based workflow and RSL RL. In the new template generator, users can choose from any supported +workflow and RL library, along with the desired RL algorithm. We will be deprecating the standalone +`IsaacLabExtensionTemplate `_ in the near future. + +NVIDIA has also released `HOVER `_ as an independent repo, hosting a neural whole body +controller for humanoids built on top of Isaac Lab. HOVER includes sim-to-real workflows for deployment on the Unitree +H1 robot, which we have also added a tutorial guide for the deployment process in the Isaac Lab documentation. + +**Full Changelog**: https://github.com/isaac-sim/IsaacLab/compare/v2.0.2...v2.1.0 + +New Features +------------ + +* Adds new external project / internal task template generator by @Toni-SM +* Adds dummy agents to the external task template generator by @louislelay +* Adds USD-level randomization mode to event manager by @Mayankm96 +* Adds texture and scale randomization event terms by @hapatel-bdai +* Adds replicator event for randomizing colors by @Mayankm96 +* Adds interactive demo script for H1 locomotion by @kellyguo11 +* Adds blueprint environment for Franka stacking mimic by @chengronglai +* Adds action clipping to rsl-rl wrapper by @Mayankm96 +* Adds Gymnasium spaces showcase tasks by @Toni-SM +* Add configs and adapt exporter for RSL-RL distillation by @ClemensSchwarke +* Adds support for head pose for Open XR device by @rwiltz +* Adds handtracking joints and retargetting pipeline by @rwiltz +* Adds documentation for openxr device and retargeters by @rwiltz +* Adds tutorial for training & validating HOVER policy using Isaac Lab by @pulkitg01 +* Adds rendering mode presets by @matthewtrepte +* Adds GR1 scene with Pink IK + Groot Mimic data generation and training by @ashwinvkNV +* Adds absolute pose franka cube stacking environment for mimic by @rwiltz +* Enables CloudXR OpenXR runtime container by @jaczhangnv +* Adds a quick start guide for quick installation and introduction by @mpgussert + +Improvements +------------ + +* Clarifies the default parameters in ArticulationData by @Mayankm96 +* Removes storage of meshes inside the TerrainImporter class by @Mayankm96 +* Adds more details about state in InteractiveScene by @Mayankm96 +* Mounts scripts to docker container by @Mayankm96 +* Initializes manager term classes only when sim starts by @Mayankm96 +* Updates to latest RSL-RL v2.3.0 release by @Mayankm96 +* Skips dependency installation for directories with no extension.toml by @jsmith-bdai +* Clarifies layer instructions in animation docs by @tylerlum +* Lowers the default number of environments for camera envs by @kellyguo11 +* Updates Rendering Mode guide in documentation by @matthewtrepte +* Adds task instruction UI support for mimic by @chengronglai +* Adds ExplicitAction class to track argument usage in AppLauncher by @nv-mhaselton +* Allows physics reset during simulation by @oahmednv +* Updates mimic to support multi-eef (DexMimicGen) data generation by @nvcyc + +Bug Fixes +--------- + +* Fixes default effort limit behavior for implicit actuators by @jtigue-bdai +* Fixes docstrings inconsistencies the code by @Bardreamaster +* Fixes missing stage recorder extension for animation recorder by @kellyguo11 +* Fixes ground height in factory environment by @louislelay +* Removes double definition of render settings by @pascal-roth +* Fixes device settings in env tutorials by @Mayankm96 +* Changes default ground color back to dark grey by @Mayankm96 +* Initializes extras dict before loading managers by @kousheekc +* Fixes typos in development.rst by @vi3itor +* Fixes SE gamepad omniverse subscription API by @PinkPanther-ny +* Fixes modify_action_space in RslRlVecEnvWrapper by @felipemohr +* Fixes distributed setup in benchmarking scripts by @kellyguo11 +* Fixes typo ``RF_FOOT`` to ``RH_FOOT`` in tutorials by @likecanyon +* Checks if success term exists before recording in RecorderManager by @peterd-NV +* Unsubscribes from debug vis handle when timeline is stopped by @jsmith-bdai +* Fixes wait time in ``play.py`` by using ``env.step_dt`` by @tylerlum +* Fixes 50 series installation instruction to include torchvision by @kellyguo11 +* Fixes importing MotionViewer from external scripts by @T-K-233 +* Resets cuda device after each app.update call by @kellyguo11 +* Fixes resume flag in rsl-rl cli args by @Mayankm96 + + v2.0.2 ====== @@ -153,7 +1210,7 @@ tiled-rendering performance, but at the cost of reduced rendering quality. This particularly affected dome lighting in the scene, which is the default in many of our examples. As reported by several users, this change negatively impacted render quality, even in -cases where it wasnโ€™t necessary (such as when recording videos of the simulation). In +cases where it wasn't necessary (such as when recording videos of the simulation). In response to this feedback, we have reverted to the previous render settings by default to restore the quality users expected. @@ -168,31 +1225,27 @@ Overview This release contains a small set of fixes and improvements. -The main change was to maintain combability with the updated library name for RSL RL, which breaks the previous installation methods for Isaac Lab. This release provides the necessary fixes and updates in Isaac Lab to accommodate for the name change and maintain combability with installation for RSL RL. +The main change was to maintain combability with the updated library name for RSL RL, which breaks the previous +installation methods for Isaac Lab. This release provides the necessary fixes and updates in Isaac Lab to accommodate +for the name change and maintain compatibility with installation for RSL RL. **Full Changelog**: https://github.com/isaac-sim/IsaacLab/compare/v2.0.0...v2.0.1 Improvements ------------ -* Switches to RSL-RL install from PyPI by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1811 -* Updates the script path in the document by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1766 -* Disables extension auto-reload when saving files by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1788 -* Updates documentation for v2.0.1 installation by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1818 +* Switches to RSL-RL install from PyPI by @Mayankm96 +* Updates the script path in the document by @fan-ziqi +* Disables extension auto-reload when saving files by @kellyguo11 +* Updates documentation for v2.0.1 installation by @kellyguo11 Bug Fixes --------- -* Fixes timestamp of com and link buffers when writing articulation pose to sim by @Jackkert in https://github.com/isaac-sim/IsaacLab/pull/1765 -* Fixes incorrect local documentation preview path in xdg-open command by @louislelay in https://github.com/isaac-sim/IsaacLab/pull/1776 -* Fixes no matching distribution found for rsl-rl (unavailable) by @samibouziri in https://github.com/isaac-sim/IsaacLab/pull/1808 -* Fixes reset of sensor drift inside the RayCaster sensor by @zoctipus in https://github.com/isaac-sim/IsaacLab/pull/1821 - -New Contributors ----------------- - -* @Jackkert made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1765 - +* Fixes timestamp of com and link buffers when writing articulation pose to sim by @Jackkert +* Fixes incorrect local documentation preview path in xdg-open command by @louislelay +* Fixes no matching distribution found for rsl-rl (unavailable) by @samibouziri +* Fixes reset of sensor drift inside the RayCaster sensor by @zoctipus v2.0.0 ====== @@ -274,10 +1327,10 @@ Breaking Changes .. attention:: We have identified a breaking feature for semantic segmentation and instance segmentation when using - ``TiledCamera`` with instanceable assets. Since the Isaac Sim 4.5 / Isaac Lab 2.0 release, semantic and instance + ``Camera`` and ``TiledCamera`` with instanceable assets. Since the Isaac Sim 4.5 / Isaac Lab 2.0 release, semantic and instance segmentation outputs only render the first tile correctly and produces blank outputs for the remaining tiles. We will be introducing a workaround for this fix to remove scene instancing if semantic segmentation or instance - segmentation is required for ``TiledCamera`` until we receive a proper fix from Omniverse as part of the next Isaac Sim release. + segmentation is required for ``Camera`` and ``TiledCamera`` until we receive a proper fix from Omniverse as part of the next Isaac Sim release. Migration Guide --------------- @@ -414,46 +1467,34 @@ which introduces breaking changes that will make Isaac Lab incompatible with ear New Features ------------ -* Adds documentation and demo script for IMU sensor by @mpgussert in https://github.com/isaac-sim/IsaacLab/pull/1694 +* Adds documentation and demo script for IMU sensor by @mpgussert Improvements ------------ -* Removes deprecation for root_state_w properties and setters by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/1695 -* Fixes MARL workflows for recording videos during training/inferencing by @Rishi-V in https://github.com/isaac-sim/IsaacLab/pull/1596 -* Adds body tracking option to ViewerCfg by @KyleM73 in https://github.com/isaac-sim/IsaacLab/pull/1620 -* Fixes the ``joint_parameter_lookup`` type in ``RemotizedPDActuatorCfg`` to support list format by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1626 -* Updates pip installation documentation to clarify options by @steple in https://github.com/isaac-sim/IsaacLab/pull/1621 -* Fixes docstrings in Articulation Data that report wrong return dimension by @zoctipus in https://github.com/isaac-sim/IsaacLab/pull/1652 -* Fixes documentation error for PD Actuator by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1668 -* Clarifies ray documentation and fixes minor issues by @garylvov in https://github.com/isaac-sim/IsaacLab/pull/1717 -* Updates code snippets in documentation to reference scripts by @mpgussert in https://github.com/isaac-sim/IsaacLab/pull/1693 -* Adds dict conversion test for ActuatorBase configs by @mschweig in https://github.com/isaac-sim/IsaacLab/pull/1608 +* Removes deprecation for root_state_w properties and setters by @jtigue-bdai +* Fixes MARL workflows for recording videos during training/inferencing by @Rishi-V +* Adds body tracking option to ViewerCfg by @KyleM73 +* Fixes the ``joint_parameter_lookup`` type in ``RemotizedPDActuatorCfg`` to support list format by @fan-ziqi +* Updates pip installation documentation to clarify options by @steple +* Fixes docstrings in Articulation Data that report wrong return dimension by @zoctipus +* Fixes documentation error for PD Actuator by @kellyguo11 +* Clarifies ray documentation and fixes minor issues by @garylvov +* Updates code snippets in documentation to reference scripts by @mpgussert +* Adds dict conversion test for ActuatorBase configs by @mschweig Bug Fixes --------- -* Fixes JointAction not preserving order when using all joints by @T-K-233 in https://github.com/isaac-sim/IsaacLab/pull/1587 -* Fixes event term for pushing root by setting velocity by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1584 -* Fixes error in Articulation where ``default_joint_stiffness`` and ``default_joint_damping`` are not correctly set for implicit actuator by @zoctipus in https://github.com/isaac-sim/IsaacLab/pull/1580 -* Fixes action reset of ``pre_trained_policy_action`` in navigation environment by @nicolaloi in https://github.com/isaac-sim/IsaacLab/pull/1623 -* Fixes rigid object's root com velocities timestamp check by @ori-gadot in https://github.com/isaac-sim/IsaacLab/pull/1674 -* Adds interval resampling on event manager's reset call by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1750 -* Corrects calculation of target height adjustment based on sensor data by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1710 -* Fixes infinite loop in ``repeated_objects_terrain`` method by @nicolaloi in https://github.com/isaac-sim/IsaacLab/pull/1612 -* Fixes issue where the indices were not created correctly for articulation setters by @AntoineRichard in https://github.com/isaac-sim/IsaacLab/pull/1660 - -New Contributors -~~~~~~~~~~~~~~~~ - -* @T-K-233 made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1587 -* @steple made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1616 -* @Rishi-V made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1596 -* @nicolaloi made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1623 -* @mschweig made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1608 -* @AntoineRichard made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1660 -* @ori-gadot made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1674 -* @garylvov made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1717 +* Fixes JointAction not preserving order when using all joints by @T-K-233 +* Fixes event term for pushing root by setting velocity by @Mayankm96 +* Fixes error in Articulation where ``default_joint_stiffness`` and ``default_joint_damping`` are not correctly set for implicit actuator by @zoctipus +* Fixes action reset of ``pre_trained_policy_action`` in navigation environment by @nicolaloi +* Fixes rigid object's root com velocities timestamp check by @ori-gadot +* Adds interval resampling on event manager's reset call by @Mayankm96 +* Corrects calculation of target height adjustment based on sensor data by @fan-ziqi +* Fixes infinite loop in ``repeated_objects_terrain`` method by @nicolaloi +* Fixes issue where the indices were not created correctly for articulation setters by @AntoineRichard v1.4.0 @@ -475,55 +1516,46 @@ compatibility, but will come with many great fixes and improvements. New Features ------------ -* Adds Factory contact-rich manipulation tasks to IsaacLab by @noseworm in https://github.com/isaac-sim/IsaacLab/pull/1520 -* Adds a Franka stacking ManagerBasedRLEnv by @peterd-NV in https://github.com/isaac-sim/IsaacLab/pull/1494 -* Adds recorder manager in manager-based environments by @nvcyc in https://github.com/isaac-sim/IsaacLab/pull/1336 -* Adds Ray Workflow: Multiple Run Support, Distributed Hyperparameter Tuning, and Consistent Setup Across Local/Cloud by @glvov-bdai in https://github.com/isaac-sim/IsaacLab/pull/1301 -* Adds ``OperationSpaceController`` to docs and tests and implement corresponding action/action_cfg classes by @ozhanozen in https://github.com/isaac-sim/IsaacLab/pull/913 -* Adds null-space control option within ``OperationSpaceController`` by @ozhanozen in https://github.com/isaac-sim/IsaacLab/pull/1557 -* Adds observation term history support to Observation Manager by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/1439 -* Adds live plots to managers by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/893 +* Adds Factory contact-rich manipulation tasks to IsaacLab by @noseworm +* Adds a Franka stacking ManagerBasedRLEnv by @peterd-NV +* Adds recorder manager in manager-based environments by @nvcyc +* Adds Ray Workflow: Multiple Run Support, Distributed Hyperparameter Tuning, and Consistent Setup Across Local/Cloud by @glvov-bdai +* Adds ``OperationSpaceController`` to docs and tests and implement corresponding action/action_cfg classes by @ozhanozen +* Adds null-space control option within ``OperationSpaceController`` by @ozhanozen +* Adds observation term history support to Observation Manager by @jtigue-bdai +* Adds live plots to managers by @pascal-roth Improvements ------------ -* Adds documentation and example scripts for sensors by @mpgussert in https://github.com/isaac-sim/IsaacLab/pull/1443 -* Removes duplicated ``TerminationsCfg`` code in G1 and H1 RoughEnvCfg by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1484 -* Adds option to change the clipping behavior for all Cameras and unifies the default by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/891 -* Adds check that no articulation root API is applied on rigid bodies by @lgulich in https://github.com/isaac-sim/IsaacLab/pull/1358 -* Adds RayCaster rough terrain base height to reward by @Andy-xiong6 in https://github.com/isaac-sim/IsaacLab/pull/1525 -* Adds position threshold check for state transitions by @DorsaRoh in https://github.com/isaac-sim/IsaacLab/pull/1544 -* Adds clip range for JointAction by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1476 +* Adds documentation and example scripts for sensors by @mpgussert +* Removes duplicated ``TerminationsCfg`` code in G1 and H1 RoughEnvCfg by @fan-ziqi +* Adds option to change the clipping behavior for all Cameras and unifies the default by @pascal-roth +* Adds check that no articulation root API is applied on rigid bodies by @lgulich +* Adds RayCaster rough terrain base height to reward by @Andy-xiong6 +* Adds position threshold check for state transitions by @DorsaRoh +* Adds clip range for JointAction by @fan-ziqi Bug Fixes --------- -* Fixes noise_model initialized in direct_marl_env by @NoneJou072 in https://github.com/isaac-sim/IsaacLab/pull/1480 -* Fixes entry_point and kwargs in isaaclab_tasks README by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1485 -* Fixes syntax for checking if pre-commit is installed in isaaclab.sh by @louislelay in https://github.com/isaac-sim/IsaacLab/pull/1422 -* Corrects fisheye camera projection types in spawner configuration by @command-z-z in https://github.com/isaac-sim/IsaacLab/pull/1361 -* Fixes actuator velocity limits propagation down the articulation root_physx_view by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/1509 -* Computes Jacobian in the root frame inside the ``DifferentialInverseKinematicsAction`` class by @zoctipus in https://github.com/isaac-sim/IsaacLab/pull/967 -* Adds transform for mesh_prim of ray caster sensor by @clearsky-mio in https://github.com/isaac-sim/IsaacLab/pull/1448 -* Fixes configclass dict conversion for torch tensors by @lgulich in https://github.com/isaac-sim/IsaacLab/pull/1530 -* Fixes error in apply_actions method in ``NonHolonomicAction`` action term. by @KyleM73 in https://github.com/isaac-sim/IsaacLab/pull/1513 -* Fixes outdated sensor data after reset by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1276 -* Fixes order of logging metrics and sampling commands in command manager by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1352 +* Fixes noise_model initialized in direct_marl_env by @NoneJou072 +* Fixes entry_point and kwargs in isaaclab_tasks README by @fan-ziqi +* Fixes syntax for checking if pre-commit is installed in isaaclab.sh by @louislelay +* Corrects fisheye camera projection types in spawner configuration by @command-z-z +* Fixes actuator velocity limits propagation down the articulation root_physx_view by @jtigue-bdai +* Computes Jacobian in the root frame inside the ``DifferentialInverseKinematicsAction`` class by @zoctipus +* Adds transform for mesh_prim of ray caster sensor by @clearsky-mio +* Fixes configclass dict conversion for torch tensors by @lgulich +* Fixes error in apply_actions method in ``NonHolonomicAction`` action term. by @KyleM73 +* Fixes outdated sensor data after reset by @kellyguo11 +* Fixes order of logging metrics and sampling commands in command manager by @Mayankm96 Breaking Changes ---------------- -* Refactors pose and velocities to link frame and COM frame APIs by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/966 - -New Contributors ----------------- +* Refactors pose and velocities to link frame and COM frame APIs by @jtigue-bdai -* @nvcyc made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1336 -* @peterd-NV made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1494 -* @NoneJou072 made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1480 -* @clearsky-mio made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1448 -* @Andy-xiong6 made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1525 -* @noseworm made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1520 v1.3.0 ====== @@ -541,85 +1573,66 @@ gymnasium spaces, manager-based perception environments, and more. New Features ------------ -* Adds ``IMU`` sensor by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/619 -* Add Camera Benchmark Tool and Allow Correct Unprojection of distance_to_camera depth image by @glvov-bdai in https://github.com/isaac-sim/IsaacLab/pull/976 -* Creates Manager Based Cartpole Vision Example Environments by @glvov-bdai in https://github.com/isaac-sim/IsaacLab/pull/995 -* Adds image extracted features observation term and cartpole examples for it by @glvov-bdai in https://github.com/isaac-sim/IsaacLab/pull/1191 -* Supports other gymnasium spaces in Direct workflow by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/1117 -* Adds configuration classes for spawning different assets at prim paths by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1164 -* Adds a rigid body collection class by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/1288 -* Adds option to scale/translate/rotate meshes in the ``mesh_converter`` by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/1228 -* Adds event term to randomize gains of explicit actuators by @MoreTore in https://github.com/isaac-sim/IsaacLab/pull/1005 -* Adds Isaac Lab Reference Architecture documentation by @OOmotuyi in https://github.com/isaac-sim/IsaacLab/pull/1371 +* Adds ``IMU`` sensor by @pascal-roth +* Add Camera Benchmark Tool and Allow Correct Unprojection of distance_to_camera depth image by @glvov-bdai +* Creates Manager Based Cartpole Vision Example Environments by @glvov-bdai +* Adds image extracted features observation term and cartpole examples for it by @glvov-bdai +* Supports other gymnasium spaces in Direct workflow by @Toni-SM +* Adds configuration classes for spawning different assets at prim paths by @Mayankm96 +* Adds a rigid body collection class by @Dhoeller19 +* Adds option to scale/translate/rotate meshes in the ``mesh_converter`` by @pascal-roth +* Adds event term to randomize gains of explicit actuators by @MoreTore +* Adds Isaac Lab Reference Architecture documentation by @OOmotuyi Improvements ------------ -* Expands functionality of FrameTransformer to allow multi-body transforms by @jsmith-bdai in https://github.com/isaac-sim/IsaacLab/pull/858 -* Inverts SE-2 keyboard device actions (Z, X) for yaw command by @riccardorancan in https://github.com/isaac-sim/IsaacLab/pull/1030 -* Disables backward pass compilation of warp kernels by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1222 -* Replaces TensorDict with native dictionary by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/1348 -* Improves omni.isaac.lab_tasks loading time by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/1353 -* Caches PhysX view's joint paths when processing fixed articulation tendons by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/1347 -* Replaces hardcoded module paths with ``__name__`` dunder by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1357 -* Expands observation term scaling to support list of floats by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/1269 -* Removes extension startup messages from the Simulation App by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1217 -* Adds a render config to the simulation and tiledCamera limitations to the docs by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1246 -* Adds Kit command line argument support by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1293 -* Modifies workflow scripts to generate random seed when seed=-1 by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1048 -* Adds benchmark script to measure robot loading by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1195 -* Switches from ``carb`` to ``omni.log`` for logging by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1215 -* Excludes cache files from vscode explorer by @Divelix in https://github.com/isaac-sim/IsaacLab/pull/1131 -* Adds versioning to the docs by @sheikh-nv in https://github.com/isaac-sim/IsaacLab/pull/1247 -* Adds better error message for invalid actuator parameters by @lgulich in https://github.com/isaac-sim/IsaacLab/pull/1235 -* Updates tested docker and apptainer versions for cluster deployment by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/1230 -* Removes ``ml_archive`` as a dependency of ``omni.isaac.lab`` extension by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1266 -* Adds a validity check for configclasses by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/1214 -* Ensures mesh name is compatible with USD convention in mesh converter by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/1302 -* Adds sanity check for the term type inside the command manager by @command-z-z in https://github.com/isaac-sim/IsaacLab/pull/1315 -* Allows configclass ``to_dict`` operation to handle a list of configclasses by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/1227 +* Expands functionality of FrameTransformer to allow multi-body transforms by @jsmith-bdai +* Inverts SE-2 keyboard device actions (Z, X) for yaw command by @riccardorancan +* Disables backward pass compilation of warp kernels by @Mayankm96 +* Replaces TensorDict with native dictionary by @Toni-SM +* Improves omni.isaac.lab_tasks loading time by @Toni-SM +* Caches PhysX view's joint paths when processing fixed articulation tendons by @Toni-SM +* Replaces hardcoded module paths with ``__name__`` dunder by @Mayankm96 +* Expands observation term scaling to support list of floats by @pascal-roth +* Removes extension startup messages from the Simulation App by @Mayankm96 +* Adds a render config to the simulation and tiledCamera limitations to the docs by @kellyguo11 +* Adds Kit command line argument support by @kellyguo11 +* Modifies workflow scripts to generate random seed when seed=-1 by @kellyguo11 +* Adds benchmark script to measure robot loading by @Mayankm96 +* Switches from ``carb`` to ``omni.log`` for logging by @Mayankm96 +* Excludes cache files from vscode explorer by @Divelix +* Adds versioning to the docs by @sheikh-nv +* Adds better error message for invalid actuator parameters by @lgulich +* Updates tested docker and apptainer versions for cluster deployment by @pascal-roth +* Removes ``ml_archive`` as a dependency of ``omni.isaac.lab`` extension by @fan-ziqi +* Adds a validity check for configclasses by @Dhoeller19 +* Ensures mesh name is compatible with USD convention in mesh converter by @fan-ziqi +* Adds sanity check for the term type inside the command manager by @command-z-z +* Allows configclass ``to_dict`` operation to handle a list of configclasses by @jtigue-bdai Bug Fixes --------- -* Disables replicate physics for deformable teddy lift environment by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1026 -* Fixes Jacobian joint indices for floating base articulations by @lorenwel in https://github.com/isaac-sim/IsaacLab/pull/1033 -* Fixes setting the seed from CLI for RSL-RL by @kaixi287 in https://github.com/isaac-sim/IsaacLab/pull/1084 -* Fixes camera MDP term name and reprojection docstrings by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1130 -* Fixes deprecation notice for using ``pxr.Semantics`` by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1129 -* Fixes scaling of default ground plane by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1133 -* Fixes Isaac Sim executable on pip installation by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/1172 -* Passes device from CLI args to simulation config in standalone scripts by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/1114 -* Fixes the event for randomizing rigid body material by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/1140 -* Fixes the ray_caster_camera tutorial script when saving the data by @mpgussert in https://github.com/isaac-sim/IsaacLab/pull/1198 -* Fixes running the docker container when the DISPLAY env variable is not defined by @GiulioRomualdi in https://github.com/isaac-sim/IsaacLab/pull/1163 -* Fixes default joint pos when setting joint limits by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/1040 -* Fixes device propagation for noise and adds noise tests by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/1175 -* Removes additional sbatch and fixes default profile in cluster deployment by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/1229 -* Fixes the checkpoint loading error in RSL-RL training script by @bearpaw in https://github.com/isaac-sim/IsaacLab/pull/1210 -* Fixes pytorch broadcasting issue in ``EMAJointPositionToLimitsAction`` by @bearpaw in https://github.com/isaac-sim/IsaacLab/pull/1207 -* Fixes body IDs selection when computing ``feet_slide`` reward for locomotion-velocity task by @dtc103 in https://github.com/isaac-sim/IsaacLab/pull/1277 -* Fixes broken URLs in markdown files by @DorsaRoh in https://github.com/isaac-sim/IsaacLab/pull/1272 -* Fixes ``net_arch`` in ``sb3_ppo_cfg.yaml`` for Isaac-Lift-Cube-Franka-v0 task by @LinghengMeng in https://github.com/isaac-sim/IsaacLab/pull/1249 - -New Contributors ----------------- - -* @riccardorancan made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1030 -* @glvov-bdai made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/976 -* @kaixi287 made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1084 -* @lgulich made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1119 -* @nv-apoddubny made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1118 -* @GiulioRomualdi made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1163 -* @Divelix made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1131 -* @sheikh-nv made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1247 -* @dtc103 made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1277 -* @DorsaRoh made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1272 -* @louislelay made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1271 -* @LinghengMeng made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1249 -* @OOmotuyi made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1337 -* @command-z-z made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1315 -* @MoreTore made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/1005 +* Disables replicate physics for deformable teddy lift environment by @Mayankm96 +* Fixes Jacobian joint indices for floating base articulations by @lorenwel +* Fixes setting the seed from CLI for RSL-RL by @kaixi287 +* Fixes camera MDP term name and reprojection docstrings by @Mayankm96 +* Fixes deprecation notice for using ``pxr.Semantics`` by @Mayankm96 +* Fixes scaling of default ground plane by @kellyguo11 +* Fixes Isaac Sim executable on pip installation by @Toni-SM +* Passes device from CLI args to simulation config in standalone scripts by @Mayankm96 +* Fixes the event for randomizing rigid body material by @pascal-roth +* Fixes the ray_caster_camera tutorial script when saving the data by @mpgussert +* Fixes running the docker container when the DISPLAY env variable is not defined by @GiulioRomualdi +* Fixes default joint pos when setting joint limits by @kellyguo11 +* Fixes device propagation for noise and adds noise tests by @jtigue-bdai +* Removes additional sbatch and fixes default profile in cluster deployment by @pascal-roth +* Fixes the checkpoint loading error in RSL-RL training script by @bearpaw +* Fixes pytorch broadcasting issue in ``EMAJointPositionToLimitsAction`` by @bearpaw +* Fixes body IDs selection when computing ``feet_slide`` reward for locomotion-velocity task by @dtc103 +* Fixes broken URLs in markdown files by @DorsaRoh +* Fixes ``net_arch`` in ``sb3_ppo_cfg.yaml`` for Isaac-Lift-Cube-Franka-v0 task by @LinghengMeng v1.2.0 @@ -643,67 +1656,69 @@ New Features * Adds the direct workflow perceptive Shadowhand Cube Repose environment ``Isaac-Repose-Cube-Shadow-Vision-Direct-v0`` by @kellyguo11. * Adds support for multi-agent environments with the Direct workflow, with support for MAPPO and IPPO in SKRL by @Toni-SM * Adds the direct workflow multi-agent environments ``Isaac-Cart-Double-Pendulum-Direct-v0`` and ``Isaac-Shadow-Hand-Over-Direct-v0`` by @Toni-SM -* Adds throughput benchmarking scripts for the different learning workflows by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/759 +* Adds throughput benchmarking scripts for the different learning workflows by @kellyguo11 * Adds results for the benchmarks in the documentation `here `__ for different types of hardware by @kellyguo11 -* Adds the direct workflow Allegro hand environment by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/709 -* Adds video recording to the play scripts in RL workflows by @j3soon in https://github.com/isaac-sim/IsaacLab/pull/763 +* Adds the direct workflow Allegro hand environment by @kellyguo11 +* Adds video recording to the play scripts in RL workflows by @j3soon * Adds comparison tables for the supported RL libraries `here `__ by @kellyguo11 -* Add APIs for deformable asset by @masoudmoghani in https://github.com/isaac-sim/IsaacLab/pull/630 -* Adds support for MJCF converter by @qqqwan in https://github.com/isaac-sim/IsaacLab/pull/957 -* Adds a function to define camera configs through intrinsic matrix by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/617 -* Adds configurable modifiers to observation manager by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/830 -* Adds the Hydra configuration system for RL training by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/700 +* Add APIs for deformable asset by @masoudmoghani +* Adds support for MJCF converter by @qqqwan +* Adds a function to define camera configs through intrinsic matrix by @pascal-roth +* Adds configurable modifiers to observation manager by @jtigue-bdai +* Adds the Hydra configuration system for RL training by @Dhoeller19 Improvements ------------ -* Uses PhysX accelerations for rigid body acceleration data by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/760 -* Adds documentation on the frames for asset data by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/742 -* Renames Unitree configs in locomotion tasks to match properly by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/714 -* Adds option to set the height of the border in the ``TerrainGenerator`` by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/744 -* Adds a cli arg to ``run_all_tests.py`` for testing a selected extension by @jsmith-bdai in https://github.com/isaac-sim/IsaacLab/pull/753 -* Decouples rigid object and articulation asset classes by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/644 -* Adds performance optimizations for domain randomization by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/494 -* Allows having hybrid dimensional terms inside an observation group by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/772 -* Adds a flag to preserve joint order inside ``JointActionCfg`` action term by @xav-nal in https://github.com/isaac-sim/IsaacLab/pull/787 -* Adds the ability to resume training from a checkpoint with rl_games by @sizsJEon in https://github.com/isaac-sim/IsaacLab/pull/797 -* Adds windows configuration to VS code tasks by @johnBuffer in https://github.com/isaac-sim/IsaacLab/pull/963 -* Adapts A and D button bindings in the keyboard device by @zoctipus in https://github.com/isaac-sim/IsaacLab/pull/910 -* Uses ``torch.einsum`` for quat_rotate and quat_rotate_inverse operations by @dxyy1 in https://github.com/isaac-sim/IsaacLab/pull/900 -* Expands on articulation test for multiple instances and devices by @jsmith-bdai in https://github.com/isaac-sim/IsaacLab/pull/872 -* Adds setting of environment seed at initialization by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/940 -* Disables default viewport when headless but cameras are enabled by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/851 -* Simplifies the return type for ``parse_env_cfg`` method by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/965 -* Simplifies the if-elses inside the event manager apply method by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/948 +* Uses PhysX accelerations for rigid body acceleration data by @Mayankm96 +* Adds documentation on the frames for asset data by @Mayankm96 +* Renames Unitree configs in locomotion tasks to match properly by @Mayankm96 +* Adds option to set the height of the border in the ``TerrainGenerator`` by @pascal-roth +* Adds a cli arg to ``run_all_tests.py`` for testing a selected extension by @jsmith-bdai +* Decouples rigid object and articulation asset classes by @Mayankm96 +* Adds performance optimizations for domain randomization by @kellyguo11 +* Allows having hybrid dimensional terms inside an observation group by @Mayankm96 +* Adds a flag to preserve joint order inside ``JointActionCfg`` action term by @xav-nal +* Adds the ability to resume training from a checkpoint with rl_games by @sizsJEon +* Adds windows configuration to VS code tasks by @johnBuffer +* Adapts A and D button bindings in the keyboard device by @zoctipus +* Uses ``torch.einsum`` for quat_rotate and quat_rotate_inverse operations by @dxyy1 +* Expands on articulation test for multiple instances and devices by @jsmith-bdai +* Adds setting of environment seed at initialization by @Mayankm96 +* Disables default viewport when headless but cameras are enabled by @kellyguo11 +* Simplifies the return type for ``parse_env_cfg`` method by @Mayankm96 +* Simplifies the if-elses inside the event manager apply method by @Mayankm96 Bug Fixes --------- -* Fixes rendering frame delays. Rendered images now faithfully represent the latest state of the physics scene. We added the flag - ``rerender_on_reset`` in the environment configs to toggle an additional render step when a reset happens. When activated, the images/observation always represent the latest state of the environment, but this also reduces performance. -* Fixes ``wrap_to_pi`` function in math utilities by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/771 -* Fixes setting of pose when spawning a mesh by @masoudmoghani in https://github.com/isaac-sim/IsaacLab/pull/692 -* Fixes caching of the terrain using the terrain generator by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/757 -* Fixes running train scripts when rsl_rl is not installed by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/784, https://github.com/isaac-sim/IsaacLab/pull/789 -* Adds flag to recompute inertia when randomizing the mass of a rigid body by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/989 -* Fixes support for ``classmethod`` when defining a configclass by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/901 -* Fixes ``Sb3VecEnvWrapper`` to clear buffer on reset by @EricJin2002 in https://github.com/isaac-sim/IsaacLab/pull/974 -* Fixes venv and conda pip installation on windows by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/970 -* Sets native livestream extensions to Isaac Sim 4.1-4.0 defaults by @jtigue-bdai in https://github.com/isaac-sim/IsaacLab/pull/954 -* Defaults the gym video recorder fps to match episode decimation by @ozhanozen in https://github.com/isaac-sim/IsaacLab/pull/894 -* Fixes the event manager's apply method by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/936 -* Updates camera docs with world units and introduces new test for intrinsics by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/886 -* Adds the ability to resume training from a checkpoint with rl_games by @sizsJEon in https://github.com/isaac-sim/IsaacLab/pull/797 +* Fixes rendering frame delays. Rendered images now faithfully represent the latest state of the physics scene. + We added the flag ``rerender_on_reset`` in the environment configs to toggle an additional render step when a + reset happens. When activated, the images/observation always represent the latest state of the environment, but + this also reduces performance. +* Fixes ``wrap_to_pi`` function in math utilities by @Mayankm96 +* Fixes setting of pose when spawning a mesh by @masoudmoghani +* Fixes caching of the terrain using the terrain generator by @Mayankm96 +* Fixes running train scripts when rsl_rl is not installed by @Dhoeller19 +* Adds flag to recompute inertia when randomizing the mass of a rigid body by @Mayankm96 +* Fixes support for ``classmethod`` when defining a configclass by @Mayankm96 +* Fixes ``Sb3VecEnvWrapper`` to clear buffer on reset by @EricJin2002 +* Fixes venv and conda pip installation on windows by @kellyguo11 +* Sets native livestream extensions to Isaac Sim 4.1-4.0 defaults by @jtigue-bdai +* Defaults the gym video recorder fps to match episode decimation by @ozhanozen +* Fixes the event manager's apply method by @kellyguo11 +* Updates camera docs with world units and introduces new test for intrinsics by @pascal-roth +* Adds the ability to resume training from a checkpoint with rl_games by @sizsJEon Breaking Changes ---------------- -* Simplifies device setting in SimulationCfg and AppLauncher by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/696 -* Fixes conflict in teleop-device command line argument in scripts by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/791 -* Converts container.sh into Python utilities by @hhansen-bdai in https://github.com/isaac-sim/IsaacLab/commit/f565c33d7716db1be813b30ddbcf9321712fc497 +* Simplifies device setting in SimulationCfg and AppLauncher by @Dhoeller19 +* Fixes conflict in teleop-device command line argument in scripts by @Dhoeller19 +* Converts container.sh into Python utilities by @hhansen-bdai * Drops support for ``TiledCamera`` for Isaac Sim 4.1 Migration Guide @@ -712,7 +1727,10 @@ Migration Guide Setting the simulation device into the simulation context ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Previously, changing the simulation device to CPU required users to set other simulation parameters (such as disabling GPU physics and GPU pipelines). This made setting up the device appear complex. We now simplify the checks for device directly inside the simulation context, so users only need to specify the device through the configuration object. +Previously, changing the simulation device to CPU required users to set other simulation parameters +(such as disabling GPU physics and GPU pipelines). This made setting up the device appear complex. +We now simplify the checks for device directly inside the simulation context, so users only need to +specify the device through the configuration object. Before: @@ -729,7 +1747,10 @@ Now: Setting the simulation device from CLI ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Previously, users could specify the device through the command line argument ``--device_id``. However, this made it ambiguous when users wanted to set the device to CPU. Thus, instead of the device ID, users need to specify the device explicitly through the argument ``--device``. The valid options for the device name are: +Previously, users could specify the device through the command line argument ``--device_id``. However, +this made it ambiguous when users wanted to set the device to CPU. Thus, instead of the device ID, +users need to specify the device explicitly through the argument ``--device``. +The valid options for the device name are: * "cpu": runs simulation on CPU * "cuda": runs simulation on GPU with device ID at default index @@ -752,7 +1773,9 @@ Now: Renaming of teleoperation device CLI in standalone scripts ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Since ``--device`` is now an argument provided by the AppLauncher, it conflicted with the command-line argument used for specifying the teleoperation-device in some of the standalone scripts. Thus, to fix this conflict, the teleoperation-device now needs to be specified through ``--teleop_device`` argument. +Since ``--device`` is now an argument provided by the AppLauncher, it conflicted with the command-line +argument used for specifying the teleoperation-device in some of the standalone scripts. Thus, to fix +this conflict, the teleoperation-device now needs to be specified through ``--teleop_device`` argument. Before: @@ -770,9 +1793,14 @@ Now: Using Python-version of container utility script ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -The prior `container.sh `_ became quite complex as it had many different use cases in one script. For instance, building a docker image for "base" or "ros2", as well as cluster deployment. As more users wanted to have the flexibility to overlay their own docker settings, maintaining this bash script became cumbersome. Hence, we migrated its features into a Python script in this release. Additionally, we split the cluster-related utilities into their own script inside the ``docker/cluster`` directory. +The prior `container.sh `_ became quite +complex as it had many different use cases in one script. For instance, building a docker image for "base" or "ros2", +as well as cluster deployment. As more users wanted to have the flexibility to overlay their own docker settings, +maintaining this bash script became cumbersome. Hence, we migrated its features into a Python script in this release. +Additionally, we split the cluster-related utilities into their own script inside the ``docker/cluster`` directory. -We still maintain backward compatibility for ``container.sh``. Internally, it calls the Python script ``container.py``. We request users to use the Python script directly. +We still maintain backward compatibility for ``container.sh``. Internally, it calls the Python script ``container.py``. +We request users to use the Python script directly. Before: @@ -791,31 +1819,22 @@ Now: Using separate directories for logging videos in RL workflows ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Previously, users could record videos during the RL training by specifying the ``--video`` flag to the ``train.py`` script. The videos would be saved inside the ``videos`` directory in the corresponding log directory of the run. +Previously, users could record videos during the RL training by specifying the ``--video`` flag to the +``train.py`` script. The videos would be saved inside the ``videos`` directory in the corresponding log +directory of the run. -Since many users requested to also be able to record videos while inferencing the policy, recording videos have also been added to the ``play.py`` script. Since changing the prefix of the video file names is not possible, the videos from the train and play scripts are saved inside the ``videos/train`` and ``videos/play`` directories, respectively. +Since many users requested to also be able to record videos while inferencing the policy, recording +videos have also been added to the ``play.py`` script. Since changing the prefix of the video file +names is not possible, the videos from the train and play scripts are saved inside the ``videos/train`` +and ``videos/play`` directories, respectively. Drops support for the tiled camera with Isaac Sim 4.1 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Various fixes have been made to the tiled camera rendering pipeline in Isaac Sim 4.2. This made supporting the tiled camera with Isaac Sim 4.1 difficult. Hence, for the best experience, we advice switching to Isaac Sim 4.2 with this release of Isaac Lab. - -New Contributors ----------------- +Various fixes have been made to the tiled camera rendering pipeline in Isaac Sim 4.2. This made +supporting the tiled camera with Isaac Sim 4.1 difficult. Hence, for the best experience, we advice +switching to Isaac Sim 4.2 with this release of Isaac Lab. -* @xav-nal made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/787 -* @sizsJEon made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/797 -* @jtigue-bdai made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/830 -* @StrainFlow made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/835 -* @mpgussert made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/827 -* @Symars made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/898 -* @martinmatak made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/876 -* @bearpaw made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/945 -* @dxyy1 made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/900 -* @qqqwan made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/957 -* @johnBuffer made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/963 -* @EricJin2002 made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/974 -* @iamnambiar made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/986 v1.1.0 ====== @@ -836,41 +1855,41 @@ all our contributors for their continued support. New Features ------------ -* Adds distributed multi-GPU learning support for skrl by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/574 -* Updates skrl integration to support training/evaluation using JAX by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/592 -* Adds lidar pattern for raycaster sensor by @pascal-roth in https://github.com/isaac-sim/IsaacLab/pull/616 -* Adds support for PBS job scheduler-based clusters by @shafeef901 in https://github.com/isaac-sim/IsaacLab/pull/605 -* Adds APIs for spawning deformable meshes by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/613 +* Adds distributed multi-GPU learning support for skrl by @Toni-SM +* Updates skrl integration to support training/evaluation using JAX by @Toni-SM +* Adds lidar pattern for raycaster sensor by @pascal-roth +* Adds support for PBS job scheduler-based clusters by @shafeef901 +* Adds APIs for spawning deformable meshes by @Mayankm96 Improvements ------------ -* Changes documentation color to the green theme by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/585 -* Fixes sphinx tabs to make them work in dark theme by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/584 -* Fixes VSCode settings to work with pip installation of Isaac Sim by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/628 -* Fixes ``isaaclab`` scripts to deal with Isaac Sim pip installation by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/631 -* Optimizes interactive scene for homogeneous cloning by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/636 -* Improves docker X11 forwarding documentation by @j3soon in https://github.com/isaac-sim/IsaacLab/pull/685 +* Changes documentation color to the green theme by @Mayankm96 +* Fixes sphinx tabs to make them work in dark theme by @Mayankm96 +* Fixes VSCode settings to work with pip installation of Isaac Sim by @Mayankm96 +* Fixes ``isaaclab`` scripts to deal with Isaac Sim pip installation by @Mayankm96 +* Optimizes interactive scene for homogeneous cloning by @kellyguo11 +* Improves docker X11 forwarding documentation by @j3soon Bug Fixes --------- -* Reads gravity direction from simulation inside ``RigidObjectData`` by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/582 -* Fixes reference count in asset instances due to circular references by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/580 -* Fixes issue with asset deinitialization due to torch > 2.1 by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/640 -* Fixes the rendering logic regression in environments by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/614 -* Fixes the check for action-space inside Stable-Baselines3 wrapper by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/610 -* Fixes warning message in Articulation config processing by @locoxsoco in https://github.com/isaac-sim/IsaacLab/pull/699 -* Fixes action term in the reach environment by @masoudmoghani in https://github.com/isaac-sim/IsaacLab/pull/710 -* Fixes training UR10 reach with RL_GAMES and SKRL by @sudhirpratapyadav in https://github.com/isaac-sim/IsaacLab/pull/678 -* Adds event manager call to simple manage-based env by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/666 +* Reads gravity direction from simulation inside ``RigidObjectData`` by @Mayankm96 +* Fixes reference count in asset instances due to circular references by @Mayankm96 +* Fixes issue with asset deinitialization due to torch > 2.1 by @Mayankm96 +* Fixes the rendering logic regression in environments by @Dhoeller19 +* Fixes the check for action-space inside Stable-Baselines3 wrapper by @Mayankm96 +* Fixes warning message in Articulation config processing by @locoxsoco +* Fixes action term in the reach environment by @masoudmoghani +* Fixes training UR10 reach with RL_GAMES and SKRL by @sudhirpratapyadav +* Adds event manager call to simple manage-based env by @Mayankm96 Breaking Changes ---------------- * Drops official support for Isaac Sim 2023.1.1 -* Removes the use of body view inside the asset classes by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/643 -* Renames ``SimulationCfg.substeps`` to ``SimulationCfg.render_interval`` by @Dhoeller19 in https://github.com/isaac-sim/IsaacLab/pull/515 +* Removes the use of body view inside the asset classes by @Mayankm96 +* Renames ``SimulationCfg.substeps`` to ``SimulationCfg.render_interval`` by @Dhoeller19 Migration Guide --------------- @@ -878,7 +1897,10 @@ Migration Guide Renaming of ``SimulationCfg.substeps`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Previously, the users set both ``omni.isaac.lab.sim.SimulationCfg.dt`` and ``omni.isaac.lab.sim.SimulationCfg.substeps``, which marked the physics insulation time-step and sub-steps, respectively. It was unclear whether sub-steps meant the number of integration steps inside the physics time-step ``dt`` or the number of physics steps inside a rendering step. +Previously, the users set both ``omni.isaac.lab.sim.SimulationCfg.dt`` and +``omni.isaac.lab.sim.SimulationCfg.substeps``, which marked the physics insulation time-step and sub-steps, +respectively. It was unclear whether sub-steps meant the number of integration steps inside the physics time-step +``dt`` or the number of physics steps inside a rendering step. Since in the code base, the attribute was used as the latter, it has been renamed to ``render_interval`` for clarity. @@ -909,7 +1931,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 @@ -922,11 +1944,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 sensor implementation. This provides optimized RGB-D rendering throughputs of up to 10k frames per second. +* 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 @@ -938,7 +1961,8 @@ Improvements ------------ * Reduced start-up time for scripts (inherited from Isaac Sim 4.0 improvements) -* Added lazy buffer implementation for rigid object and articulation data. Instead of updating all the quantities at every step call, the lazy buffers are updated only when the user queries them +* Added lazy buffer implementation for rigid object and articulation data. Instead of updating all the quantities + at every step call, the lazy buffers are updated only when the user queries them * Added SKRL support to more environments Breaking Changes @@ -954,12 +1978,4 @@ Please find detailed migration guides as follows: * `From Orbit to IsaacLab `_ * `From OmniIsaacGymEnvs to IsaacLab `_ -New Contributors ----------------- - -* @abizovnuralem made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/452 -* @eltociear made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/460 -* @zoctipus made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/486 -* @JunghwanRo made their first contribution in https://github.com/isaac-sim/IsaacLab/pull/497 - .. _simple script: https://gist.github.com/kellyguo11/3e8f73f739b1c013b1069ad372277a85 diff --git a/docs/source/refs/snippets/code_skeleton.py b/docs/source/refs/snippets/code_skeleton.py new file mode 100644 index 000000000000..759ca38ae0f9 --- /dev/null +++ b/docs/source/refs/snippets/code_skeleton.py @@ -0,0 +1,152 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from typing import ClassVar + +DEFAULT_TIMEOUT: int = 30 +"""Default timeout for the task.""" + +_MAX_RETRIES: int = 3 # private constant (note the underscore) +"""Maximum number of retries for the task.""" + + +def run_task(task_name: str): + """Run a task by name. + + Args: + task_name: The name of the task to run. + """ + print(f"Running task: {task_name}") + + +class TaskRunner: + """Runs and manages tasks.""" + + DEFAULT_NAME: ClassVar[str] = "runner" + """Default name for the runner.""" + + _registry: ClassVar[dict] = {} + """Registry of runners.""" + + def __init__(self, name: str): + """Initialize the runner. + + Args: + name: The name of the runner. + """ + self.name = name + self._tasks = [] # private instance variable + + def __del__(self): + """Clean up the runner.""" + print(f"Cleaning up {self.name}") + + def __repr__(self) -> str: + return f"TaskRunner(name={self.name!r})" + + def __str__(self) -> str: + return f"TaskRunner: {self.name}" + + """ + Properties. + """ + + @property + def task_count(self) -> int: + return len(self._tasks) + + """ + Operations. + """ + + def initialize(self): + """Initialize the runner.""" + print("Initializing runner...") + + def update(self, task: str): + """Update the runner with a new task. + + Args: + task: The task to add. + """ + self._tasks.append(task) + print(f"Added task: {task}") + + def close(self): + """Close the runner.""" + print("Closing runner...") + + """ + Operations: Registration. + """ + + @classmethod + def register(cls, name: str, runner: "TaskRunner"): + """Register a runner. + + Args: + name: The name of the runner. + runner: The runner to register. + """ + if name in cls._registry: + _log_error(f"Runner {name} already registered. Skipping registration.") + return + cls._registry[name] = runner + + @staticmethod + def validate_task(task: str) -> bool: + """Validate a task. + + Args: + task: The task to validate. + + Returns: + True if the task is valid, False otherwise. + """ + return bool(task and task.strip()) + + """ + Internal operations. + """ + + def _reset(self): + """Reset the runner.""" + self._tasks.clear() + + @classmethod + def _get_registry(cls) -> dict: + """Get the registry.""" + return cls._registry + + @staticmethod + def _internal_helper(): + """Internal helper.""" + print("Internal helper called.") + + +""" +Helper operations. +""" + + +def _log_error(message: str): + """Internal helper to log errors. + + Args: + message: The message to log. + """ + print(f"[ERROR] {message}") + + +class _TaskHelper: + """Private utility class for internal task logic.""" + + def compute(self) -> int: + """Compute the result. + + Returns: + The result of the computation. + """ + return 42 diff --git a/docs/source/refs/snippets/tutorial_modify_direct_rl_env.py b/docs/source/refs/snippets/tutorial_modify_direct_rl_env.py index 48e8e5912e21..e839abf20099 100644 --- a/docs/source/refs/snippets/tutorial_modify_direct_rl_env.py +++ b/docs/source/refs/snippets/tutorial_modify_direct_rl_env.py @@ -1,8 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +# ruff: noqa # fmt: off # [start-init-import] diff --git a/docs/source/refs/troubleshooting.rst b/docs/source/refs/troubleshooting.rst index ea6ac86882c6..8f3a82f3f150 100644 --- a/docs/source/refs/troubleshooting.rst +++ b/docs/source/refs/troubleshooting.rst @@ -75,34 +75,8 @@ For instance, to run a standalone script with verbose logging, you can use the f # Run the standalone script with info logging ./isaaclab.sh -p scripts/tutorials/00_sim/create_empty.py --headless --info -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 +For more fine-grained control, you can modify the logging channels through the ``logger`` module. +For more information, please refer to its `documentation `__. Observing long load times at the start of the simulation diff --git a/docs/source/setup/ecosystem.rst b/docs/source/setup/ecosystem.rst index 77b59be1b14b..5978443ac21c 100644 --- a/docs/source/setup/ecosystem.rst +++ b/docs/source/setup/ecosystem.rst @@ -83,15 +83,46 @@ be the single robot learning framework for Isaac Sim. Previously released framew and we encourage users to follow our migration guides to transition over to Isaac Lab. +Is Isaac Lab a simulator? +------------------------- + +Often, when people think of simulators, they think of various commonly available engines, such as +`MuJoCo`_, `Bullet`_, and `Flex`_. These engines are powerful and have been used in a number of +research projects. However, they are not designed to be a general purpose simulator for robotics. +Rather they are primarily physics engines that are used to simulate the dynamics of rigid and +deformable bodies. They are shipped with some basic rendering capabilities to visualize the +simulation and provide parsing capabilities of different scene description formats. + +Various recent works combine these physics engines with different rendering engines to provide +a more complete simulation environment. They include APIs that allow reading and writing to the +physics and rendering engines. In some cases, they support ROS and hardware-in-the-loop simulation +for more robotic-specific applications. An example of these include `AirSim`_, `DoorGym`_, `ManiSkill`_, +`ThreeDWorld`_ and lastly, `Isaac Sim`_. + +At its core, Isaac Lab is **not** a robotics simulator, but a framework for building robot learning +applications on top of Isaac Sim. An equivalent example of such a framework is `RoboSuite`_, which +is built on top of `MuJoCo`_ and is specific to fixed-base robots. Other examples include +`MuJoCo Playground`_ and `Isaac Gym`_ which use `MJX`_ and `PhysX`_ respectively. They +include a number of pre-built tasks with separated out stand-alone implementations for individual +tasks. While this is a good starting point (and often convenient), a lot of code +repetition occurs across different task implementations, which can reduce code-reuse for larger +projects and teams. + +The main goal of Isaac Lab is to provide a unified framework for robot learning that includes +a variety of tooling and features that are required for robot learning, while being easy to +use and extend. It includes design patterns that simplify many of the common requirements for +robotics research. These include simulating sensors at different frequencies, connecting to different +teleoperation interfaces for data collection, switching action spaces for policy learning, +using Hydra for configuration management, supporting different learning libraries and more. +Isaac Lab supports designing tasks using *manager-based (modularized)* and *direct (single-script +similar to Isaac Gym)* patterns, leaving it up to the user to choose the best approach for their +use-case. For each of these patterns, Isaac Lab includes a number of pre-built tasks that can be +used for benchmarking and research. + + Why should I use Isaac Lab? --------------------------- -Since Isaac Sim remains closed-sourced, it is difficult for users to contribute to the simulator and build a -common framework for research. On its current path, we see the community using the simulator will simply -develop their own frameworks that will result in scattered efforts with a lot of duplication of work. -This has happened in the past with other simulators, and we believe that it is not the best way to move -forward as a community. - Isaac Lab provides an open-sourced platform for the community to drive progress with consolidated efforts toward designing benchmarks and robot learning systems as a joint initiative. This allows us to reuse existing components and algorithms, and to build on top of each other's work. Doing so not only saves @@ -113,3 +144,13 @@ to Isaac Lab, please reach out to us. .. _OmniIsaacGymEnvs: https://github.com/isaac-sim/OmniIsaacGymEnvs .. _Orbit: https://isaac-orbit.github.io/ .. _Isaac Automator: https://github.com/isaac-sim/IsaacAutomator +.. _AirSim: https://microsoft.github.io/AirSim/ +.. _DoorGym: https://github.com/PSVL/DoorGym/ +.. _ManiSkill: https://github.com/haosulab/ManiSkill +.. _ThreeDWorld: https://www.threedworld.org/ +.. _RoboSuite: https://robosuite.ai/ +.. _MuJoCo: https://mujoco.org/ +.. _MuJoCo Playground: https://playground.mujoco.org/ +.. _MJX: https://mujoco.readthedocs.io/en/stable/mjx.html +.. _Bullet: https://github.com/bulletphysics/bullet3 +.. _Flex: https://developer.nvidia.com/flex diff --git a/docs/source/setup/installation/asset_caching.rst b/docs/source/setup/installation/asset_caching.rst index 5ee0760b6818..5cee207fae36 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 4fe835c4b763..82754d6871e3 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,27 +26,12 @@ To check the minimum system requirements, refer to the documentation .. tab-item:: :icon:`fa-brands fa-linux` Linux :sync: linux - .. note:: - - We have tested Isaac Lab with Isaac Sim 4.5 release on Ubuntu - 22.04 LTS with NVIDIA driver 535.129. - - 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 ``${HOME}/isaacsim`` directory. - - 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:: - - We have tested Isaac Lab with Isaac Sim 4.5 release on Windows11 with NVIDIA driver 552.86. - - 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 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -74,428 +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 - - -.. 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 unittest 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 unittest 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``. - -.. attention:: +.. include:: include/src_clone_isaaclab.rst - For 50 series GPUs, please use the latest PyTorch nightly build instead of PyTorch 2.5.1, which comes with Isaac Sim: - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh -p -m pip install --upgrade --pre torch --index-url https://download.pytorch.org/whl/nightly/cu128 - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - isaaclab.bat -p -m pip install --upgrade --pre torch --index-url https://download.pytorch.org/whl/nightly/cu128 - -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:| - -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 +.. include:: include/src_symlink_isaacsim.rst - 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 3208b04b1522..c555377bb6c0 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`_. -* 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.2 + + .. tab-item:: Azure + :sync: azure + + .. code-block:: bash -.. code-block:: bash + ./deploy-azure --isaaclab v2.3.2 - # 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.2 + + .. tab-item:: Alibaba Cloud + :sync: alicloud + + .. code-block:: bash + + ./deploy-alicloud --isaaclab v2.3.2 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 000000000000..19da95e16236 --- /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 000000000000..822df92e4d6e --- /dev/null +++ b/docs/source/setup/installation/include/pip_python_virtual_env.rst @@ -0,0 +1,131 @@ +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:: 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 + + .. tab-item:: UV Environment (experimental) + + To install ``uv``, please follow the instructions `here `__. + + .. note:: + + A virtual environment created by ``uv venv`` does **not** include ``pip``. + Since Isaac Lab installation requires ``pip``, please install it manually + after activating the environment. + + 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 and pip + uv venv --python 3.11 --seed 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 and pip + uv venv --python 3.11 --seed 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 000000000000..111b47d271bb --- /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 000000000000..ba822ae7b2c5 --- /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 000000000000..844cac2f3fd1 --- /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 000000000000..6e995f2f0e89 --- /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:: 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" + + .. tab-item:: UV Environment (experimental) + + 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. + +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 000000000000..be8ae17cdbd2 --- /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 000000000000..a747a1ccdc35 --- /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 47d30a976896..725f31f9bdcc 100644 --- a/docs/source/setup/installation/index.rst +++ b/docs/source/setup/installation/index.rst @@ -1,62 +1,192 @@ +.. _isaaclab-installation-root: + Local Installation ================== -.. image:: https://img.shields.io/badge/IsaacSim-4.5.0-silver.svg +.. image:: https://img.shields.io/badge/IsaacSim-5.1.0-silver.svg :target: https://developer.nvidia.com/isaac-sim - :alt: IsaacSim 4.5.0 + :alt: IsaacSim 5.1.0 -.. image:: https://img.shields.io/badge/python-3.10-blue.svg +.. image:: https://img.shields.io/badge/python-3.11-blue.svg :target: https://www.python.org/downloads/release/python-31013/ - :alt: Python 3.10 + :alt: Python 3.11 .. image:: https://img.shields.io/badge/platform-linux--64-orange.svg - :target: https://releases.ubuntu.com/20.04/ - :alt: Ubuntu 20.04 + :target: https://releases.ubuntu.com/22.04/ + :alt: Ubuntu 22.04 .. image:: https://img.shields.io/badge/platform-windows--64-orange.svg :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 4.5.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 the full list of system requirements for Isaac Sim, please refer to the - `Isaac Sim system requirements `_. +System Requirements +------------------- +General Requirements +~~~~~~~~~~~~~~~~~~~~ -Isaac Lab is built on top of the Isaac Sim platform. Therefore, it is required to first install Isaac Sim -before using Isaac Lab. +For detailed requirements, please see the +`Isaac Sim system requirements `_. +The basic requirements are: -Both Isaac Sim and Isaac Lab provide two ways of installation: -either through binary download/source file, or through Python's package installer ``pip``. +- **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) -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. +**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: -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. +- For Isaac Sim 5.X, the required Python version is 3.11. +- For Isaac Sim 4.X, the required Python version is 3.10. -For Ubuntu 22.04 and Windows systems, we recommend using Isaac Sim pip installation. -For Ubuntu 20.04 systems, we recommend installing Isaac Sim through binaries. -For users getting started with Isaac Lab, we recommend installing Isaac Lab by cloning the repo. +Driver Requirements +~~~~~~~~~~~~~~~~~~~ +Drivers other than those recommended on `Omniverse Technical Requirements `_ +may work but have not been validated against all Omniverse tests. -.. toctree:: - :maxdepth: 2 +- 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 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +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. + +#. :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: - Pip installation (recommended for Ubuntu 22.04 and Windows) - Binary installation (recommended for Ubuntu 20.04) - Advanced installation (Isaac Lab pip) - Asset caching ++-------------------+------------------------------+------------------------------+---------------------------+------------+ +| 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 | ++-------------------+------------------------------+------------------------------+---------------------------+------------+ + +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 9c52f50130cf..bddc2a3bfaa4 100644 --- a/docs/source/setup/installation/isaaclab_pip_installation.rst +++ b/docs/source/setup/installation/isaaclab_pip_installation.rst @@ -1,162 +1,116 @@ -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 -that are built on top of Isaac Lab. Isaac Lab pip packages **do not** include any standalone python scripts for +that are built on top of Isaac Lab. Isaac Lab pip packages **does not** include any standalone python scripts for training, inferencing, or running standalone workflows such as demos and examples. Therefore, users are required -to define your own runner scripts when installing Isaac Lab from pip. +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 `_. + 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. -- 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.10**. +.. include:: include/pip_python_virtual_env.rst - .. tab-set:: - - .. tab-item:: conda environment - - .. code-block:: bash - - conda create -n env_isaaclab python=3.10 - conda activate env_isaaclab +Installing dependencies +~~~~~~~~~~~~~~~~~~~~~~~ - .. tab-item:: venv environment - - .. 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.10 - python3.10 -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.10 - python3.10 -m venv env_isaaclab - # activate the virtual environment - env_isaaclab\Scripts\activate + .. code-block:: none + pip install isaaclab[isaacsim,all]==2.3.2 --extra-index-url https://pypi.nvidia.com -- Next, install a CUDA-enabled PyTorch 2.5.1 build based on the CUDA version available on your system. This step is optional for Linux, but required for Windows to ensure a CUDA-compatible version of PyTorch is installed. +- Install a CUDA-enabled PyTorch 2.7.0 build for CUDA 12.8 that matches your system architecture: .. tab-set:: + :sync-group: pip-platform - .. tab-item:: CUDA 11 + .. tab-item:: :icon:`fa-brands fa-linux` Linux (x86_64) + :sync: linux-x86_64 .. code-block:: bash - pip install torch==2.5.1 --index-url https://download.pytorch.org/whl/cu118 + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 - .. tab-item:: CUDA 12 + .. tab-item:: :icon:`fa-brands fa-windows` Windows (x86_64) + :sync: windows-x86_64 .. code-block:: bash - pip install torch==2.5.1 --index-url https://download.pytorch.org/whl/cu121 - - -- Before installing Isaac Lab, ensure the latest pip version is installed. To update pip, run - - .. tab-set:: - :sync-group: os + 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-linux` Linux - :sync: linux + .. tab-item:: :icon:`fa-brands fa-linux` Linux (aarch64) + :sync: linux-aarch64 .. code-block:: bash - pip install --upgrade pip + pip install -U torch==2.9.0 torchvision==0.24.0 --index-url https://download.pytorch.org/whl/cu130 - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code-block:: batch - - python -m pip install --upgrade pip - -- Then, install the Isaac Lab packages, this will also install Isaac Sim. - - .. code-block:: none + .. note:: - pip install isaaclab[isaacsim,all]==2.0.2 --extra-index-url https://pypi.nvidia.com + 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. - For 50 series GPUs, please use the latest PyTorch nightly build instead of PyTorch 2.5.1, which comes with Isaac Sim: + 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. - .. code:: bash + To fix this, unset any existing ``LD_PRELOAD`` and set it to use the system library only: - pip install --upgrade --pre torch --index-url https://download.pytorch.org/whl/nightly/cu128 + .. code-block:: bash + unset LD_PRELOAD + export LD_PRELOAD="$LD_PRELOAD:/lib/aarch64-linux-gnu/libgomp.so.1" -Verifying the Isaac Sim installation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + This ensures the correct ``libgomp`` library is preloaded for both Isaac Sim and Isaac Lab, + removing the preload warnings during runtime. -- Make sure that your virtual environment is activated (if applicable) +- If you want to use ``rl_games`` for training and inferencing, install + its Python 3.11 enabled fork: + .. code-block:: none -- 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 - - -.. attention:: - - 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. + pip install git+https://github.com/isaac-sim/rl_games.git@python3.11 -.. attention:: +.. include:: include/pip_verify_isaacsim.rst - 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: +Running Isaac Lab Scripts +~~~~~~~~~~~~~~~~~~~~~~~~~ - .. code:: bash +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 - 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 +.. code:: bash - Do you accept the EULA? (Yes/No): Yes + python my_awesome_script.py +Generating VS Code Settings +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -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 `__. +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 -Running Isaac Lab Scripts -~~~~~~~~~~~~~~~~~~~~~~~~~ + python -m isaaclab --generate-vscode-settings -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 +.. warning:: - python my_awesome_script.py + 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 f509ff6d8b5e..5a6a5a7956d3 100644 --- a/docs/source/setup/installation/pip_installation.rst +++ b/docs/source/setup/installation/pip_installation.rst @@ -1,406 +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.34+ version compatibility. + 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:: - - On Windows with CUDA 12, the GPU driver version 552.86 is required. - - Also, 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.10**. - - .. tab-set:: - - .. tab-item:: conda environment - - .. code-block:: bash +Installing Isaac Sim +-------------------- - conda create -n env_isaaclab python=3.10 - 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.10 - python3.10 -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.10 - python3.10 -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 -- Next, install a CUDA-enabled PyTorch 2.5.1 build based on the CUDA version available on your system. This step is optional for Linux, but required for Windows to ensure a CUDA-compatible version of PyTorch is installed. +- Install a CUDA-enabled PyTorch build that matches your system architecture: .. tab-set:: + :sync-group: pip-platform - .. tab-item:: CUDA 11 + .. tab-item:: :icon:`fa-brands fa-linux` Linux (x86_64) + :sync: linux-x86_64 .. code-block:: bash - pip install torch==2.5.1 --index-url https://download.pytorch.org/whl/cu118 + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 - .. tab-item:: CUDA 12 + .. tab-item:: :icon:`fa-brands fa-windows` Windows (x86_64) + :sync: windows-x86_64 .. code-block:: bash - pip install torch==2.5.1 --index-url https://download.pytorch.org/whl/cu121 + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 -- Before installing Isaac Sim, ensure the latest pip version is installed. To update pip, run - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux + .. tab-item:: :icon:`fa-brands fa-linux` Linux (aarch64) + :sync: linux-aarch64 .. 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 - -- Then, install the Isaac Sim packages. - - .. code-block:: none - - pip install 'isaacsim[all,extscache]==4.5.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 + pip install -U torch==2.9.0 torchvision==0.24.0 --index-url https://download.pytorch.org/whl/cu130 -- It's also possible to run with a specific experience file, run: + .. note:: - .. code:: bash + After installing Isaac Lab on aarch64, you may encounter warnings such as: - # experience files can be absolute path, or relative path searched in isaacsim/apps or omni/apps - isaacsim isaacsim.exp.full.kit + .. code-block:: none + ERROR: ld.so: object '...torch.libs/libgomp-XXXX.so.1.0.0' cannot be preloaded: ignored. -.. attention:: - - 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 Software License Agreement. - To accept the EULA, reply ``Yes`` when prompted with the below message: + 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. - .. code:: bash + To fix this, unset any existing ``LD_PRELOAD`` and set it to use the system library only: - 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 + .. code-block:: bash - 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 -`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 unittest 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 unittest 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``. - -.. attention:: - - For 50 series GPUs, please use the latest PyTorch nightly build instead of PyTorch 2.5.1, which comes with Isaac Sim: - - .. code:: bash - - pip install --upgrade --pre torch --index-url https://download.pytorch.org/whl/nightly/cu128 - -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 000000000000..c697c1dd2054 --- /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 new file mode 100644 index 000000000000..70b5c7a7c6ea --- /dev/null +++ b/docs/source/setup/quickstart.rst @@ -0,0 +1,397 @@ +.. _isaac-lab-quickstart: + +Quickstart Guide +======================= + + +This guide is written for those who just can't wait to get their hands dirty and will touch on the most common concepts you will encounter as you build your own +projects with Isaac Lab! This includes installation, running RL, finding environments, creating new projects, and more! + +The power of Isaac Lab comes from from a few key features that we will very briefly touch on in this guide. + +1) **Vectorization**: Reinforcement Learning requires attempting a task many times. Isaac Lab speeds this process along by vectorizing the + environment, a process by which training can be run in parallel across many copies of the same environment, thus reducing the amount of time + spent on collecting data before the weights of the model can be updated. Most of the codebase is devoted to defining those parts of the environment + that need to be touched by this vectorization system + +2) **Modular Design**: Isaac Lab is designed to be modular, meaning that you can design your projects to have various components that can be + swapped out for different needs. For example, suppose you want to train a policy that supports a specific subset of robots. You could design + the environment and task to be robot agnostic by writing a controller interface layer in the form of one of our Manager classes (the ``ActionManager`` + in this specific case). Most of the rest of the codebase is devoted to defining those parts of your project that need to be touched by this manager system. + +To get started, we will first install Isaac Lab and launch a training script. + +Quick Installation Guide +------------------------- + +There are many ways to :ref:`install ` Isaac Lab, but for the purposes of this quickstart guide, we will follow the +pip install route using virtual environments. + +To begin, we first define our virtual environment. + +.. tab-set:: + + .. tab-item:: conda + + .. code-block:: bash + + # create a virtual environment named env_isaaclab with python3.11 and pip + conda create -n env_isaaclab python=3.11 + # activate the virtual environment + conda activate env_isaaclab + + .. tab-item:: uv (experimental) + + .. 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 and pip + uv venv --python 3.11 --seed 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 + + +Next, install a CUDA-enabled PyTorch 2.7.0 build. + + .. code-block:: bash + + 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 + +.. 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 + +and now we can install the Isaac Sim packages. + +.. code-block:: none + + 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 + +.. 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 + +Installation is now as easy as navigating to the repo and then calling the root script with the ``--install`` 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" + + +Quick Start Using Isaac Launchable +---------------------------------- + +For users first learning Isaac Lab, without sufficient local compute resources, the `Isaac Launchable `_ project is a quick way to get started without manual installation. + +Through this project, users can interact with Isaac Sim and Isaac Lab purely from a web browser, with one tab running Visual Studio Code for development and command execution, and another tab providing the streamed user interface for Isaac Sim. + +This method uses `NVIDIA Brev `_, a platform that offers easily configurable pay-by-the-hour cloud compute. Brev Launchables are preconfigured, optimized compute and software environments. + +To try now, click the button below. To learn more about how to use this project, or how to create your own Launchable, please see the project repo `here `_. + +.. image:: https://brev-assets.s3.us-west-1.amazonaws.com/nv-lb-dark.svg + :target: https://brev.nvidia.com/launchable/deploy/now?launchableID=env-35JP2ywERLgqtD0b0MIeK1HnF46 + :alt: Click here to deploy + + +Launch Training +------------------- + +The various backends of Isaac Lab are accessed through their corresponding ``train.py`` and ``play.py`` scripts located in the ``isaaclab/scripts/reinforcement_learning`` directory. +Invoking these scripts will require a **Task Name** and a corresponding **Entry Point** to the gymnasium API. For example + +.. code-block:: bash + + python scripts/reinforcement_learning/skrl/train.py --task=Isaac-Ant-v0 + +This will train the mujoco ant to "run". You can see the various launch option available to you with the ``--help`` flag. Note specifically the ``--num_envs`` option and the ``--headless`` flag, +both of which can be useful when trying to develop and debug a new environment. Options specified at this level automatically overwrite any configuration equivalent that may be defined in the code +(so long as those definitions are part of a ``@configclass``, see below). + +List Available Environments +----------------------------- + +Above, ``Isaac-Ant-v0`` is the task name and ``skrl`` is the RL framework being used. The ``Isaac-Ant-v0`` environment +has been registered with the `Gymnasium API `_, and you can see how the entry point is defined +by calling the ``list_envs.py`` script, which can be found in ``isaaclab/scripts/environments/list_envs.py``. You should see entries like the following + +.. code-block:: bash + + $> python scripts/environments/list_envs.py + + +--------------------------------------------------------------------------------------------------------------------------------------------+ + | Available Environments in Isaac Lab + +--------+----------------------+--------------------------------------------+---------------------------------------------------------------+ + | S. No. | Task Name | Entry Point | Config + . + . + . + +--------+----------------------+--------------------------------------------+---------------------------------------------------------------+ + | 2 | Isaac-Ant-Direct-v0 | isaaclab_tasks.direct.ant.ant_env:AntEnv | isaaclab_tasks.direct.ant.ant_env:AntEnvCfg + +--------+----------------------+--------------------------------------------+---------------------------------------------------------------+ + . + . + . + +--------+----------------------+--------------------------------------------+---------------------------------------------------------------+ + | 48 | Isaac-Ant-v0 | isaaclab.envs:ManagerBasedRLEnv | isaaclab_tasks.manager_based.classic.ant.ant_env_cfg:AntEnvCfg + +--------+----------------------+--------------------------------------------+---------------------------------------------------------------+ + +Notice that there are two different ``Ant`` tasks, one for a ``Direct`` environment and one for a ``ManagerBased`` environment. +These are the :ref:`two primary workflows` that you can use with Isaac Lab out of the box. The Direct workflow will give you the +shortest path to a working custom environment for reinforcement learning, but the Manager based workflow will give your project the modularity required +for more generalized development. For the purposes of this quickstart guide, we will only focus on the Direct workflow. + + +Generate Your Own Project +-------------------------- + +Getting a new project started with Isaac Lab can seem daunting at first, but this is why we provide the :ref:`template +generator`, to rapidly boilerplate a new project via the command line. + +.. code-block:: bash + + ./isaaclab.sh --new + +This will create a new project for you based on the settings you choose + +* **External vs Internal**: Determines if the project is meant to be built as a part of the isaac lab repository, or if + it is meant to be loaded as an external extension. +* **Direct vs Manager**: A direct task primarily contains all the implementation details within the environment definition, + while a manager based project is meant to use our modular definitions for the different "parts" of an environment. +* **Framework**: You can select more than one option here. This determines which RL frameworks you intend to natively use with your project + (which specific algorithm implementations you want to use for training). + +Once created, navigate to the installed project and run + +.. code-block:: bash + + python -m pip install -e source/ + +to complete the installation process and register the environment. Within the directories created by the template +generator, you will find at least one ``__init__.py`` file with something that looks like the following + +.. code-block:: python + + import gymnasium as gym + + gym.register( + id="Template-isaaclabtutorial_env-v0", + entry_point=f"{__name__}.isaaclabtutorial_env:IsaaclabtutorialEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.isaaclabtutorial_env_cfg:IsaaclabtutorialEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}.skrl_ppo_cfg:PPORunnerCfg", + }, + ) + +This is the function that actually registers an environment for future use. Notice that the ``entry_point`` is literally +just the python module path to the environment definition. This is why we need to install the project as a package: the module path **is** the +entry point for the gymnasium API. + +Configurations +--------------- + +Regardless of what you are going to be doing with Isaac Lab, you will need to deal with **Configurations**. Configurations +can all be identified by the inclusion of the ``@configclass`` decorator above their class definition and the lack of an ``__init__`` function. For example, consider +this configuration class for the :ref:`cartpole environment `. + +.. code-block:: python + + @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 + +Notice that the entire class definition is just a list of value fields and other configurations. Configuration classes are +necessary for anything that needs to care about being vectorized by the lab during training. If you want to be able to copy an +environment thousands of times, and manage the data from each asynchronously, you need to somehow "label" what parts of the scene matter +to this copying process (vectorization). This is what the configuration classes accomplish! + +In this case, the class defines the configuration for the entire training environment! Notice also the ``num_envs`` variable in the ``InteractiveSceneCfg``. This actually gets overwritten +by the CLI argument from within the ``train.py`` script. Configurations provide a direct path to any variable in the configuration hierarchy, making it easy +to modify anything "configured" by the environment at launch time. + +Robots +------- + +Robots are entirely defined as instances of configurations within Isaac Lab. If you examine ``source/isaaclab_assets/isaaclab_assets/robots``, you will see a number of files, each of which +contains configurations for the robot in question. The purpose of these individual files is to better define scope for all the different robots, but there is nothing preventing +you from :ref:`adding your own ` to your project or even to the ``isaaclab`` repository! For example, consider the following configuration for +the Dofbot + +.. code-block:: python + + 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 + + DOFBOT_CONFIG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Dofbot/dofbot.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 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "joint1": 0.0, + "joint2": 0.0, + "joint3": 0.0, + "joint4": 0.0, + }, + pos=(0.25, -0.25, 0.0), + ), + actuators={ + "front_joints": ImplicitActuatorCfg( + joint_names_expr=["joint[1-2]"], + effort_limit_sim=100.0, + velocity_limit_sim=100.0, + stiffness=10000.0, + damping=100.0, + ), + "joint3_act": ImplicitActuatorCfg( + joint_names_expr=["joint3"], + effort_limit_sim=100.0, + velocity_limit_sim=100.0, + stiffness=10000.0, + damping=100.0, + ), + "joint4_act": ImplicitActuatorCfg( + joint_names_expr=["joint4"], + effort_limit_sim=100.0, + velocity_limit_sim=100.0, + stiffness=10000.0, + damping=100.0, + ), + }, + ) + +This completely defines the dofbot! You could copy this into a ``.py`` file and import it as a module and you would be able to use the dofbot in +your own lab sims. One common feature you will see in any config defining things with state is the presence of an ``InitialStateCfg``. Remember, the configurations +are what informs vectorization, and it's the ``InitialStateCfg`` that describes the state of the joints of our robot when it gets created in each environment. The +``ImplicitActuatorCfg`` defines the joints of the robot using the default actuation model determined by the joint time. Not all joints need to be actuated, but you +will get warnings if you don't. If you aren't planning on using those undefined joints, you can generally ignore these. + +Apps and Sims +-------------- + +Using the simulation means launching the Isaac Sim app to provide simulation context. If you are not running a task defined by the standard workflows, then you +are responsible for creating the app, managing the context, and stepping the simulation forward through time. This is the "third workflow": a **Standalone** app, which +is what we call the scripts for the frameworks, demos, benchmarks, etc... + +The Standalone workflow gives you total control over *everything* in the app and simulation +context. Developing standalone apps is discussed at length in the `Isaac Sim documentation `_ but there +are a few points worth touching on that can be incredibly useful. + +.. code-block:: python + + import argparse + + from isaaclab.app import AppLauncher + # add argparse arguments + parser = argparse.ArgumentParser( + description="This script demonstrates adding a custom robot to an Isaac Lab environment." + ) + parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.") + # 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 + +The ``AppLauncher`` is the entrypoint to any and all Isaac Sim applications, like Isaac Lab! *Many Isaac Lab and Isaac Sim modules +cannot be imported until the app is launched!*. This is done on the second to last line of the code above, when the ``AppLauncher`` is constructed. +The ``app_launcher.app`` is our interface to the Kit App Framework; the broader interstitial code that binds the simulation to things the extension +management system, or the GUI, etc... In the standalone workflow, this interface, often called the ``simulation_app`` is predominantly used +to check if the simulation is running, and cleanup after the simulation finishes. diff --git a/docs/source/setup/walkthrough/api_env_design.rst b/docs/source/setup/walkthrough/api_env_design.rst new file mode 100644 index 000000000000..07471ec2ea5a --- /dev/null +++ b/docs/source/setup/walkthrough/api_env_design.rst @@ -0,0 +1,158 @@ +.. _walkthrough_api_env_design: + +Classes and Configs +==================================== + +To begin, navigate to the task: ``source/isaac_lab_tutorial/isaac_lab_tutorial/tasks/direct/isaac_lab_tutorial``, and take a look +and the contents of ``isaac_lab_tutorial_env_cfg.py``. You should see something that looks like the following + +.. code-block:: python + + from isaaclab_assets.robots.cartpole import CARTPOLE_CFG + + from isaaclab.assets import ArticulationCfg + from isaaclab.envs import DirectRLEnvCfg + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.sim import SimulationCfg + from isaaclab.utils import configclass + + + @configclass + class IsaacLabTutorialEnvCfg(DirectRLEnvCfg): + + # Some useful fields + . + . + . + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=2) + + # robot(s) + robot_cfg: ArticulationCfg = CARTPOLE_CFG.replace(prim_path="/World/envs/env_.*/Robot") + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + + # Some more useful fields + . + . + . + +This is the default configuration for a simple cartpole environment that comes with the template and defines the ``self`` scope +for anything you do within the corresponding environment. + +.. currentmodule:: isaaclab.envs + +The first thing to note is the presence of the ``@configclass`` decorator. This defines a class as a configuration class, which holds +a special place in Isaac Lab. Configuration classes are part of how Isaac Lab determines what to "care" about when it comes to cloning +the environment to scale up training. Isaac Lab provides different base configuration classes depending on your goals, and in this +case we are using the :class:`DirectRLEnvCfg` class because we are interested in performing reinforcement learning in the direct workflow. + +.. currentmodule:: isaaclab.sim + +The second thing to note is the content of the configuration class. As the author, you can specify any fields you desire but, generally speaking, there are three things you +will always define here: The **sim**, the **scene**, and the **robot**. Notice that these fields are also configuration classes! Configuration classes +are compositional in this way as a solution for cloning arbitrarily complex environments. + +The **sim** is an instance of :class:`SimulationCfg`, and this is the config that controls the nature of the simulated reality we are building. This field is a member +of the base class, ``DirecRLEnvCfg``, but has a default sim configuration, so it's *technically* optional. The ``SimulationCfg`` dictates +how finely to step through time (dt), the direction of gravity, and even how physics should be simulated. In this case we only specify the time step and the render interval, with the +former indicating that each step through time should simulate :math:`1/120` th of a second, and the latter being how many steps we should take before we render a frame (a value of 2 means +render every other frame). + +.. currentmodule:: isaaclab.scene + +The **scene** is an instance of :class:`InteractiveSceneCfg`. The scene describes what goes "on the stage" and manages those simulation entities to be cloned across environments. +The scene is also a member of the base class ``DirectRLEnvCfg``, but unlike the sim it has no default and must be defined in every ``DirectRLEnvCfg``. The ``InteractiveSceneCfg`` +describes how many copies of the scene we want to create for training purposes, as well as how far apart they should be spaced on the stage. + +.. currentmodule:: isaaclab.assets + +Finally we have the **robot** definition, which is an instance of :class:`ArticulationCfg`. An environment could have multiple articulations, and so the presence of +an ``ArticulationCfg`` is not strictly required in order to define a ``DirectRLEnv``. Instead, the usual workflow is to define a regex path to the robot, and replace +the ``prim_path`` attribute in the base configuration. In this case, ``CARTPOLE_CFG`` is a configuration defined in ``isaaclab_assets.robots.cartpole`` and by replacing +the prim path with ``/World/envs/env_.*/Robot`` we are implicitly saying that every copy of the scene will have a robot named ``Robot``. + + +The Environment +----------------- + +Next, let's take a look at the contents of the other python file in our task directory: ``isaac_lab_tutorial_env.py`` + +.. code-block:: python + + # imports + . + . + . + from .isaac_lab_tutorial_env_cfg import IsaacLabTutorialEnvCfg + + class IsaacLabTutorialEnv(DirectRLEnv): + cfg: IsaacLabTutorialEnvCfg + + def __init__(self, cfg: IsaacLabTutorialEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + . . . + + def _setup_scene(self): + self.robot = Articulation(self.cfg.robot_cfg) + # add ground plane + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + # add articulation to scene + self.scene.articulations["robot"] = self.robot + # 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) -> None: + . . . + + def _apply_action(self) -> None: + . . . + + def _get_observations(self) -> dict: + . . . + + def _get_rewards(self) -> torch.Tensor: + total_reward = compute_rewards(...) + return total_reward + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + . . . + + def _reset_idx(self, env_ids: Sequence[int] | None): + . . . + + @torch.jit.script + def compute_rewards(...): + . . . + return total_reward + + +.. currentmodule:: isaaclab.envs + +Some of the code has been omitted for clarity, in order to aid in discussion. This is where the actual "meat" of the +direct workflow exists and where most of our modifications will take place as we tweak the template to suit our needs. +Currently, all of the member functions of ``IsaacLabTutorialEnv`` are directly inherited from the :class:`DirectRLEnv`. This +known interface is how Isaac Lab and its supported RL frameworks interact with the environment. + +When the environment is initialized, it receives its own config as an argument, which is then immediately passed to super in order +to initialize the ``DirectRLEnv``. This super call also calls ``_setup_scene``, which actually constructs the scene and clones +it appropriately. Notably is how the robot is created and registered to the scene in ``_setup_scene``. First, the robot articulation +is created by using the ``robot_config`` we defined in ``IsaacLabTutorialEnvCfg``: it doesn't exist before this point! When the +articulation is created, the robot exists on the stage at ``/World/envs/env_0/Robot``. The call to ``scene.clone_environments`` then +copies ``env_0`` appropriately. At this point the robot exists as many copies on the stage, so all that's left is to notify the ``scene`` +object of the existence of this articulation to be tracked. The articulations of the scene are kept as a dictionary, so ``scene.articulations["robot"] = self.robot`` +creates a new ``robot`` element of the ``articulations`` dictionary and sets the value to be ``self.robot``. + +Notice also that the remaining functions do not take additional arguments except ``_reset_idx``. This is because the environment only manages the application of +actions to the agent being simulated, and then updating the sim. This is what the ``_pre_physics_step`` and ``_apply_action`` steps are for: we set the drive commands +to the robot so that when the simulation steps forward, the actions are applied and the joints are driven to new targets. This process is broken into steps like this +in order to ensure systematic control over how the environment is executed, and is especially important in the manager workflow. A similar relationship exists between the +``_get_dones`` function and ``_reset_idx``. The former, ``_get_dones`` determines if each of the environments is in a terminal state, and populates tensors of boolean +values to indicate which environments terminated due to entering a terminal state vs time out (the two returned tensors of the function). The latter, ``_reset_idx`` takes a +list environment index values (integers) and then actually resets those environments. It is important that things like updating drive targets or resetting environments +do not happen **during** the physics or rendering steps, and breaking up the interface in this way helps prevent that. diff --git a/docs/source/setup/walkthrough/concepts_env_design.rst b/docs/source/setup/walkthrough/concepts_env_design.rst new file mode 100644 index 000000000000..d446820a1472 --- /dev/null +++ b/docs/source/setup/walkthrough/concepts_env_design.rst @@ -0,0 +1,70 @@ +.. _walkthrough_concepts_env_design: + +Environment Design Background +============================== + +Now that we have our project installed, we can start designing the environment. In the traditional description +of a reinforcement learning (RL) problem, the environment is responsible for using the actions produced by the agent to +update the state of the "world", and finally compute and return the observations and the reward signal. However, there are +some additional concepts that are unique to Isaac Sim and Lab regarding the mechanics of the simulation itself. +The traditional description of a reinforcement learning problem presumes a "world", but we get no such luxury; we must define +the world ourselves, and success depends on understanding on how to construct that world and how it will fit into the simulation. + +App, Sim, World, Stage, and Scene +---------------------------------- + +.. figure:: ../../_static/setup/walkthrough_sim_stage_scene.svg + :align: center + :figwidth: 100% + :alt: How the sim is organized. + +The **World** is defined by the origin of a cartesian coordinate system and the units that define it. How big or how small? How +near or how far? The answers to questions like these can only be defined *relative* to some contextual reference frame, and that +reference frame is what defines the world. + +"Above" the world in structure is the **Sim**\ ulation and the **App**\ lication. The **Application** is "the thing responsible for +everything else": It governs all resource management as well as launching and destroying the simulation when we are done with it. +When we :ref:`launched training with the template`, the window that appears with the viewport of cartpoles +training is the Application window. The application is not defined by the GUI however, and even when running in headless mode all +simulations have an application that governs them. + +The **Simulation** controls the "rules" of the world. It defines the laws of physics, such as how time and gravity should work, and how frequently to perform +rendering. If the application holds the sim, then the sim holds the world. The simulation governs a single step through time by dividing it into many different +sub-steps, each devoted to a specific aspect of updating the world into a state. Many of the APIs in Isaac Lab are written to specifically hook into +these various steps and you will often see functions named like ``_pre_XYZ_step`` and ``_post_XYZ_step`` where ``XYZ_step`` is the name of one of these sub-steps of +the simulation, such as the ``physics_step`` or the ``render_step``. + +"Below" the world in structure is the **Stage** and the **Scene**. If the world provides spatial context to the sim, then +the **Stage** provides the *compositional context* for the world. Suppose we want to simulate a table set for a meal in a room: +the room is the "world" in this case, and we choose the origin of the world to be one of the corners of the room. The position of the +table in the room is defined as a vector from the origin of the world to some point on the table that we choose to be the origin of a *new* coordinate +system, fixed to the table. It's not useful to us, *the agent*\ , to talk about the location of the food and the utensils on the table with respect to the +corner of the room: instead it is preferable to use the coordinates defined with respect to the table. However, the simulation needs to know +these global coordinates in order to properly simulate the next time step, so we must define how these two coordinate systems are *composed* together. + +This is what the stage accomplishes: everything in the simulation is a `USD primitive `_ and the +stage represents the relationships between these primitives as a tree, with the context being defined by the relative path in the tree. Every prim on the stage +has a name and therefore a path in this tree, such as ``/room/table/food`` or ``room/table/utensils``. Relationships are defined by the "parents" and "children" +of a given node in this tree: the ``table`` is a child of the ``room`` but a parent of ``food``. Compositional properties of the parent are applied to all of its +children, but child prims have the ability to override parent properties if necessary, as is often the case for materials. + +.. figure:: ../../_static/setup/walkthrough_stage_context.svg + :align: center + :figwidth: 100% + :alt: How the stage organizes context + +Armed with this vocabulary, we can finally talk about the **Scene**, one of the most critical elements to understand about Isaac Lab. Deep learning, in +all its forms, is rooted in the analysis of data. This is true even in robot learning, where data is acquired through the sensors of the robot being trained. +The time required to setup the robot, collect data, and reset the robot to collect more, is a fundamental bottleneck in teaching robots to do *anything*, with any method. +Isaac Sim gives us access to robots without the need for literal physical robots, but Isaac Lab gives us access to *vectorization*: the ability to simulate many copies +of a training procedure efficiently, thus multiplying the rate of data generation and accelerating training proportionally. The scene governs those primitives on the stage +that matter to this vectorization process, known as **simulation entities**. + +Suppose the reason why you want to simulate a table set for a meal is because you would like to train a robot to place the table settings for you! The robot, the table, +and all the things on it can be registered to the scene of an environment. We can then specify how many copies we want and the scene will automatically +construct and run those copies on the stage. These copies are placed at new coordinates on the stage, defining a new reference frame from which observations +and rewards can be computed. Every copy of the scene exists on the stage and is being simulated by the same world. This is much more efficient +than running unique simulations for each copy, but it does open up the possibility of unwanted interactions between copies of the scene, so it's important +to keep this in mind while debugging. + +Now that we have a grasp on the mechanics, we can take a look at the code generated for our template project! diff --git a/docs/source/setup/walkthrough/index.rst b/docs/source/setup/walkthrough/index.rst new file mode 100644 index 000000000000..2ba226625583 --- /dev/null +++ b/docs/source/setup/walkthrough/index.rst @@ -0,0 +1,24 @@ +.. _walkthrough: + +Walkthrough +======================== + +So you finished installing Isaac Sim and Isaac Lab, and you verified that everything is working as expected... + +Now what? + +The following walkthrough will guide you through setting up an Isaac Lab extension project, adding a new robot to lab, designing an environment, and training a policy for that robot. +For this walkthrough, we will be starting with the Jetbot, a simple two wheeled differential base robot with a camera mounted on top, but the intent is for these guides to be general enough that you can use them to add your own robots and environments to Isaac Lab! + +The end result of this walkthrough can be found in our tutorial project repository `here `_. Each branch of this repository +represents a different stage of modifying the default template project to achieve our goals. + +.. toctree:: + :maxdepth: 1 + :titlesonly: + + concepts_env_design + api_env_design + technical_env_design + training_jetbot_gt + training_jetbot_reward_exploration diff --git a/docs/source/setup/walkthrough/technical_env_design.rst b/docs/source/setup/walkthrough/technical_env_design.rst new file mode 100644 index 000000000000..982a579f6831 --- /dev/null +++ b/docs/source/setup/walkthrough/technical_env_design.rst @@ -0,0 +1,204 @@ +.. _walkthrough_technical_env_design: + +Environment Design +==================== + +Armed with our understanding of the project and its structure, we are ready to start modifying the code to suit our Jetbot training needs. +Our template is set up for the **direct** workflow, which means the environment class will manage all of these details +centrally. We will need to write the code that will... + +#. Define the robot +#. Define the training simulation and manage cloning +#. Apply the actions from the agent to the robot +#. Calculate and return the rewards and observations +#. Manage resetting and terminal states + +As a first step, our goal will be to get the environment training pipeline to load and run. We will use a dummy reward signal +for the purposes of this part of the walkthrough. You can find the code for these modifications `here `_! + +Define the Robot +------------------ + +As our project grows, we may have many robots that we want to train. With malice aforethought we will add a new ``module`` to our +tutorial ``extension`` named ``robots`` where we will keep the definitions for robots as individual python scripts. Navigate +to ``isaac_lab_tutorial/source/isaac_lab_tutorial/isaac_lab_tutorial`` and create a new folder called ``robots``. Within this folder +create two files: ``__init__.py`` and ``jetbot.py``. The ``__init__.py`` file marks this directory as a python module and we will +be able to import the contents of ``jetbot.py`` in the usual way. + +The contents of ``jetbot.py`` is fairly minimal + +.. code-block:: python + + import isaaclab.sim as sim_utils + from isaaclab.assets import ArticulationCfg + from isaaclab.actuators import ImplicitActuatorCfg + from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + + JETBOT_CONFIG = ArticulationCfg( + 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)}, + ) + +The only purpose of this file is to define a unique scope in which to save our configurations. The details of robot configurations +can be explored in :ref:`this tutorial ` but most noteworthy for this walkthrough is the ``usd_path`` for the ``spawn`` +argument of this ``ArticulationCfg``. The Jetbot asset is available to the public via a hosted nucleus server, and that path is defined by +``ISAAC_NUCLEUS_DIR``, however any path to a USD file is valid, including local ones! + +Environment Configuration +--------------------------- + +Navigate to the environment configuration, ``isaac_lab_tutorial/source/isaac_lab_tutorial/isaac_lab_tutorial/tasks/direct/isaac_lab_tutorial/isaac_lab_tutorial_env_cfg.py``, and +replace its contents with the following + +.. code-block:: python + + from isaac_lab_tutorial.robots.jetbot import JETBOT_CONFIG + + from isaaclab.assets import ArticulationCfg + from isaaclab.envs import DirectRLEnvCfg + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.sim import SimulationCfg + from isaaclab.utils import configclass + + @configclass + class IsaacLabTutorialEnvCfg(DirectRLEnvCfg): + # env + decimation = 2 + episode_length_s = 5.0 + # - spaces definition + action_space = 2 + observation_space = 3 + state_space = 0 + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + # robot(s) + robot_cfg: ArticulationCfg = JETBOT_CONFIG.replace(prim_path="/World/envs/env_.*/Robot") + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=100, env_spacing=4.0, replicate_physics=True) + dof_names = ["left_wheel_joint", "right_wheel_joint"] + +Here we have, effectively, the same environment configuration as before, but with the Jetbot instead of the cartpole. The +parameters ``decimation``, ``episode_length_s``, ``action_space``, ``observation_space``, and ``state_space`` are members of +the base class, ``DirectRLEnvCfg``, and must be defined for every ``DirectRLEnv``. The space parameters are interpreted as vectors of +the given integer dimension, but they can also be defined as `gymnasium spaces `_! + +Notice the difference in the action and observation spaces. As the designers of the environment, we get to choose these. For the Jetbot, we want to +directly control the joints of the robot, of which only two are actuated (hence the action space of two). The observation space is *chosen* to be +3 because we are just going to feed the agent the linear velocity of the Jetbot, for now. We will change these later as we develop the environment. Our policy isn't going +to need an internal state maintained, so our state space is zero. + +Attack of the clones +--------------------- + +With the config defined, it's time to fill in the details of the environment, starting with the initialization and setup. +Navigate to the environment definition, ``isaac_lab_tutorial/source/isaac_lab_tutorial/isaac_lab_tutorial/tasks/direct/isaac_lab_tutorial/isaac_lab_tutorial_env.py``, and +replace the contents of the ``__init__`` and ``_setup_scene`` methods with the following. + +.. code-block:: python + + class IsaacLabTutorialEnv(DirectRLEnv): + cfg: IsaacLabTutorialEnvCfg + + def __init__(self, cfg: IsaacLabTutorialEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + self.dof_idx, _ = self.robot.find_joints(self.cfg.dof_names) + + 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) + +Notice that the ``_setup_scene`` method doesn't change and the ``_init__`` method is simply grabbing the joint indices from the robot (remember, setup is called in super). + +The next thing our environment needs is the definitions for how to handle actions, observations, and rewards. First, replace the contents of ``_pre_physics_step`` and +``_apply_action`` with the following. + +.. code-block:: python + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions.clone() + + def _apply_action(self) -> None: + self.robot.set_joint_velocity_target(self.actions, joint_ids=self.dof_idx) + +Here the act of applying actions to the robot in the environment is broken into two steps: ``_pre_physics_step`` and ``_apply_action``. The physics +simulation is decimated with respect to querying the policy for actions, meaning that multiple physics steps may occur per action taken by the policy. +The ``_pre_physics_step`` method is called just before this simulation step takes place and lets us detach the process of getting data from the +policy being trained and applying updates to the physics simulation. The ``_apply_action`` method is where those actions are actually applied to the robots +on the stage, after which the simulation is actually stepped forward in time. + +Next is the observations and rewards, which is just going to depend on the linear velocity of the Jetbot in the body frame of the robot. Replace the contents of ``_get_observations`` +and ``_get_rewards`` with the following. + +.. code-block:: python + + def _get_observations(self) -> dict: + self.velocity = self.robot.data.root_com_lin_vel_b + observations = {"policy": self.velocity} + return observations + + def _get_rewards(self) -> torch.Tensor: + total_reward = torch.linalg.norm(self.velocity, dim=-1, keepdim=True) + return total_reward + +The robot exists as an Articulation object within the Isaac Lab API. That object carries a data class, the ``ArticulationData``, which contains all the data for **specific** robots on the stage. +When we talk about a scene entity like the robot, we can either be talking about the robot broadly, as an entity that exists in every scene, or we can be describing a specific, singular clone +of the robot on the stage. The ``ArticulationData`` contains the data for those individual clones. This includes things like various kinematic vectors (like ``root_com_lin_vel_b``) and reference +vectors (like ``robot.data.FORWARD_VEC_B``). + +Notice how in the ``_apply_action`` method, we are calling a method of ``self.robot`` which is a method of ``Articulation``. The actions being applied are in the form of a 2D tensor +of shape ``[num_envs, num_actions]``. We are applying actions to **all** robots on the stage at once! Here, when we need to get the observations, we need the body frame velocity for all robots on the +stage, and so access ``self.robot.data`` to get that information. The ``root_com_lin_vel_b`` is a property of the ``ArticulationData`` that handles the conversion of the center-of-mass linear velocity from the world frame +to the body frame for us. Finally, Isaac Lab expects the observations to be returned as a dictionary, with ``policy`` defining those observations for the policy model and ``critic`` defining those observations for +the critic model (in the case of asymmetric actor critic training). Since we are not doing asymmetric actor critic, we only need to define ``policy``. + +The rewards are more straightforward. For each clone of the scene, we need to compute a reward value and return it as a tensor of shape ``[num_envs, 1]``. As a place holder, we will make the reward the +magnitude of the linear velocity of the Jetbot in the body frame. With this reward and observation space, the agent should learn to drive the Jetbot forward or backward, with the direction determined at random +shortly after training starts. + +Finally, we can write the parts of the environment to handle termination and resetting. Replace the contents of ``_get_dones`` and ``_reset_idx`` with the following. + +.. code-block:: python + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + time_out = self.episode_length_buf >= self.max_episode_length - 1 + + return False, time_out + + 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) + + 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_state_to_sim(default_root_state, env_ids) + +Like the actions, termination and resetting are handled in two parts. First is the ``_get_dones`` method, the goal of which is simply to mark which environments need to be reset and why. +Traditionally in reinforcement learning, an "episode" ends in one of two ways: either the agent reaches a terminal state, or the episode reaches a maximum duration. +Isaac Lab is kind to us, because it manages all of this episode duration tracking behind the scenes. The configuration parameter ``episode_length_s`` defines this maximum episode length in +seconds and the parameters ``episode_length_buff`` and ``max_episode_length`` contain the number of steps taken by individual scenes (allowing for asynchronous running of the environment) and the +maximum length of the episode as converted from ``episode_length_s``. The boolean operation computing ``time_out`` just compares the current buffer size to the max and returns true if it's greater, thus +indicating which scenes are at the episode length limit. Since our current environment is a dummy, we don't define terminal states and so just return ``False`` for the first tensor (this gets projected automatically +to the correct shape through the power of pytorch). + +Finally, the ``_reset_idx`` method accepts a tensor of booleans indicating which scenes need to be reset, and resets them. Notice that this is the only other method of ``DirectRLEnv`` that directly calls +``super``, which is done so here to manage the internal buffers related to episode length. For those environments indicated by ``env_ids`` we retrieve the root default state, and reset the robot to that state while +also offsetting the position of each robot according to the origin of the corresponding scene. This is a consequence of the cloning procedure, which starts with a single robot and a single default state defined in the world +frame. Don't forget this step for your own custom environments! + +With these changes complete, you should see the Jetbot slowly learn to drive forward when you launch the task with the template ``train.py`` script. + +.. figure:: ../../_static/setup/walkthrough_1_1_result.jpg + :align: center + :figwidth: 100% + :alt: The Jetbot invasion begins! diff --git a/docs/source/setup/walkthrough/training_jetbot_gt.rst b/docs/source/setup/walkthrough/training_jetbot_gt.rst new file mode 100644 index 000000000000..05e89ef45644 --- /dev/null +++ b/docs/source/setup/walkthrough/training_jetbot_gt.rst @@ -0,0 +1,221 @@ +.. _walkthrough_training_jetbot_gt: + +Training the Jetbot: Ground Truth +====================================== + +With the environment defined, we can now start modifying our observations and rewards in order to train a policy +to act as a controller for the Jetbot. As a user, we would like to be able to specify the desired direction for the Jetbot to drive, +and have the wheels turn such that the robot drives in that specified direction as fast as possible. How do we achieve this with +Reinforcement Learning (RL)? If you want to cut to the end and checkout the result of this stage of the walk through, checkout +`this branch of the tutorial repository `_! + +Expanding the Environment +-------------------------- + +The very first thing we need to do is create the logic for setting commands for each Jetbot on the stage. Each command will be a unit vector, and +we need one for every clone of the robot on the stage, which means a tensor of shape ``[num_envs, 3]``. Even though the Jetbot only navigates in the +2D plane, by working with 3D vectors we get to make use of all the math utilities provided by Isaac Lab. + +It would also be a good idea to setup visualizations, so we can more easily tell what the policy is doing during training and inference. +In this case, we will define two arrow ``VisualizationMarkers``: one to represent the "forward" direction of the robot, and one to +represent the command direction. When the policy is fully trained, these arrows should be aligned! Having these visualizations in place +early helps us avoid "silent bugs": issues in the code that do not cause it to crash. + +To begin, we need to define the marker config and then instantiate the markers with that config. Add the following to the global scope of ``isaac_lab_tutorial_env.py`` + +.. code-block:: python + + from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + import isaaclab.utils.math as math_utils + + def define_markers() -> VisualizationMarkers: + """Define markers with various different shapes.""" + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/myMarkers", + markers={ + "forward": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd", + scale=(0.25, 0.25, 0.5), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 1.0)), + ), + "command": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd", + scale=(0.25, 0.25, 0.5), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + return VisualizationMarkers(cfg=marker_cfg) + +The ``VisualizationMarkersCfg`` defines USD prims to serve as the "marker". Any prim will do, but generally you want to keep markers as simple as possible because the cloning of markers occurs at runtime on every time step. +This is because the purpose of these markers is for *debug visualization only* and not to be a part of the simulation: the user has full control over how many markers to draw when and where. +NVIDIA provides several simple meshes on our public nucleus server, located at ``ISAAC_NUCLEUS_DIR``, and for obvious reasons we choose to use ``arrow_x.usd``. + +For a more detailed example of using ``VisualizationMarkers`` checkout the ``markers.py`` demo! + +.. dropdown:: Code for the markers.py demo + :icon: code + + .. literalinclude:: ../../../../scripts/demos/markers.py + :language: python + :linenos: + +Next, we need to expand the initialization and setup steps to construct the data we need for tracking the commands as well as the marker positions and rotations. Replace the contents of +``_setup_scene`` with the following + +.. code-block:: python + + 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) + + self.visualization_markers = define_markers() + + # setting aside useful variables for later + self.up_dir = torch.tensor([0.0, 0.0, 1.0]).cuda() + self.yaws = torch.zeros((self.cfg.scene.num_envs, 1)).cuda() + self.commands = torch.randn((self.cfg.scene.num_envs, 3)).cuda() + self.commands[:,-1] = 0.0 + self.commands = self.commands/torch.linalg.norm(self.commands, dim=1, keepdim=True) + + # offsets to account for atan range and keep things on [-pi, pi] + ratio = self.commands[:,1]/(self.commands[:,0]+1E-8) + gzero = torch.where(self.commands > 0, True, False) + lzero = torch.where(self.commands < 0, True, False) + plus = lzero[:,0]*gzero[:,1] + minus = lzero[:,0]*lzero[:,1] + offsets = torch.pi*plus - torch.pi*minus + self.yaws = torch.atan(ratio).reshape(-1,1) + offsets.reshape(-1,1) + + self.marker_locations = torch.zeros((self.cfg.scene.num_envs, 3)).cuda() + self.marker_offset = torch.zeros((self.cfg.scene.num_envs, 3)).cuda() + self.marker_offset[:,-1] = 0.5 + self.forward_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4)).cuda() + self.command_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4)).cuda() + +Most of this is setting up the book keeping for the commands and markers, but the command initialization and the yaw calculations are worth diving into. The commands +are sampled from a multivariate normal distribution via ``torch.randn`` with the z component fixed to zero and then normalized to unit length. In order to point our +command markers along these vectors, we need to rotate the base arrow mesh appropriately. This means we need to define a `quaternion `_ that will rotate the arrow +prim about the z axis by some angle defined by the command. By convention, rotations about the z axis are called a "yaw" rotation (akin to roll and pitch). + +Luckily for us, Isaac Lab provides a utility to generate a quaternion from an axis of rotation and an angle: :func:`isaaclab.utils.math.quat_from_axis_angle`, so the only +tricky part now is determining that angle. + +.. figure:: ../../_static/setup/walkthrough_training_vectors.svg + :align: center + :figwidth: 100% + :alt: Useful vector definitions for training + +The yaw is defined about the z axis, with a yaw of 0 aligning with the x axis and positive angles opening counterclockwise. The x and y components of the command vector +define the tangent of this angle, and so we need the *arctangent* of that ratio to get the yaw. + +Now, consider two commands: Command A is in quadrant 2 at (-x, y), while command B is in quadrant 4 at (x, -y). The ratio of the +y component to the x component is identical for both A and B. If we do not account for this, then some of our command arrows will be +pointing in the opposite direction of the command! Essentially, our commands are defined on ``[-pi, pi]`` but ``arctangent`` is +only defined on ``[-pi/2, pi/2]``. + +To remedy this, we add or subtract ``pi`` from the yaw depending on the quadrant of the command. + +.. code-block:: python + + ratio = self.commands[:,1]/(self.commands[:,0]+1E-8) #in case the x component is zero + gzero = torch.where(self.commands > 0, True, False) + lzero = torch.where(self.commands < 0, True, False) + plus = lzero[:,0]*gzero[:,1] + minus = lzero[:,0]*lzero[:,1] + offsets = torch.pi*plus - torch.pi*minus + self.yaws = torch.atan(ratio).reshape(-1,1) + offsets.reshape(-1,1) + +Boolean expressions involving tensors can have ambiguous definitions and pytorch will throw errors regarding this. Pytorch provides +various methods to make the definitions explicit. The method ``torch.where`` produces a tensor with the same shape as the input +with each element of the output is determined by the evaluation of that expression on only that element. A reliable way to handle +boolean operations with tensors is to simply produce boolean indexing tensors and then represent the operation algebraically, with ``AND`` +as multiplication and ``OR`` as addition, which is what we do above. This is equivalent to the pseudocode: + +.. code-block:: python + + yaws = torch.atan(ratio) + yaws[commands[:,0] < 0 and commands[:,1] > 0] += torch.pi + yaws[commands[:,0] < 0 and commands[:,1] < 0] -= torch.pi + +Next we have the method for actually visualizing the markers. Remember, these markers aren't scene entities! We need to "draw" them whenever we +want to see them. + +.. code-block:: python + + def _visualize_markers(self): + # get marker locations and orientations + self.marker_locations = self.robot.data.root_pos_w + self.forward_marker_orientations = self.robot.data.root_quat_w + self.command_marker_orientations = math_utils.quat_from_angle_axis(self.yaws, self.up_dir).squeeze() + + # offset markers so they are above the jetbot + loc = self.marker_locations + self.marker_offset + loc = torch.vstack((loc, loc)) + rots = torch.vstack((self.forward_marker_orientations, self.command_marker_orientations)) + + # render the markers + all_envs = torch.arange(self.cfg.scene.num_envs) + indices = torch.hstack((torch.zeros_like(all_envs), torch.ones_like(all_envs))) + self.visualization_markers.visualize(loc, rots, marker_indices=indices) + +The ``visualize`` method of ``VisualizationMarkers`` is like this "draw" function. It accepts tensors for the spatial +transformations of the markers, and a ``marker_indices`` tensor to specify which marker prototype to use for each marker. So +long as the first dimension of all of these tensors match, this function will draw those markers with the specified transformations. +This is why we stack the locations, rotations, and indices. + +Now we just need to call ``_visualize_markers`` on the pre physics step to make the arrows visible. Replace ``_pre_physics_step`` with the following + +.. code-block:: python + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions.clone() + self._visualize_markers() + +The last major modification before we dig into the RL training is to update the ``_reset_idx`` method to account for the commands and markers. Whenever we reset an environment, +we need to generate a new command and reset the markers. The logic for this is already covered above. Replace the contents of ``_reset_idx`` with the following: + +.. code-block:: python + + 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) + + # pick new commands for reset envs + self.commands[env_ids] = torch.randn((len(env_ids), 3)).cuda() + self.commands[env_ids,-1] = 0.0 + self.commands[env_ids] = self.commands[env_ids]/torch.linalg.norm(self.commands[env_ids], dim=1, keepdim=True) + + # recalculate the orientations for the command markers with the new commands + ratio = self.commands[env_ids][:,1]/(self.commands[env_ids][:,0]+1E-8) + gzero = torch.where(self.commands[env_ids] > 0, True, False) + lzero = torch.where(self.commands[env_ids]< 0, True, False) + plus = lzero[:,0]*gzero[:,1] + minus = lzero[:,0]*lzero[:,1] + offsets = torch.pi*plus - torch.pi*minus + self.yaws[env_ids] = torch.atan(ratio).reshape(-1,1) + offsets.reshape(-1,1) + + # set the root state for the reset envs + 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_state_to_sim(default_root_state, env_ids) + self._visualize_markers() + + +And that's it! We now generate commands and can visualize it the heading of the Jetbot. We are ready to start tinkering with the observations and rewards. + +.. figure:: ../../_static/setup/walkthrough_1_2_arrows.jpg + :align: center + :figwidth: 100% + :alt: Visualization of the command markers diff --git a/docs/source/setup/walkthrough/training_jetbot_reward_exploration.rst b/docs/source/setup/walkthrough/training_jetbot_reward_exploration.rst new file mode 100644 index 000000000000..efdce4689c99 --- /dev/null +++ b/docs/source/setup/walkthrough/training_jetbot_reward_exploration.rst @@ -0,0 +1,146 @@ +.. _walkthrough_training_jetbot_reward_exploration: + +Exploring the RL problem +========================= + +The command to the Jetbot is a unit vector in specifying the desired drive direction and we must make the agent aware of this somehow +so it can adjust its actions accordingly. There are many possible ways to do this, with the "zeroth order" approach to simply change the observation space to include +this command. To start, **edit the ``IsaacLabTutorialEnvCfg`` to set the observation space to 9**: the world velocity vector contains the linear and angular velocities +of the robot, which is 6 dimensions and if we append the command to this vector, that's 9 dimensions for the observation space in total. + +Next, we just need to do that appending when we get the observations. We also need to calculate our forward vectors for later use. The forward vector for the Jetbot is +the x axis, so we apply the ``root_link_quat_w`` to ``[1,0,0]`` to get the forward vector in the world frame. Replace the ``_get_observations`` method with the following: + +.. code-block:: python + + def _get_observations(self) -> dict: + self.velocity = self.robot.data.root_com_vel_w + self.forwards = math_utils.quat_apply(self.robot.data.root_link_quat_w, self.robot.data.FORWARD_VEC_B) + obs = torch.hstack((self.velocity, self.commands)) + observations = {"policy": obs} + return observations + +So now what should the reward be? + +When the robot is behaving as desired, it will be driving at full speed in the direction of the command. If we reward both +"driving forward" and "alignment to the command", then maximizing that combined signal should result in driving to the command... right? + +Let's give it a try! Replace the ``_get_rewards`` method with the following: + +.. code-block:: python + + def _get_rewards(self) -> torch.Tensor: + forward_reward = self.robot.data.root_com_lin_vel_b[:,0].reshape(-1,1) + alignment_reward = torch.sum(self.forwards * self.commands, dim=-1, keepdim=True) + total_reward = forward_reward + alignment_reward + return total_reward + +The ``forward_reward`` is the x component of the linear center of mass velocity of the robot in the body frame. We know that +the x direction is the forward direction for the asset, so this should be equivalent to inner product between the forward vector and +the linear velocity in the world frame. The alignment term is the inner product between the forward vector and the command vector: when they are +pointing in the same direction this term will be 1, but in the opposite direction it will be -1. We add them together to get the combined reward and +we can finally run training! Let's see what happens! + +.. code-block:: bash + + python scripts/skrl/train.py --task=Template-Isaac-Lab-Tutorial-Direct-v0 + + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/walkthrough_naive_webp.webp + :align: center + :figwidth: 100% + :alt: Naive results + +Surely we can do better! + +Reward and Observation Tuning +------------------------------- + +When tuning an environment for training, as a rule of thumb, you want to keep the observation space as small as possible. This is to +reduce the number parameters in the model (the literal interpretation of Occam's razor) and thus improve training time. In this case we +need to somehow encode our alignment to the command and our forward speed. One way to do this is to exploit the dot and cross products +from linear algebra! Replace the contents of ``_get_observations`` with the following: + +.. code-block:: python + + def _get_observations(self) -> dict: + self.velocity = self.robot.data.root_com_vel_w + self.forwards = math_utils.quat_apply(self.robot.data.root_link_quat_w, self.robot.data.FORWARD_VEC_B) + + dot = torch.sum(self.forwards * self.commands, dim=-1, keepdim=True) + cross = torch.cross(self.forwards, self.commands, dim=-1)[:,-1].reshape(-1,1) + forward_speed = self.robot.data.root_com_lin_vel_b[:,0].reshape(-1,1) + obs = torch.hstack((dot, cross, forward_speed)) + + observations = {"policy": obs} + return observations + +We also need to **edit the ``IsaacLabTutorialEnvCfg`` to set the observation space back to 3** which includes the dot product, the z component of the cross product, and the forward speed. + +The dot or inner product tells us how aligned two vectors are as a single scalar quantity. If they are very aligned and pointed in the same direction, then the inner +product will be large and positive, but if they are aligned and in opposite directions, it will be large and negative. If two vectors are +perpendicular, the inner product is zero. This means that the inner product between the forward vector and the command vector can tell us +how much we are facing towards or away from the command, but not which direction we need to turn to improve alignment. + +The cross product also tells us how aligned two vectors are, but it expresses this relationship as a vector. The cross product between any +two vectors defines an axis that is perpendicular to the plane containing the two argument vectors, where the direction of the result vector along this axis is +determined by the chirality (dimension ordering, or handedness) of the coordinate system. In our case, we can exploit the fact that we are operating in 2D to only +examine the z component of the result of :math:`\vec{forward} \times \vec{command}`. This component will be zero if the vectors are colinear, positive if the +command vector is to the left of forward, and negative if it's to the right. + +Finally, the x component of the center of mass linear velocity tells us our forward speed, with positive being forward and negative being backwards. We stack these together +"horizontally" (along dim 1) to generate the observations for each Jetbot. This alone improves performance! + + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/walkthrough_improved_webp.webp + :align: center + :figwidth: 100% + :alt: Improved results + +It seems to qualitatively train better, and the Jetbots are somewhat inching forward... Surely we can do better still! + +Another rule of thumb for training is to reduce and simplify the reward function as much as possible. Terms in the reward behave similarly to +the logical "OR" operation. In our case, we are rewarding driving forward and being aligned to the command by adding them together, so our agent +can be reward for driving forward OR being aligned to the command. To force the agent to learn to drive in the direction of the command, we should only +reward the agent driving forward AND being aligned. Logical AND suggests multiplication and therefore the following reward function: + +.. code-block:: python + + def _get_rewards(self) -> torch.Tensor: + forward_reward = self.robot.data.root_com_lin_vel_b[:,0].reshape(-1,1) + alignment_reward = torch.sum(self.forwards * self.commands, dim=-1, keepdim=True) + total_reward = forward_reward*alignment_reward + return total_reward + +Now we will only get rewarded for driving forward if our alignment reward is non zero. Let's see what kind of result this produces! + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/walkthrough_tuned_webp.webp + :align: center + :figwidth: 100% + :alt: Tuned results + +It definitely trains faster, but the Jetbots have learned to drive in reverse if the command is pointed behind them. This may be desirable in our +case, but it shows just how dependent the policy behavior is on the reward function. In this case, there are **degenerate solutions** to our +reward function: The reward is maximized for driving forward and aligned to the command, but if the Jetbot drives in reverse, then the forward +term is negative, and if its driving in reverse towards the command, then the alignment term is **also negative**, meaning hat the reward is positive! +When you design your own environments, you will run into degenerate solutions like this and a significant amount of reward engineering is devoted to +suppressing or supporting these behaviors by modifying the reward function. + +Let's say, in our case, we don't want this behavior. In our case, the alignment term has a domain of ``[-1, 1]``, but we would much prefer it to be mapped +only to positive values. We don't want to *eliminate* the sign on the alignment term, rather, we would like large negative values to be near zero, so if we +are misaligned, we don't get rewarded. The exponential function accomplishes this! + +.. code-block:: python + + def _get_rewards(self) -> torch.Tensor: + forward_reward = self.robot.data.root_com_lin_vel_b[:,0].reshape(-1,1) + alignment_reward = torch.sum(self.forwards * self.commands, dim=-1, keepdim=True) + total_reward = forward_reward*torch.exp(alignment_reward) + return total_reward + +Now when we train, the Jetbots will turn to always drive towards the command in the forward direction! + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/walkthrough_directed_webp.webp + :align: center + :figwidth: 100% + :alt: Directed results diff --git a/docs/source/tutorials/00_sim/launch_app.rst b/docs/source/tutorials/00_sim/launch_app.rst index 8013e9975c32..05fa32c4648d 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/00_sim/spawn_prims.rst b/docs/source/tutorials/00_sim/spawn_prims.rst index caeafa714b31..66941ddb1c7f 100644 --- a/docs/source/tutorials/00_sim/spawn_prims.rst +++ b/docs/source/tutorials/00_sim/spawn_prims.rst @@ -114,7 +114,7 @@ Here we make an Xform prim to group all the primitive shapes under it. .. literalinclude:: ../../../../scripts/tutorials/00_sim/spawn_prims.py :language: python :start-at: # create a new xform prim for all objects to be spawned under - :end-at: prim_utils.create_prim("/World/Objects", "Xform") + :end-at: sim_utils.create_prim("/World/Objects", "Xform") Next, we spawn a cone using the :class:`~sim.spawners.shapes.ConeCfg` class. It is possible to specify the radius, height, physics properties, and material properties of the cone. By default, the physics and material diff --git a/docs/source/tutorials/01_assets/add_new_robot.rst b/docs/source/tutorials/01_assets/add_new_robot.rst new file mode 100644 index 000000000000..a4d258f82c15 --- /dev/null +++ b/docs/source/tutorials/01_assets/add_new_robot.rst @@ -0,0 +1,113 @@ +.. _tutorial-add-new-robot: + +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 `_. +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. + +In this tutorial, we will examine how to add a new robot to Isaac Lab. The key step is creating an ``AssetBaseCfg`` that defines +the interface between the USD articulation of the robot and the learning algorithms available through Isaac Lab. + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``add_new_robot`` script in the ``scripts/tutorials/01_assets`` directory. + +.. dropdown:: Code for add_new_robot.py + :icon: code + + .. literalinclude:: ../../../../scripts/tutorials/01_assets/add_new_robot.py + :language: python + :linenos: + +The Code Explained +~~~~~~~~~~~~~~~~~~ + +Fundamentally, a robot is an articulation with joint drives. To move a robot around in the simulation, we must apply +targets to its drives and step the sim forward in time. However, to control a robot strictly through joint drives is tedious, especially if +you want to control anything complex, and doubly so if you want to clone the robot across multiple environments. + +To facilitate this, Isaac Lab provides a collection of ``configuration`` classes that define which parts of the USD need +to be cloned, which parts are actuators to be controlled by an agent, how it should be reset, etc... There are many ways +you can configure a single robot asset for Isaac Lab depending on how much fine tuning the asset requires. To demonstrate, +the tutorial script imports two robots: The first robot, the ``Jetbot``, is configured minimally while the second robot, the ``Dofbot``, is configured with additional parameters. + +The Jetbot is a simple, two wheeled differential base with a camera on top. The asset is used for a number of demonstrations and +tutorials in Isaac Sim, so we know it's good to go! To bring it into Isaac lab, we must first define one of these configurations. +Because a robot is an articulation with joint drives, we define an ``ArticulationCfg`` that describes the robot. + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/add_new_robot.py + :language: python + :lines: 27-38 + +This is the minimal configuration for a robot in Isaac Lab. There are only two required parameters: ``spawn`` and ``actuators``. + +The ``spawn`` parameter is looking for a ``SpawnerCfg``, and is used to specify the USD asset that defines the robot in the sim. +The Isaac Lab simulation utilities, ``isaaclab.sim``, provides us with a ``USDFileCfg`` class that consumes a path to our USD +asset, and generates the ``SpawnerCfg`` we need. In this case, the ``jetbot.usd`` is located +with the `Isaac Assets `_ under ``Robots/Jetbot/jetbot.usd``. + +The ``actuators`` parameter is a dictionary of Actuator Configs and defines what parts of the robot we intend to control with an agent. +There are many different ways to update the state of a joint in time towards some target. Isaac Lab provides a collection of actuator +classes that can be used to match common actuator models or even implement your own! In this case, we are using the ``ImplicitActuatorCfg`` class to specify +the actuators for the robot, because they are simple wheels and the defaults are fine. + +Specifying joint name keys for this dictionary can be done to varying levels of specificity. +The jetbot only has a few joints, and we are just going to use the defaults specified in the USD asset, so we can use a simple regex, ``.*`` to specify all joints. +Other regex can also be used to group joints and associated configurations. + +.. note:: + + Both stiffness and damping must be specified in the implicit actuator, but a value of ``None`` will use the defaults defined in the USD asset. + +While this is the minimal configuration, there are a number of other parameters we could specify + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/add_new_robot.py + :language: python + :lines: 39-82 + +This configuration can be used to add a Dofbot to the scene, and it contains some of those parameters. +The Dofbot is a hobbiest robot arm with several joints, and so we have more options available for configuration. +The two most notable differences though is the addition of configurations for physics properties, and the initial state of the robot, ``init_state``. + +The ``USDFileCfg`` has special parameters for rigid bodies and robots, among others. The ``rigid_props`` parameter expects +a ``RigidBodyPropertiesCfg`` that allows you to specify body link properties of the robot being spawned relating to its behavior +as a "physical object" in the simulation. The ``articulation_props`` meanwhile governs the properties relating to the solver +being used to step the joints through time, and so it expects an ``ArticulationRootPropertiesCfg`` to be configured. +There are many other physics properties and parameters that can be specified through configurations provided by :class:`isaaclab.sim.schemas`. + +The ``ArticulationCfg`` can optionally include the ``init_state`` parameter, that defines the initial state of the articulation. +The initial state of an articulation is a special, user defined state that is used when the robot is spawned or reset by Isaac Lab. +The initial joint state, ``joint_pos``, is specified by a dictionary of floats with the USD joint names as keys (**not** the actuator names). +Something else worth noting here is the coordinate system of the initial position, ``pos``, which is that of the environment. +In this case, by specifying a position of ``(0.25, -0.25, 0.0)`` we are offsetting the spawn position of the robot **from the origin of the environment**, and not the world. + +Armed with the configurations for these robots, we can now add them to the scene and interact with them in the usual way +for the direct workflow: by defining an ``InteractiveSceneCfg`` containing the articulation configs for the robots ... + + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/add_new_robot.py + :language: python + :lines: 85 - 99 + + +...and then stepping the simulation while updating the scene entities appropriately. + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/add_new_robot.py + :language: python + :lines: 101 - 158 + + +.. note:: + + You may see a warning that not all actuators are configured! This is expected because we don't handle the gripper for this tutorial. + +.. figure:: ../../_static/tutorials/tutorial_add_new_robot_result.jpg + :align: center + :figwidth: 100% + :alt: The new robots say hi! diff --git a/docs/source/tutorials/01_assets/run_surface_gripper.rst b/docs/source/tutorials/01_assets/run_surface_gripper.rst new file mode 100644 index 000000000000..402d8e084701 --- /dev/null +++ b/docs/source/tutorials/01_assets/run_surface_gripper.rst @@ -0,0 +1,170 @@ +.. _tutorial-interact-surface-gripper: + +Interacting with a surface gripper +================================== + +.. currentmodule:: isaaclab + + +This tutorial shows how to interact with an articulated robot with a surface gripper attached to its end-effector in +the simulation. It is a continuation of the :ref:`tutorial-interact-articulation` tutorial, where we learned how to +interact with an articulated robot. Note that as of IsaacSim 5.0 the surface gripper are only supported on the cpu +backend. + + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``run_surface_gripper.py`` script in the ``scripts/tutorials/01_assets`` +directory. + +.. dropdown:: Code for run_surface_gripper.py + :icon: code + + .. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py + :language: python + :emphasize-lines: 61-85, 124-125, 128-142, 147-150 + :linenos: + + +The Code Explained +~~~~~~~~~~~~~~~~~~ + +Designing the scene +------------------- + +Similarly to the previous tutorial, we populate the scene with a ground plane and a distant light. Then, we spawn +an articulation from its USD file. This time a pick-and-place robot is spawned. The pick-and-place robot is a simple +robot with 3 driven axes, its gantry allows it to move along the x and y axes, as well as up and down along the z-axis. +Furthermore, the robot end-effector is outfitted with a surface gripper. +The USD file for the pick-and-place robot contains the robot's geometry, joints, and other physical properties +as well as the surface gripper. Before implementing a similar gripper on your own robot, we recommend to +check out the USD file for the gripper found on Isaaclab's Nucleus. + +For the pick-and-place robot, we use its pre-defined configuration object, you can find out more about it in the +:ref:`how-to-write-articulation-config` tutorial. For the surface gripper, we also need to create a configuration +object. This is done by instantiating a :class:`assets.SurfaceGripperCfg` object and passing it the relevant +parameters. + +The available parameters are: + +- ``max_grip_distance``: The maximum distance at which the gripper can grasp an object. +- ``shear_force_limit``: The maximum force the gripper can exert in the direction perpendicular to the gripper's axis. +- ``coaxial_force_limit``: The maximum force the gripper can exert in the direction of the gripper's axis. +- ``retry_interval``: The time the gripper will stay in a grasping state. + +As seen in the previous tutorial, we can spawn the articulation into the scene in a similar fashion by creating +an instance of the :class:`assets.Articulation` class by passing the configuration object to its constructor. The same +principle applies to the surface gripper. By passing the configuration object to the :class:`assets.SurfaceGripper` +constructor, the surface gripper is created and can be added to the scene. In practice, the object will only be +initialized when the play button is pressed. + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py + :language: python + :start-at: # Create separate groups called "Origin1", "Origin2" + :end-at: surface_gripper = SurfaceGripper(cfg=surface_gripper_cfg) + + +Running the simulation loop +--------------------------- + +Continuing from the previous tutorial, we reset the simulation at regular intervals, set commands to the articulation, +step the simulation, and update the articulation's internal buffers. + +Resetting the simulation +"""""""""""""""""""""""" + +To reset the surface gripper, we only need to call the :meth:`SurfaceGripper.reset` method which will reset the +internal buffers and caches. + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py + :language: python + :start-at: # Opens the gripper and makes sure the gripper is in the open state + :end-at: surface_gripper.reset() + +Stepping the simulation +""""""""""""""""""""""" + +Applying commands to the surface gripper involves two steps: + +1. *Setting the desired commands*: This sets the desired gripper commands (Open, Close, or Idle). +2. *Writing the data to the simulation*: Based on the surface gripper's configuration, this step handles writes the + converted values to the PhysX buffer. + +In this tutorial, we use a random command to set the gripper's command. The gripper behavior is as follows: + +- -1 < command < -0.3 --> Gripper is Opening +- -0.3 < command < 0.3 --> Gripper is Idle +- 0.3 < command < 1 --> Gripper is Closing + +At every step, we randomly sample commands and set them to the gripper by calling the +:meth:`SurfaceGripper.set_grippers_command` method. After setting the commands, we call the +:meth:`SurfaceGripper.write_data_to_sim` method to write the data to the PhysX buffer. Finally, we step +the simulation. + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py + :language: python + :start-at: # Sample a random command between -1 and 1. + :end-at: surface_gripper.write_data_to_sim() + + +Updating the state +"""""""""""""""""" + +To know the current state of the surface gripper, we can query the :meth:`assets.SurfaceGripper.state` property. +This property returns a tensor of size ``[num_envs]`` where each element is either ``-1``, ``0``, or ``1`` +corresponding to the gripper state. This property is updated every time the :meth:`assets.SurfaceGripper.update` method +is called. + +- ``-1`` --> Gripper is Open +- ``0`` --> Gripper is Closing +- ``1`` --> Gripper is Closed + +.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py + :language: python + :start-at: # Read the gripper state from the simulation + :end-at: surface_gripper_state = surface_gripper.state + + +The Code Execution +~~~~~~~~~~~~~~~~~~ + + +To run the code and see the results, let's run the script from the terminal: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/tutorials/01_assets/run_surface_gripper.py --device cpu + + +This command should open a stage with a ground plane, lights, and two pick-and-place robots. +In the terminal, you should see the gripper state and the command being printed. +To stop the simulation, you can either close the window, or press ``Ctrl+C`` in the terminal. + +.. figure:: ../../_static/tutorials/tutorial_run_surface_gripper.jpg + :align: center + :figwidth: 100% + :alt: result of run_surface_gripper.py + +In this tutorial, we learned how to create and interact with a surface gripper. We saw how to set commands and +query the gripper state. We also saw how to update its buffers to read the latest state from the simulation. + +In addition to this tutorial, we also provide a few other scripts that spawn different robots. These are included +in the ``scripts/demos`` directory. You can run these scripts as: + +.. code-block:: bash + + # Spawn many pick-and-place robots and perform a pick-and-place task + ./isaaclab.sh -p scripts/demos/pick_and_place.py + +Note that in practice, the users would be expected to register their :class:`assets.SurfaceGripper` instances inside +a :class:`isaaclab.InteractiveScene` object, which will automatically handle the calls to the +:meth:`assets.SurfaceGripper.write_data_to_sim` and :meth:`assets.SurfaceGripper.update` methods. + +.. code-block:: python + + # Create a scene + scene = InteractiveScene() + + # Register the surface gripper + scene.surface_grippers["gripper"] = surface_gripper diff --git a/docs/source/tutorials/03_envs/configuring_rl_training.rst b/docs/source/tutorials/03_envs/configuring_rl_training.rst new file mode 100644 index 000000000000..2eb2b0b5e763 --- /dev/null +++ b/docs/source/tutorials/03_envs/configuring_rl_training.rst @@ -0,0 +1,140 @@ +.. _tutorial-configure-rl-training: + +Configuring an RL Agent +======================= + +.. currentmodule:: isaaclab + +In the previous tutorial, we saw how to train an RL agent to solve the cartpole balancing task +using the `Stable-Baselines3`_ library. In this tutorial, we will see how to configure the +training process to use different RL libraries and different training algorithms. + +In the directory ``scripts/reinforcement_learning``, you will find the scripts for +different RL libraries. These are organized into subdirectories named after the library name. +Each subdirectory contains the training and playing scripts for the library. + +To configure a learning library with a specific task, you need to create a configuration file +for the learning agent. This configuration file is used to create an instance of the learning agent +and is used to configure the training process. Similar to the environment registration shown in +the :ref:`tutorial-register-rl-env-gym` tutorial, you can register the learning agent with the +``gymnasium.register`` method. + +The Code +-------- + +As an example, we will look at the configuration included for the task ``Isaac-Cartpole-v0`` +in the ``isaaclab_tasks`` package. This is the same task that we used in the +:ref:`tutorial-run-rl-training` tutorial. + +.. literalinclude:: ../../../../source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/__init__.py + :language: python + :lines: 18-29 + +The Code Explained +------------------ + +Under the attribute ``kwargs``, we can see the configuration for the different learning libraries. +The key is the name of the library and the value is the path to the configuration instance. +This configuration instance can be a string, a class, or an instance of the class. +For example, the value of the key ``"rl_games_cfg_entry_point"`` is a string that points to the +configuration YAML file for the RL-Games library. Meanwhile, the value of the key +``"rsl_rl_cfg_entry_point"`` points to the configuration class for the RSL-RL library. + +The pattern used for specifying an agent configuration class follows closely to that used for +specifying the environment configuration entry point. This means that while the following +are equivalent: + + +.. dropdown:: Specifying the configuration entry point as a string + :icon: code + + .. code-block:: python + + from . import agents + + 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", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CartpolePPORunnerCfg", + }, + ) + +.. dropdown:: Specifying the configuration entry point as a class + :icon: code + + .. code-block:: python + + from . import agents + + 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", + "rsl_rl_cfg_entry_point": agents.rsl_rl_ppo_cfg.CartpolePPORunnerCfg, + }, + ) + +The first code block is the preferred way to specify the configuration entry point. +The second code block is equivalent to the first one, but it leads to import of the configuration +class which slows down the import time. This is why we recommend using strings for the configuration +entry point. + +All the scripts in the ``scripts/reinforcement_learning`` directory are configured by default to read the +``_cfg_entry_point`` from the ``kwargs`` dictionary to retrieve the configuration instance. + +For instance, the following code block shows how the ``train.py`` script reads the configuration +instance for the Stable-Baselines3 library: + +.. dropdown:: Code for train.py with SB3 + :icon: code + + .. literalinclude:: ../../../../scripts/reinforcement_learning/sb3/train.py + :language: python + :emphasize-lines: 26-28, 102-103 + :linenos: + +The argument ``--agent`` is used to specify the learning library to use. This is used to +retrieve the configuration instance from the ``kwargs`` dictionary. You can manually specify +alternate configuration instances by passing the ``--agent`` argument. + +The Code Execution +------------------ + +Since for the cartpole balancing task, RSL-RL library offers two configuration instances, +we can use the ``--agent`` argument to specify the configuration instance to use. + +* Training with the standard PPO configuration: + + .. code-block:: bash + + # standard PPO training + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-v0 --headless \ + --run_name ppo + +* Training with the PPO configuration with symmetry augmentation: + + .. code-block:: bash + + # PPO training with symmetry augmentation + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-v0 --headless \ + --agent rsl_rl_with_symmetry_cfg_entry_point \ + --run_name ppo_with_symmetry_data_augmentation + + # you can use hydra to disable symmetry augmentation but enable mirror loss computation + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task Isaac-Cartpole-v0 --headless \ + --agent rsl_rl_with_symmetry_cfg_entry_point \ + --run_name ppo_without_symmetry_data_augmentation \ + agent.algorithm.symmetry_cfg.use_data_augmentation=false + +The ``--run_name`` argument is used to specify the name of the run. This is used to +create a directory for the run in the ``logs/rsl_rl/cartpole`` directory. + +.. _Stable-Baselines3: https://stable-baselines3.readthedocs.io/en/master/ +.. _RL-Games: https://github.com/Denys88/rl_games +.. _RSL-RL: https://github.com/leggedrobotics/rsl_rl +.. _SKRL: https://skrl.readthedocs.io diff --git a/docs/source/tutorials/03_envs/policy_inference_in_usd.rst b/docs/source/tutorials/03_envs/policy_inference_in_usd.rst index 46df6d0537be..fa004352610b 100644 --- a/docs/source/tutorials/03_envs/policy_inference_in_usd.rst +++ b/docs/source/tutorials/03_envs/policy_inference_in_usd.rst @@ -58,7 +58,8 @@ Note that not all learning libraries support exporting the policy to a jit or on For libraries that don't currently support this functionality, please refer to the corresponding ``play.py`` script for the library to learn about how to initialize the policy. -We can then load the warehouse asset and run inference on the H1 robot using the exported jit policy. +We can then load the warehouse asset and run inference on the H1 robot using the exported jit policy +(``policy.pt`` file in the ``exported/`` directory). .. code-block:: bash diff --git a/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst b/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst index 5d0fcae4d909..3d9f40667b62 100644 --- a/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst +++ b/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst @@ -53,7 +53,7 @@ in seconds through the :attr:`sensors.SensorBaseCfg.update_period` attribute. Depending on the specified path and the sensor type, the sensors are attached to the prims in the scene. They may have an associated prim that is created in the scene or they may be attached to an existing prim. For instance, the camera sensor has a corresponding prim that is created in the scene, whereas for the -contact sensor, the activating the contact reporting is a property on a rigid body prim. +contact sensor, activating the contact reporting is a property on a rigid body prim. In the following, we introduce the different sensors we use in this tutorial and how they are configured. For more description about them, please check the :mod:`sensors` module. @@ -101,7 +101,7 @@ For this tutorial, the ray-cast based height scanner is attached to the base fra The pattern of rays is specified using the :attr:`~sensors.RayCasterCfg.pattern` attribute. For a uniform grid pattern, we specify the pattern using :class:`~sensors.patterns.GridPatternCfg`. Since we only care about the height information, we do not need to consider the roll and pitch -of the robot. Hence, we set the :attr:`~sensors.RayCasterCfg.attach_yaw_only` to true. +of the robot. Hence, we set the :attr:`~sensors.RayCasterCfg.ray_alignment` to "yaw". For the height-scanner, you can visualize the points where the rays hit the mesh. This is done by setting the :attr:`~sensors.SensorBaseCfg.debug_vis` attribute to true. @@ -126,7 +126,7 @@ obtain the contact information. Additional flags can be set to obtain more infor the contact, such as the contact air time, contact forces between filtered prims, etc. In this tutorial, we attach the contact sensor to the feet of the robot. The feet of the robot are -named ``"LF_FOOT"``, ``"RF_FOOT"``, ``"LH_FOOT"``, and ``"RF_FOOT"``. We pass a Regex expression +named ``"LF_FOOT"``, ``"RF_FOOT"``, ``"LH_FOOT"``, and ``"RH_FOOT"``. We pass a Regex expression ``".*_FOOT"`` to simplify the prim path specification. This Regex expression matches all prims that end with ``"_FOOT"``. diff --git a/docs/source/tutorials/05_controllers/run_diff_ik.rst b/docs/source/tutorials/05_controllers/run_diff_ik.rst index 2de81057cf6e..dda5568c0f41 100644 --- a/docs/source/tutorials/05_controllers/run_diff_ik.rst +++ b/docs/source/tutorials/05_controllers/run_diff_ik.rst @@ -79,7 +79,7 @@ joint positions, current end-effector pose, and the Jacobian matrix. While the attribute :attr:`assets.ArticulationData.joint_pos` provides the joint positions, we only want the joint positions of the robot's arm, and not the gripper. Similarly, while -the attribute :attr:`assets.Articulationdata.body_state_w` provides the state of all the +the attribute :attr:`assets.ArticulationData.body_state_w` provides the state of all the robot's bodies, we only want the state of the robot's end-effector. Thus, we need to index into these arrays to obtain the desired quantities. diff --git a/docs/source/tutorials/index.rst b/docs/source/tutorials/index.rst index 3c5864152f3b..f1096e6c05b0 100644 --- a/docs/source/tutorials/index.rst +++ b/docs/source/tutorials/index.rst @@ -43,9 +43,11 @@ class and its derivatives such as :class:`~isaaclab.assets.RigidObject`, :maxdepth: 1 :titlesonly: + 01_assets/add_new_robot 01_assets/run_rigid_object 01_assets/run_articulation 01_assets/run_deformable_object + 01_assets/run_surface_gripper Creating a Scene ---------------- @@ -77,6 +79,7 @@ different aspects of the framework to create a simulation environment for agent 03_envs/create_direct_rl_env 03_envs/register_rl_env_gym 03_envs/run_rl_training + 03_envs/configuring_rl_training 03_envs/modify_direct_rl_env 03_envs/policy_inference_in_usd diff --git a/environment.yml b/environment.yml new file mode 100644 index 000000000000..053fef4e99db --- /dev/null +++ b/environment.yml @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +channels: + - conda-forge + - defaults +dependencies: + - python=3.11 + - importlib_metadata diff --git a/greptile.json b/greptile.json new file mode 100644 index 000000000000..1f195339e5b7 --- /dev/null +++ b/greptile.json @@ -0,0 +1,3 @@ +{ + "triggerOnUpdates": false +} diff --git a/isaaclab.bat b/isaaclab.bat index ac8b390562a3..f47364e00b3b 100644 --- a/isaaclab.bat +++ b/isaaclab.bat @@ -38,6 +38,60 @@ if not exist "%isaac_path%" ( ) goto :eof +rem --- Ensure CUDA PyTorch helper ------------------------------------------ +:ensure_cuda_torch +rem expects: !python_exe! set by :extract_python_exe +setlocal EnableExtensions EnableDelayedExpansion +set "TORCH_VER=2.7.0" +set "TV_VER=0.22.0" +set "CUDA_TAG=cu128" +set "PYTORCH_INDEX=https://download.pytorch.org/whl/%CUDA_TAG%" + +rem Do we already have torch? +call "!python_exe!" -m pip show torch >nul 2>&1 +if errorlevel 1 ( + echo [INFO] Installing PyTorch !TORCH_VER! with CUDA !CUDA_TAG!... + call "!python_exe!" -m pip install "torch==!TORCH_VER!" "torchvision==!TV_VER!" --index-url "!PYTORCH_INDEX!" +) else ( + for /f "tokens=2" %%V in ('"!python_exe!" -m pip show torch ^| findstr /B /C:"Version:"') do set "TORCH_CUR=%%V" + echo [INFO] Found PyTorch version !TORCH_CUR!. + if /I not "!TORCH_CUR!"=="!TORCH_VER!+!CUDA_TAG!" ( + echo [INFO] Replacing PyTorch !TORCH_CUR! -> !TORCH_VER!+!CUDA_TAG!... + call "!python_exe!" -m pip uninstall -y torch torchvision torchaudio >nul 2>&1 + call "!python_exe!" -m pip install "torch==!TORCH_VER!" "torchvision==!TV_VER!" --index-url "!PYTORCH_INDEX!" + ) else ( + echo [INFO] PyTorch !TORCH_VER!+!CUDA_TAG! already installed. + ) +) +endlocal & exit /b 0 + +rem ----------------------------------------------------------------------- +rem Returns success (exit code 0) if Isaac Sim's version starts with "4.5" +rem ----------------------------------------------------------------------- +:is_isaacsim_version_4_5 + rem make sure we have %python_exe% + call :extract_python_exe + + rem 1) try to locate the VERSION file via the kit install + for /f "delims=" %%V in ('!python_exe! -c "import isaacsim,os;print(os.path.abspath(os.path.join(os.path.dirname(isaacsim.__file__), os.pardir, os.pardir, 'VERSION')))"') do set "VERSION_PATH=%%V" + if exist "!VERSION_PATH!" ( + for /f "usebackq delims=" %%L in ("!VERSION_PATH!") do set "ISAACSIM_VER=%%L" + ) else ( + rem 2) fallback to importlib.metadata if no VERSION file + for /f "delims=" %%L in ('!python_exe! -c "from importlib.metadata import version;print(version(''isaacsim''))"') do set "ISAACSIM_VER=%%L" + ) + + rem Clean up the version string (remove any trailing whitespace or newlines) + set "ISAACSIM_VER=!ISAACSIM_VER: =!" + + rem Use string comparison instead of findstr for more reliable matching + if "!ISAACSIM_VER:~0,3!"=="4.5" ( + exit /b 0 + ) else ( + exit /b 1 + ) + goto :eof + rem extract the python from isaacsim :extract_python_exe rem check if using conda @@ -114,13 +168,49 @@ if errorlevel 1 ( echo [ERROR] Conda could not be found. Please install conda and try again. exit /b 1 ) + +rem check if _isaac_sim symlink exists and isaacsim-rl is not installed via pip +if not exist "%ISAACLAB_PATH%\_isaac_sim" ( + python -m pip list | findstr /C:"isaacsim-rl" >nul + if errorlevel 1 ( + echo [WARNING] _isaac_sim symlink not found at %ISAACLAB_PATH%\_isaac_sim + echo This warning can be ignored if you plan to install Isaac Sim via pip. + echo If you are using a binary installation of Isaac Sim, please ensure the symlink is created before setting up the conda environment. + ) +) + rem check if the environment exists call conda env list | findstr /c:"%env_name%" >nul if %errorlevel% equ 0 ( echo [INFO] Conda environment named '%env_name%' already exists. ) else ( echo [INFO] Creating conda environment named '%env_name%'... - call conda create -y --name %env_name% python=3.10 + echo [INFO] Installing dependencies from %ISAACLAB_PATH%\environment.yml + rem โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€” + rem patch Python version if needed, but back up first + rem โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€”โ€” + copy "%ISAACLAB_PATH%environment.yml" "%ISAACLAB_PATH%environment.yml.bak" >nul + call :is_isaacsim_version_4_5 + if !ERRORLEVEL! EQU 0 ( + echo [INFO] Detected Isaac Sim 4.5 --^> forcing python=3.10 + rem Use findstr to replace the python version line + ( + for /f "delims=" %%L in ('type "%ISAACLAB_PATH%environment.yml"') do ( + set "line=%%L" + set "line=!line: =!" + if "!line:~0,15!"=="-python=3.11" ( + echo - python=3.10 + ) else ( + echo %%L + ) + ) + ) > "%ISAACLAB_PATH%environment.yml.tmp" + rem Replace the original file with the modified version + move /y "%ISAACLAB_PATH%environment.yml.tmp" "%ISAACLAB_PATH%environment.yml" >nul + ) else ( + echo [INFO] Isaac Sim ^>=5.0, installing python=3.11 + ) + call conda env create -y --file %ISAACLAB_PATH%\environment.yml -n %env_name% ) rem cache current paths for later set "cache_pythonpath=%PYTHONPATH%" @@ -199,10 +289,6 @@ rem remove variables from environment during deactivation echo $env:LD_LIBRARY_PATH="%cache_pythonpath%" ) > "%CONDA_PREFIX%\etc\conda\deactivate.d\unsetenv_vars.ps1" -rem install some extra dependencies -echo [INFO] Installing extra dependencies (this might take a few minutes)... -call conda install -c conda-forge -y importlib_metadata >nul 2>&1 - rem deactivate the environment call conda deactivate rem add information to the user about alias @@ -244,7 +330,7 @@ echo -i, --install [LIB] Install the extensions inside Isaac Lab and learni echo -f, --format Run pre-commit to format the code and check lints. echo -p, --python Run the python executable (python.bat) provided by Isaac Sim. echo -s, --sim Run the simulator executable (isaac-sim.bat) provided by Isaac Sim. -echo -t, --test Run all python unittest tests. +echo -t, --test Run all python pytest tests. echo -v, --vscode Generate the VSCode settings file from template. echo -d, --docs Build the documentation from source using sphinx. echo -n, --new Create a new external project or internal task from template. @@ -273,6 +359,10 @@ if "%arg%"=="-i" ( rem install the python packages in isaaclab/source directory echo [INFO] Installing extensions inside the Isaac Lab repository... call :extract_python_exe + rem check if pytorch is installed and its version + rem install pytorch with cuda 12.8 for blackwell support + call :ensure_cuda_torch + for /d %%d in ("%ISAACLAB_PATH%\source\*") do ( set ext_folder="%%d" call :install_isaaclab_extension @@ -293,11 +383,22 @@ if "%arg%"=="-i" ( ) rem install the rl-frameworks specified call !python_exe! -m pip install -e %ISAACLAB_PATH%\source\isaaclab_rl[!framework_name!] + rem in rare case if some packages or flaky setup override default torch installation, ensure right torch is + rem installed again + call :ensure_cuda_torch + rem update the vscode settings + rem once we have a docker container, we need to disable vscode settings + call :update_vscode_settings + shift shift ) else if "%arg%"=="--install" ( rem install the python packages in source directory echo [INFO] Installing extensions inside the Isaac Lab repository... call :extract_python_exe + rem check if pytorch is installed and its version + rem install pytorch with cuda 12.8 for blackwell support + call :ensure_cuda_torch + for /d %%d in ("%ISAACLAB_PATH%\source\*") do ( set ext_folder="%%d" call :install_isaaclab_extension @@ -318,6 +419,9 @@ if "%arg%"=="-i" ( ) rem install the rl-frameworks specified call !python_exe! -m pip install -e %ISAACLAB_PATH%\source\isaaclab_rl[!framework_name!] + rem in rare case if some packages or flaky setup override default torch installation, ensure right torch is + rem installed again + call :ensure_cuda_torch rem update the vscode settings rem once we have a docker container, we need to disable vscode settings call :update_vscode_settings @@ -407,32 +511,20 @@ 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" ) - !python_exe! !allArgs! + call !python_exe! !allArgs! goto :end ) else if "%arg%"=="--python" ( rem run the python provided by Isaac Sim 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" ) - !python_exe! !allArgs! + call !python_exe! !allArgs! goto :end ) else if "%arg%"=="-s" ( rem run the simulator exe provided by isaacsim @@ -447,7 +539,7 @@ if "%arg%"=="-i" ( set "skip=1" ) ) - !isaacsim_exe! --ext-folder %ISAACLAB_PATH%\source !allArgs1 + !isaacsim_exe! --ext-folder %ISAACLAB_PATH%\source !allArgs! goto :end ) else if "%arg%"=="--sim" ( rem run the simulator exe provided by Isaac Sim @@ -462,7 +554,7 @@ if "%arg%"=="-i" ( set "skip=1" ) ) - !isaacsim_exe! --ext-folder %ISAACLAB_PATH%\source !allArgs1 + !isaacsim_exe! --ext-folder %ISAACLAB_PATH%\source !allArgs! goto :end ) else if "%arg%"=="-n" ( rem run the template generator script @@ -477,11 +569,11 @@ if "%arg%"=="-i" ( ) ) echo [INFO] Installing template dependencies... - !python_exe! -m pip install -q -r tools\template\requirements.txt + call !python_exe! -m pip install -q -r tools\template\requirements.txt echo. echo [INFO] Running template generator... echo. - !python_exe! tools\template\cli.py !allArgs! + call !python_exe! tools\template\cli.py !allArgs! goto :end ) else if "%arg%"=="--new" ( rem run the template generator script @@ -496,11 +588,11 @@ if "%arg%"=="-i" ( ) ) echo [INFO] Installing template dependencies... - !python_exe! -m pip install -q -r tools\template\requirements.txt + call !python_exe! -m pip install -q -r tools\template\requirements.txt echo. echo [INFO] Running template generator... echo. - !python_exe! tools\template\cli.py !allArgs! + call !python_exe! tools\template\cli.py !allArgs! goto :end ) else if "%arg%"=="-t" ( rem run the python provided by Isaac Sim @@ -514,7 +606,7 @@ if "%arg%"=="-i" ( set "skip=1" ) ) - !python_exe! tools\run_all_tests.py !allArgs! + call !python_exe! -m pytest tools !allArgs! goto :end ) else if "%arg%"=="--test" ( rem run the python provided by Isaac Sim @@ -528,7 +620,7 @@ if "%arg%"=="-i" ( set "skip=1" ) ) - !python_exe! tools\run_all_tests.py !allArgs! + call !python_exe! -m pytest tools !allArgs! goto :end ) else if "%arg%"=="-v" ( rem update the vscode settings diff --git a/isaaclab.sh b/isaaclab.sh index 1347c123c8dd..00008d6ec827 100755 --- a/isaaclab.sh +++ b/isaaclab.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -22,13 +22,126 @@ export ISAACLAB_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && p # Helper functions #== +# install system dependencies +install_system_deps() { + # check if cmake is already installed + if command -v cmake &> /dev/null; then + echo "[INFO] cmake is already installed." + else + # check if running as root + if [ "$EUID" -ne 0 ]; then + echo "[INFO] Installing system dependencies..." + sudo apt-get update && sudo apt-get install -y --no-install-recommends \ + cmake \ + build-essential + else + echo "[INFO] Installing system dependencies..." + apt-get update && apt-get install -y --no-install-recommends \ + cmake \ + build-essential + fi + fi +} + +# Returns success (exit code 0 / "true") if the detected Isaac Sim version starts with 4.5, +# otherwise returns non-zero ("false"). Works with both symlinked binary installs and pip installs. +is_isaacsim_version_4_5() { + local version="" + local python_exe + python_exe=$(extract_python_exe) + + # 0) Fast path: read VERSION file from the symlinked _isaac_sim directory (binary install) + # If the repository has _isaac_sim โ†’ symlink, the VERSION file is the simplest source of truth. + if [[ -f "${ISAACLAB_PATH}/_isaac_sim/VERSION" ]]; then + # Read first line of the VERSION file; don't fail the whole script on errors. + version=$(head -n1 "${ISAACLAB_PATH}/_isaac_sim/VERSION" || true) + fi + + # 1) Package-path probe: import isaacsim and walk up to ../../VERSION (pip or nonstandard layouts) + # If we still don't know the version, ask Python where the isaacsim package lives + if [[ -z "$version" ]]; then + local sim_file="" + # Print isaacsim.__file__; suppress errors so set -e won't abort. + sim_file=$("${python_exe}" -c 'import isaacsim, os; print(isaacsim.__file__)' 2>/dev/null || true) + if [[ -n "$sim_file" ]]; then + local version_path + version_path="$(dirname "$sim_file")/../../VERSION" + # If that VERSION file exists, read it. + [[ -f "$version_path" ]] && version=$(head -n1 "$version_path" || true) + fi + fi + + # 2) Fallback: use package metadata via importlib.metadata.version("isaacsim") + if [[ -z "$version" ]]; then + version=$("${python_exe}" <<'PY' 2>/dev/null || true +from importlib.metadata import version, PackageNotFoundError +try: + print(version("isaacsim")) +except PackageNotFoundError: + pass +PY +) + fi + + # Final decision: return success if version begins with "4.5", 0 if match, 1 otherwise. + [[ "$version" == 4.5* ]] +} + # check if running in docker is_docker() { - [ -f /.dockerenv ] || \ - grep -q docker /proc/1/cgroup || \ - [[ $(cat /proc/1/comm) == "containerd-shim" ]] || \ - grep -q docker /proc/mounts || \ - [[ "$(hostname)" == *"."* ]] + [ -f /.dockerenv ] || [ -f /run/.containerenv ] || \ + grep -qaE '(docker|containerd|kubepods|podman)' /proc/1/cgroup || \ + [[ $(cat /proc/1/comm) == "containerd-shim" ]] +} + +# check if running on ARM architecture +is_arm() { + [[ "$(uname -m)" == "aarch64" ]] || [[ "$(uname -m)" == "arm64" ]] +} + +ensure_cuda_torch() { + local python_exe=$(extract_python_exe) + local pip_install_command=$(extract_pip_command) + local pip_uninstall_command=$(extract_pip_uninstall_command) + # 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 + 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="" + cur="$(${python_exe} - <<'PY' 2>/dev/null || true +try: + import torch +except Exception: + pass +else: + print(torch.__version__, end="") +PY +)" + + # skip install if version is already satisfied + if [[ "$cur" == "$want_torch" ]]; then + return 0 + fi + + # clean install torch + echo "[INFO] Installing torch==${torch_ver} and torchvision==${tv_ver} (cu${cuda_ver}) from ${index}..." + ${pip_uninstall_command} torch torchvision torchaudio >/dev/null 2>&1 || true + ${pip_install_command} -U --index-url "${index}" "torch==${torch_ver}" "torchvision==${tv_ver}" } # extract isaac sim path @@ -65,6 +178,9 @@ extract_python_exe() { if ! [[ -z "${CONDA_PREFIX}" ]]; then # use conda python local python_exe=${CONDA_PREFIX}/bin/python + elif ! [[ -z "${VIRTUAL_ENV}" ]]; then + # use uv virtual environment python + local python_exe=${VIRTUAL_ENV}/bin/python else # use kit python local python_exe=${ISAACLAB_PATH}/_isaac_sim/python.sh @@ -82,7 +198,7 @@ extract_python_exe() { if [ ! -f "${python_exe}" ]; then echo -e "[ERROR] Unable to find any Python executable at path: '${python_exe}'" >&2 echo -e "\tThis could be due to the following reasons:" >&2 - echo -e "\t1. Conda environment is not activated." >&2 + echo -e "\t1. Conda or uv environment is not activated." >&2 echo -e "\t2. Isaac Sim pip package 'isaacsim-rl' is not installed." >&2 echo -e "\t3. Python executable is not available at the default path: ${ISAACLAB_PATH}/_isaac_sim/python.sh" >&2 exit 1 @@ -114,15 +230,103 @@ extract_isaacsim_exe() { echo ${isaacsim_exe} } +# find pip command based on virtualization +extract_pip_command() { + # detect if we're in a uv environment + if [ -n "${VIRTUAL_ENV}" ] && [ -f "${VIRTUAL_ENV}/pyvenv.cfg" ] && grep -q "uv" "${VIRTUAL_ENV}/pyvenv.cfg"; then + pip_command="uv pip install" + else + # retrieve the python executable + python_exe=$(extract_python_exe) + pip_command="${python_exe} -m pip install" + fi + + echo ${pip_command} +} + +extract_pip_uninstall_command() { + # detect if we're in a uv environment + if [ -n "${VIRTUAL_ENV}" ] && [ -f "${VIRTUAL_ENV}/pyvenv.cfg" ] && grep -q "uv" "${VIRTUAL_ENV}/pyvenv.cfg"; then + pip_uninstall_command="uv pip uninstall" + else + # retrieve the python executable + python_exe=$(extract_python_exe) + pip_uninstall_command="${python_exe} -m pip uninstall -y" + fi + + echo ${pip_uninstall_command} +} + # check if input directory is a python extension and install the module install_isaaclab_extension() { # retrieve the python executable python_exe=$(extract_python_exe) + pip_command=$(extract_pip_command) + # if the directory contains setup.py then install the python module if [ -f "$1/setup.py" ]; then echo -e "\t module: $1" - ${python_exe} -m pip install --editable $1 + $pip_command --editable "$1" + 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 @@ -136,12 +340,34 @@ setup_conda_env() { exit 1 fi + # check if _isaac_sim symlink exists and isaacsim-rl is not installed via pip + if [ ! -L "${ISAACLAB_PATH}/_isaac_sim" ] && ! python -m pip list | grep -q 'isaacsim-rl'; then + echo -e "[WARNING] _isaac_sim symlink not found at ${ISAACLAB_PATH}/_isaac_sim" + echo -e "\tThis warning can be ignored if you plan to install Isaac Sim via pip." + echo -e "\tIf you are using a binary installation of Isaac Sim, please ensure the symlink is created before setting up the conda environment." + fi + # check if the environment exists if { conda env list | grep -w ${env_name}; } >/dev/null 2>&1; then echo -e "[INFO] Conda environment named '${env_name}' already exists." else echo -e "[INFO] Creating conda environment named '${env_name}'..." - conda create -y --name ${env_name} python=3.10 + echo -e "[INFO] Installing dependencies from ${ISAACLAB_PATH}/environment.yml" + + # patch Python version if needed, but back up first + cp "${ISAACLAB_PATH}/environment.yml"{,.bak} + if is_isaacsim_version_4_5; then + 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 detected, installing python=3.11" + fi + + conda env create -y --file ${ISAACLAB_PATH}/environment.yml -n ${env_name} + # (optional) restore original environment.yml: + if [[ -f "${ISAACLAB_PATH}/environment.yml.bak" ]]; then + mv "${ISAACLAB_PATH}/environment.yml.bak" "${ISAACLAB_PATH}/environment.yml" + fi fi # cache current paths for later @@ -163,10 +389,11 @@ setup_conda_env() { 'export ISAACLAB_PATH='${ISAACLAB_PATH}'' \ 'alias isaaclab='${ISAACLAB_PATH}'/isaaclab.sh' \ '' \ - '# show icon if not runninng headless' \ + '# show icon if not running headless' \ '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 @@ -208,10 +435,6 @@ setup_conda_env() { '' >> ${CONDA_PREFIX}/etc/conda/deactivate.d/unsetenv.sh fi - # install some extra dependencies - echo -e "[INFO] Installing extra dependencies (this might take a few minutes)..." - conda install -c conda-forge -y importlib_metadata &> /dev/null - # deactivate the environment conda deactivate # add information to the user about alias @@ -219,11 +442,73 @@ setup_conda_env() { echo -e "[INFO] Created conda environment named '${env_name}'.\n" echo -e "\t\t1. To activate the environment, run: conda activate ${env_name}" echo -e "\t\t2. To install Isaac Lab extensions, run: isaaclab -i" - echo -e "\t\t4. To perform formatting, run: isaaclab -f" - echo -e "\t\t5. To deactivate the environment, run: conda deactivate" + echo -e "\t\t3. To perform formatting, run: isaaclab -f" + echo -e "\t\t4. To deactivate the environment, run: conda deactivate" echo -e "\n" } +# setup uv environment for Isaac Lab +setup_uv_env() { + # get environment name from input + local env_name="$1" + local python_path="$2" + + # check uv is installed + if ! command -v uv &>/dev/null; then + echo "[ERROR] uv could not be found. Please install uv and try again." + echo "[ERROR] uv can be installed here:" + echo "[ERROR] https://docs.astral.sh/uv/getting-started/installation/" + exit 1 + fi + + # check if _isaac_sim symlink exists and isaacsim-rl is not installed via pip + if [ ! -L "${ISAACLAB_PATH}/_isaac_sim" ] && ! python -m pip list | grep -q 'isaacsim-rl'; then + echo -e "[WARNING] _isaac_sim symlink not found at ${ISAACLAB_PATH}/_isaac_sim" + echo -e "\tThis warning can be ignored if you plan to install Isaac Sim via pip." + echo -e "\tIf you are using a binary installation of Isaac Sim, please ensure the symlink is created before setting up the conda environment." + fi + + # check if the environment exists + local env_path="${ISAACLAB_PATH}/${env_name}" + if [ ! -d "${env_path}" ]; then + echo -e "[INFO] Creating uv environment named '${env_name}'..." + uv venv --clear --python "${python_path}" "${env_path}" + else + echo "[INFO] uv environment '${env_name}' already exists." + fi + + # define root path for activation hooks + local isaaclab_root="${ISAACLAB_PATH}" + + # cache current paths for later + cache_pythonpath=$PYTHONPATH + cache_ld_library_path=$LD_LIBRARY_PATH + + # ensure activate file exists + touch "${env_path}/bin/activate" + + # add variables to environment during activation + cat >> "${env_path}/bin/activate" <&2 } @@ -266,7 +552,7 @@ print_help () { if [ -z "$*" ]; then echo "[Error] No arguments provided." >&2; print_help - exit 1 + exit 0 fi # pass the arguments @@ -274,12 +560,25 @@ while [[ $# -gt 0 ]]; do # read the key case "$1" in -i|--install) + # install system dependencies first + install_system_deps # install the python packages in IsaacLab/source directory echo "[INFO] Installing extensions inside the Isaac Lab repository..." python_exe=$(extract_python_exe) + pip_command=$(extract_pip_command) + pip_uninstall_command=$(extract_pip_uninstall_command) + + # 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 # recursively look into directories and install them # this does not check dependencies between extensions export -f extract_python_exe + export -f extract_pip_command + export -f extract_pip_uninstall_command export -f install_isaaclab_extension # source directory find -L "${ISAACLAB_PATH}/source" -mindepth 1 -maxdepth 1 -type d -exec bash -c 'install_isaaclab_extension "{}"' \; @@ -299,8 +598,15 @@ while [[ $# -gt 0 ]]; do shift # past argument fi # install the learning frameworks specified - ${python_exe} -m pip install -e ${ISAACLAB_PATH}/source/isaaclab_rl["${framework_name}"] - ${python_exe} -m pip install -e ${ISAACLAB_PATH}/source/isaaclab_mimic["${framework_name}"] + ${pip_command} -e "${ISAACLAB_PATH}/source/isaaclab_rl[${framework_name}]" + ${pip_command} -e "${ISAACLAB_PATH}/source/isaaclab_mimic[${framework_name}]" + + # 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 + + # 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 @@ -312,8 +618,10 @@ while [[ $# -gt 0 ]]; do update_vscode_settings fi - # unset local variables + # unset local variables unset extract_python_exe + unset extract_pip_command + unset extract_pip_uninstall_command unset install_isaaclab_extension shift # past argument ;; @@ -331,11 +639,25 @@ while [[ $# -gt 0 ]]; do setup_conda_env ${conda_env_name} shift # past argument ;; + -u|--uv) + # use default name if not provided + if [ -z "$2" ]; then + echo "[INFO] Using default uv environment name: env_isaaclab" + uv_env_name="env_isaaclab" + else + echo "[INFO] Using uv environment name: $2" + uv_env_name=$2 + shift # past argument + fi + # setup the uv environment for Isaac Lab + setup_uv_env ${uv_env_name} + shift # past argument + ;; -f|--format) # reset the python path to avoid conflicts with pre-commit # this is needed because the pre-commit hooks are installed in a separate virtual environment # and it uses the system python to run the hooks - if [ -n "${CONDA_DEFAULT_ENV}" ]; then + if [ -n "${CONDA_DEFAULT_ENV}" ] || [ -n "${VIRTUAL_ENV}" ]; then cache_pythonpath=${PYTHONPATH} export PYTHONPATH="" fi @@ -343,7 +665,9 @@ while [[ $# -gt 0 ]]; do # check if pre-commit is installed if ! command -v pre-commit &>/dev/null; then echo "[INFO] Installing pre-commit..." - pip install pre-commit + pip_command=$(extract_pip_command) + ${pip_command} pre-commit + sudo apt-get install -y pre-commit fi # always execute inside the Isaac Lab directory echo "[INFO] Formatting the repository..." @@ -351,14 +675,19 @@ while [[ $# -gt 0 ]]; do pre-commit run --all-files cd - > /dev/null # set the python path back to the original value - if [ -n "${CONDA_DEFAULT_ENV}" ]; then + if [ -n "${CONDA_DEFAULT_ENV}" ] || [ -n "${VIRTUAL_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}" @@ -379,9 +708,10 @@ while [[ $# -gt 0 ]]; do -n|--new) # run the template generator script python_exe=$(extract_python_exe) + pip_command=$(extract_pip_command) shift # past argument echo "[INFO] Installing template dependencies..." - ${python_exe} -m pip install -q -r ${ISAACLAB_PATH}/tools/template/requirements.txt + ${pip_command} -q -r ${ISAACLAB_PATH}/tools/template/requirements.txt echo -e "\n[INFO] Running template generator...\n" ${python_exe} ${ISAACLAB_PATH}/tools/template/cli.py $@ # exit neatly @@ -391,7 +721,7 @@ while [[ $# -gt 0 ]]; do # run the python provided by isaacsim python_exe=$(extract_python_exe) shift # past argument - ${python_exe} ${ISAACLAB_PATH}/tools/run_all_tests.py $@ + ${python_exe} -m pytest ${ISAACLAB_PATH}/tools $@ # exit neatly break ;; @@ -416,9 +746,10 @@ while [[ $# -gt 0 ]]; do echo "[INFO] Building documentation..." # retrieve the python executable python_exe=$(extract_python_exe) + pip_command=$(extract_pip_command) # install pip packages cd ${ISAACLAB_PATH}/docs - ${python_exe} -m pip install -r requirements.txt > /dev/null + ${pip_command} -r requirements.txt > /dev/null # build the documentation ${python_exe} -m sphinx -b html -d _build/doctrees . _build/current # open the documentation @@ -432,7 +763,7 @@ while [[ $# -gt 0 ]]; do ;; -h|--help) print_help - exit 1 + exit 0 ;; *) # unknown option echo "[Error] Invalid argument provided: $1" diff --git a/pyproject.toml b/pyproject.toml index 0817ddd7c23d..f52c70333d47 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,67 +1,104 @@ -[tool.isort] - -py_version = 310 -line_length = 120 -group_by_package = true - -# Files to skip -skip_glob = ["docs/*", "logs/*", "_isaac_sim/*", ".vscode/*"] - -# Order of imports -sections = [ - "FUTURE", - "STDLIB", - "THIRDPARTY", - "ASSETS_FIRSTPARTY", - "FIRSTPARTY", - "EXTRA_FIRSTPARTY", - "TASK_FIRSTPARTY", - "LOCALFOLDER", +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +[tool.ruff] +line-length = 120 +target-version = "py310" + +# Exclude directories +extend-exclude = [ + "logs", + "_isaac_sim", + ".vscode", + "_*", + ".git", +] + +[tool.ruff.lint] +# Enable flake8 rules and other useful ones +select = [ + "E", # pycodestyle errors + "W", # pycodestyle warnings + "F", # pyflakes + "I", # isort + "UP", # pyupgrade + "C90", # mccabe complexity + # "D", # pydocstyle + "SIM", # flake8-simplify + "RET", # flake8-return ] -# Extra standard libraries considered as part of python (permissive licenses -extra_standard_library = [ - "numpy", - "h5py", - "open3d", - "torch", - "tensordict", - "bpy", - "matplotlib", - "gymnasium", - "gym", - "scipy", - "hid", - "yaml", - "prettytable", - "toml", - "trimesh", - "tqdm", - "torchvision", - "transformers", - "einops" # Needed for transformers, doesn't always auto-install +# Ignore specific rules (matching your flake8 config) +ignore = [ + "E402", # Module level import not at top of file + "D401", # First line should be in imperative mood + "RET504", # Unnecessary variable assignment before return statement + "RET505", # Unnecessary elif after return statement + "SIM102", # Use a single if-statement instead of nested if-statements + "SIM103", # Return the negated condition directly + "SIM108", # Use ternary operator instead of if-else statement + "SIM117", # Merge with statements for context managers + "SIM118", # Use {key} in {dict} instead of {key} in {dict}.keys() + "UP006", # Use 'dict' instead of 'Dict' type annotation + "UP018", # Unnecessary `float` call (rewrite as a literal) +] + +[tool.ruff.lint.per-file-ignores] +"__init__.py" = ["F401"] # Allow unused imports in __init__.py files + +[tool.ruff.lint.mccabe] +max-complexity = 30 + +[tool.ruff.lint.pydocstyle] +convention = "google" + +[tool.ruff.lint.isort] + +# Custom import sections with separate sections for each Isaac Lab extension +section-order = [ + "future", + "standard-library", + "third-party", + # Group omniverse extensions separately since they are run-time dependencies + # which are pulled in by Isaac Lab extensions + "omniverse-extensions", + # Group Isaac Lab extensions together since they are all part of the Isaac Lab project + "isaaclab", + "isaaclab-contrib", + "isaaclab-rl", + "isaaclab-mimic", + "isaaclab-tasks", + "isaaclab-assets", + # First-party is reserved for project templates + "first-party", + "local-folder", ] -# Imports from Isaac Sim and Omniverse -known_third_party = [ - "isaacsim.core.api", - "isaacsim.replicator.common", - "omni.replicator.core", + +[tool.ruff.lint.isort.sections] +# Define what belongs in each custom section + +"omniverse-extensions" = [ + "isaacsim", + "omni", "pxr", - "omni.kit.*", - "warp", "carb", + "usdrt", "Semantics", + "curobo", ] -# Imports from this repository -known_first_party = "isaaclab" -known_assets_firstparty = "isaaclab_assets" -known_extra_firstparty = [ - "isaaclab_rl", - "isaaclab_mimic", -] -known_task_firstparty = "isaaclab_tasks" -# Imports from the local folder -known_local_folder = "config" + +"isaaclab" = ["isaaclab"] +"isaaclab-assets" = ["isaaclab_assets"] +"isaaclab-contrib" = ["isaaclab_contrib"] +"isaaclab-rl" = ["isaaclab_rl"] +"isaaclab-mimic" = ["isaaclab_mimic"] +"isaaclab-tasks" = ["isaaclab_tasks"] + +[tool.ruff.format] + +docstring-code-format = true [tool.pyright] @@ -76,7 +113,7 @@ exclude = [ ] typeCheckingMode = "basic" -pythonVersion = "3.10" +pythonVersion = "3.11" pythonPlatform = "Linux" enableTypeIgnoreComments = true @@ -92,9 +129,14 @@ reportPrivateUsage = "warning" [tool.codespell] -skip = '*.usd,*.svg,*.png,_isaac_sim*,*.bib,*.css,*/_build' +skip = '*.usd,*.usda,*.usdz,*.svg,*.png,_isaac_sim*,*.bib,*.css,*/_build' quiet-level = 0 # the world list should always have words in lower case -ignore-words-list = "haa,slq,collapsable,buss" -# todo: this is hack to deal with incorrect spelling of "Environment" in the Isaac Sim grid world asset -exclude-file = "source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py" +ignore-words-list = "haa,slq,collapsable,buss,reacher,thirdparty" + + +[tool.pytest.ini_options] + +markers = [ + "isaacsim_ci: mark test to run in isaacsim ci", +] diff --git a/scripts/benchmarks/benchmark_cameras.py b/scripts/benchmarks/benchmark_cameras.py index 050d59f73ff9..a5d6a0c00267 100644 --- a/scripts/benchmarks/benchmark_cameras.py +++ b/scripts/benchmarks/benchmark_cameras.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -239,15 +239,13 @@ """Rest everything follows.""" -import gymnasium as gym -import numpy as np import random import time -import torch -import isaacsim.core.utils.prims as prim_utils +import gymnasium as gym +import numpy as np import psutil -from isaacsim.core.utils.stage import create_new_stage +import torch import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg @@ -286,7 +284,7 @@ def create_camera_base( if instantiate: # Create the necessary prims for idx in range(num_cams): - prim_utils.create_prim(f"/World/{name}_{idx:02d}", "Xform") + sim_utils.create_prim(f"/World/{name}_{idx:02d}", "Xform") if prim_path is None: prim_path = f"/World/{name}_.*/{name}" # If valid camera settings are provided, create the camera @@ -346,7 +344,7 @@ def create_ray_caster_cameras( ) -> RayCasterCamera | RayCasterCameraCfg | None: """Create the raycaster cameras; different configuration than Standard/Tiled camera""" for idx in range(num_cams): - prim_utils.create_prim(f"/World/RayCasterCamera_{idx:02d}/RayCaster", "Xform") + sim_utils.create_prim(f"/World/RayCasterCamera_{idx:02d}/RayCaster", "Xform") if num_cams > 0 and len(data_types) > 0 and height > 0 and width > 0: cam_cfg = RayCasterCameraCfg( @@ -446,7 +444,7 @@ def design_scene( scene_entities = {} # Xform to hold objects - prim_utils.create_prim("/World/Objects", "Xform") + sim_utils.create_prim("/World/Objects", "Xform") # Random objects for i in range(num_objects): # sample random position @@ -548,7 +546,6 @@ def get_utilization_percentages(reset: bool = False, max_values: list[float] = [ # GPU utilization using pynvml if torch.cuda.is_available(): - if args_cli.autotune: pynvml.nvmlInit() # Initialize NVML for i in range(torch.cuda.device_count()): @@ -665,7 +662,6 @@ def run_simulator( # Loop through all camera lists and their data_types for camera_list, data_types, label in zip(camera_lists, camera_data_types, labels): for cam_idx, camera in enumerate(camera_list): - if env is None: # No env, need to step cams manually # Only update the camera if it hasn't been updated as part of scene_entities.update ... camera.update(dt=sim.get_physics_dt()) @@ -851,7 +847,7 @@ def main(): cur_sys_util = analysis["system_utilization_analytics"] print("Triggering reset...") env.close() - create_new_stage() + sim_utils.create_new_stage() print("[INFO]: DONE! Feel free to CTRL + C Me ") print(f"[INFO]: If you've made it this far, you can likely simulate {cur_num_cams} {camera_name_prefix}") print("Keep in mind, this is without any training running on the GPU.") diff --git a/scripts/benchmarks/benchmark_load_robot.py b/scripts/benchmarks/benchmark_load_robot.py index 9176a8f41e9b..45d066162c8f 100644 --- a/scripts/benchmarks/benchmark_load_robot.py +++ b/scripts/benchmarks/benchmark_load_robot.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/scripts/benchmarks/benchmark_non_rl.py b/scripts/benchmarks/benchmark_non_rl.py index 92f48872083d..20d4221bc30e 100644 --- a/scripts/benchmarks/benchmark_non_rl.py +++ b/scripts/benchmarks/benchmark_non_rl.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -64,6 +64,7 @@ sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../..")) from isaaclab.utils.timer import Timer + from scripts.benchmarks.utils import ( log_app_start_time, log_python_imports_time, @@ -76,11 +77,12 @@ imports_time_begin = time.perf_counter_ns() +import os +from datetime import datetime + import gymnasium as gym import numpy as np -import os import torch -from datetime import datetime from isaaclab.envs import DirectMARLEnvCfg, DirectRLEnvCfg, ManagerBasedRLEnvCfg from isaaclab.utils.dict import print_dict @@ -113,6 +115,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 @@ -193,7 +203,7 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen log_total_start_time(benchmark, (task_startup_time_end - app_start_time_begin) / 1e6) log_runtime_step_times(benchmark, environment_step_times, compute_stats=True) - benchmark.stop() + benchmark.stop() # close the simulator env.close() diff --git a/scripts/benchmarks/benchmark_rlgames.py b/scripts/benchmarks/benchmark_rlgames.py index 73171b6a55b4..b3f20ecd02a4 100644 --- a/scripts/benchmarks/benchmark_rlgames.py +++ b/scripts/benchmarks/benchmark_rlgames.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -62,20 +62,20 @@ imports_time_begin = time.perf_counter_ns() -import gymnasium as gym import math import os import random -import torch from datetime import datetime +import gymnasium as gym +import torch from rl_games.common import env_configurations, vecenv from rl_games.common.algo_observer import IsaacAlgoObserver from rl_games.torch_runner import Runner 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 @@ -87,6 +87,7 @@ sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../..")) from isaaclab.utils.timer import Timer + from scripts.benchmarks.utils import ( log_app_start_time, log_python_imports_time, @@ -128,6 +129,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 +180,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"] @@ -248,7 +258,7 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen log_rl_policy_rewards(benchmark, log_data["rewards/iter"]) log_rl_policy_episode_lengths(benchmark, log_data["episode_lengths/iter"]) - benchmark.stop() + benchmark.stop() # close the simulator env.close() diff --git a/scripts/benchmarks/benchmark_rsl_rl.py b/scripts/benchmarks/benchmark_rsl_rl.py index 0d3ad75c3d76..8e3b4e132a59 100644 --- a/scripts/benchmarks/benchmark_rsl_rl.py +++ b/scripts/benchmarks/benchmark_rsl_rl.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -31,6 +31,9 @@ parser.add_argument("--task", type=str, default=None, help="Name of the task.") parser.add_argument("--seed", type=int, default=42, help="Seed used for the environment") parser.add_argument("--max_iterations", type=int, default=10, help="RL Policy training iterations.") +parser.add_argument( + "--distributed", action="store_true", default=False, help="Run training with multiple GPUs or nodes." +) parser.add_argument( "--benchmark_backend", type=str, @@ -63,16 +66,16 @@ imports_time_begin = time.perf_counter_ns() +from datetime import datetime + import gymnasium as gym import numpy as np import torch -from datetime import datetime - from rsl_rl.runners import OnPolicyRunner 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 @@ -88,6 +91,7 @@ from isaacsim.benchmark.services import BaseIsaacBenchmark from isaaclab.utils.timer import Timer + from scripts.benchmarks.utils import ( log_app_start_time, log_python_imports_time, @@ -126,8 +130,37 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen """Train with RSL-RL agent.""" # parse configuration benchmark.set_phase("loading", start_recording_frametime=False, start_recording_runtime=True) + # 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 + agent_cfg.max_iterations = ( + args_cli.max_iterations if args_cli.max_iterations is not None else agent_cfg.max_iterations + ) + + # 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 + # 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 + world_size = 1 + if args_cli.distributed: + env_cfg.sim.device = f"cuda:{app_launcher.local_rank}" + agent_cfg.device = f"cuda:{app_launcher.local_rank}" + + # set seed to have diversity in different threads + seed = agent_cfg.seed + app_launcher.local_rank + env_cfg.seed = seed + agent_cfg.seed = seed + world_rank = app_launcher.global_rank + world_size = int(os.getenv("WORLD_SIZE", 1)) # specify directory for logging experiments log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) @@ -181,42 +214,45 @@ 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") # run training runner.learn(num_learning_iterations=agent_cfg.max_iterations, init_at_random_ep_len=True) - benchmark.store_measurements() - - # parse tensorboard file stats - log_data = parse_tf_logs(log_dir) + if world_rank == 0: + benchmark.store_measurements() + + # parse tensorboard file stats + log_data = parse_tf_logs(log_dir) + + # prepare RL timing dict + collection_fps = ( + 1 + / (np.array(log_data["Perf/collection time"])) + * env.unwrapped.num_envs + * agent_cfg.num_steps_per_env + * world_size + ) + rl_training_times = { + "Collection Time": (np.array(log_data["Perf/collection time"]) / 1000).tolist(), + "Learning Time": (np.array(log_data["Perf/learning_time"]) / 1000).tolist(), + "Collection FPS": collection_fps.tolist(), + "Total FPS": log_data["Perf/total_fps"] * world_size, + } - # prepare RL timing dict - collection_fps = ( - 1 / (np.array(log_data["Perf/collection time"])) * env.unwrapped.num_envs * agent_cfg.num_steps_per_env - ) - rl_training_times = { - "Collection Time": (np.array(log_data["Perf/collection time"]) / 1000).tolist(), - "Learning Time": (np.array(log_data["Perf/learning_time"]) / 1000).tolist(), - "Collection FPS": collection_fps.tolist(), - "Total FPS": log_data["Perf/total_fps"], - } - - # log additional metrics to benchmark services - log_app_start_time(benchmark, (app_start_time_end - app_start_time_begin) / 1e6) - log_python_imports_time(benchmark, (imports_time_end - imports_time_begin) / 1e6) - log_task_start_time(benchmark, (task_startup_time_end - task_startup_time_begin) / 1e6) - log_scene_creation_time(benchmark, Timer.get_timer_info("scene_creation") * 1000) - log_simulation_start_time(benchmark, Timer.get_timer_info("simulation_start") * 1000) - log_total_start_time(benchmark, (task_startup_time_end - app_start_time_begin) / 1e6) - log_runtime_step_times(benchmark, rl_training_times, compute_stats=True) - log_rl_policy_rewards(benchmark, log_data["Train/mean_reward"]) - log_rl_policy_episode_lengths(benchmark, log_data["Train/mean_episode_length"]) - - benchmark.stop() + # log additional metrics to benchmark services + log_app_start_time(benchmark, (app_start_time_end - app_start_time_begin) / 1e6) + log_python_imports_time(benchmark, (imports_time_end - imports_time_begin) / 1e6) + log_task_start_time(benchmark, (task_startup_time_end - task_startup_time_begin) / 1e6) + log_scene_creation_time(benchmark, Timer.get_timer_info("scene_creation") * 1000) + log_simulation_start_time(benchmark, Timer.get_timer_info("simulation_start") * 1000) + log_total_start_time(benchmark, (task_startup_time_end - app_start_time_begin) / 1e6) + log_runtime_step_times(benchmark, rl_training_times, compute_stats=True) + log_rl_policy_rewards(benchmark, log_data["Train/mean_reward"]) + log_rl_policy_episode_lengths(benchmark, log_data["Train/mean_episode_length"]) + + benchmark.stop() # close the simulator env.close() diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py new file mode 100644 index 000000000000..8fca68312ab5 --- /dev/null +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -0,0 +1,512 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Benchmark script comparing XformPrimView vs PhysX RigidBodyView for transform operations. + +This script tests the performance of batched transform operations using: + +- Isaac Lab's XformPrimView (USD-based) +- Isaac Lab's XformPrimView (Fabric-based) +- PhysX RigidBodyView (PhysX tensors-based, as used in RigidObject) + +Note: + XformPrimView operates on USD attributes directly (useful for non-physics prims), + or on Fabric attributes when Fabric is enabled. + while RigidBodyView requires rigid body physics components and operates on PhysX tensors. + This benchmark helps understand the performance trade-offs between the two approaches. + +Usage: + # Basic benchmark + ./isaaclab.sh -p scripts/benchmarks/benchmark_view_comparison.py --num_envs 1024 --device cuda:0 --headless + + # With profiling enabled (for snakeviz visualization) + ./isaaclab.sh -p scripts/benchmarks/benchmark_view_comparison.py --num_envs 1024 --profile --headless + + # Then visualize with snakeviz: + snakeviz profile_results/xform_view_benchmark.prof + snakeviz profile_results/physx_view_benchmark.prof +""" + +from __future__ import annotations + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# parse the arguments +args_cli = argparse.Namespace() + +parser = argparse.ArgumentParser(description="Benchmark XformPrimView vs PhysX RigidBodyView performance.") + +parser.add_argument("--num_envs", type=int, default=1000, help="Number of environments to simulate.") +parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") +parser.add_argument( + "--profile", + action="store_true", + help="Enable profiling with cProfile. Results saved as .prof files for snakeviz visualization.", +) +parser.add_argument( + "--profile-dir", + type=str, + default="./profile_results", + help="Directory to save profile results. Default: ./profile_results", +) + +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import cProfile +import time + +import torch + +from isaacsim.core.simulation_manager import SimulationManager + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.sim.views import XformPrimView + + +@torch.no_grad() +def benchmark_view(view_type: str, num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: + """Benchmark the specified view class. + + Args: + view_type: Type of view to benchmark ("xform", "xform_fabric", or "physx"). + num_iterations: Number of iterations to run. + + Returns: + A tuple of (timing_results, computed_results) where: + - timing_results: Dictionary containing timing results for various operations + - computed_results: Dictionary containing the computed values for validation + """ + timing_results = {} + computed_results = {} + + # Setup scene + print(" Setting up scene") + # Clear stage + sim_utils.create_new_stage() + # Create simulation context + start_time = time.perf_counter() + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device, use_fabric=(view_type == "xform_fabric")) + sim = sim_utils.SimulationContext(sim_cfg) + stage = sim_utils.get_current_stage() + + print(f" Time taken to create simulation context: {time.perf_counter() - start_time:.4f} seconds") + + # create a rigid object + object_cfg = sim_utils.ConeCfg( + radius=0.15, + height=0.5, + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ) + # Create prims + for i in range(args_cli.num_envs): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", stage=stage, translation=(i * 2.0, 0.0, 0.0)) + object_cfg.func(f"/World/Env_{i}/Object", object_cfg, translation=(0.0, 0.0, 1.0)) + + # Play simulation + sim.reset() + + # Pattern to match all prims + pattern = "/World/Env_.*/Object" if view_type == "xform" else "/World/Env_*/Object" + print(f" Pattern: {pattern}") + + # Create view based on type + start_time = time.perf_counter() + if view_type == "xform": + view = XformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) + num_prims = view.count + view_name = "XformPrimView (USD)" + elif view_type == "xform_fabric": + if "cuda" not in args_cli.device: + raise ValueError("Fabric backend requires CUDA. Please use --device cuda:0 for this benchmark.") + view = XformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) + num_prims = view.count + view_name = "XformPrimView (Fabric)" + else: # physx + physics_sim_view = SimulationManager.get_physics_sim_view() + view = physics_sim_view.create_rigid_body_view(pattern) + num_prims = view.count + view_name = "PhysX RigidBodyView" + timing_results["init"] = time.perf_counter() - start_time + # prepare indices for benchmarking + all_indices = torch.arange(num_prims, device=args_cli.device) + + print(f" {view_name} managing {num_prims} prims") + + # Fabric is write-first: initialize it to match USD before benchmarking reads. + if view_type == "xform_fabric" and num_prims > 0: + init_positions = torch.zeros((num_prims, 3), dtype=torch.float32, device=args_cli.device) + init_positions[:, 0] = 2.0 * torch.arange(num_prims, device=args_cli.device, dtype=torch.float32) + init_positions[:, 2] = 1.0 + init_orientations = torch.tensor( + [[1.0, 0.0, 0.0, 0.0]] * num_prims, dtype=torch.float32, device=args_cli.device + ) + view.set_world_poses(init_positions, init_orientations) + + # Benchmark get_world_poses + start_time = time.perf_counter() + for _ in range(num_iterations): + if view_type in ("xform", "xform_fabric"): + positions, orientations = view.get_world_poses() + else: # physx + transforms = view.get_transforms() + positions = transforms[:, :3] + orientations = transforms[:, 3:7] + # Convert quaternion from xyzw to wxyz + orientations = math_utils.convert_quat(orientations, to="wxyz") + timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Store initial world poses + computed_results["initial_world_positions"] = positions.clone() + computed_results["initial_world_orientations"] = orientations.clone() + + # Benchmark set_world_poses + new_positions = positions.clone() + new_positions[:, 2] += 0.5 + start_time = time.perf_counter() + for _ in range(num_iterations): + if view_type in ("xform", "xform_fabric"): + view.set_world_poses(new_positions, orientations) + else: # physx + # Convert quaternion from wxyz to xyzw for PhysX + orientations_xyzw = math_utils.convert_quat(orientations, to="xyzw") + new_transforms = torch.cat([new_positions, orientations_xyzw], dim=-1) + view.set_transforms(new_transforms, indices=all_indices) + timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Get world poses after setting to verify + if view_type in ("xform", "xform_fabric"): + positions_after_set, orientations_after_set = view.get_world_poses() + else: # physx + transforms_after = view.get_transforms() + positions_after_set = transforms_after[:, :3] + orientations_after_set = math_utils.convert_quat(transforms_after[:, 3:7], to="wxyz") + computed_results["world_positions_after_set"] = positions_after_set.clone() + computed_results["world_orientations_after_set"] = orientations_after_set.clone() + + # close simulation + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + return timing_results, computed_results + + +def compare_results( + results_dict: dict[str, dict[str, torch.Tensor]], tolerance: float = 1e-4 +) -> dict[str, dict[str, dict[str, float]]]: + """Compare computed results across implementations. + + Args: + results_dict: Dictionary mapping implementation names to their computed values. + tolerance: Tolerance for numerical comparison. + + Returns: + Nested dictionary: {comparison_pair: {metric: {stats}}} + """ + comparison_stats = {} + impl_names = list(results_dict.keys()) + + # Compare each pair of implementations + for i, impl1 in enumerate(impl_names): + for impl2 in impl_names[i + 1 :]: + pair_key = f"{impl1}_vs_{impl2}" + comparison_stats[pair_key] = {} + + computed1 = results_dict[impl1] + computed2 = results_dict[impl2] + + for key in computed1.keys(): + if key not in computed2: + continue + + val1 = computed1[key] + val2 = computed2[key] + + # Skip zero tensors (not applicable tests) + if torch.all(val1 == 0) or torch.all(val2 == 0): + continue + + # Compute differences + diff = torch.abs(val1 - val2) + max_diff = torch.max(diff).item() + mean_diff = torch.mean(diff).item() + + # Check if within tolerance + all_close = torch.allclose(val1, val2, atol=tolerance, rtol=0) + + comparison_stats[pair_key][key] = { + "max_diff": max_diff, + "mean_diff": mean_diff, + "all_close": all_close, + } + + return comparison_stats + + +def print_comparison_results(comparison_stats: dict[str, dict[str, dict[str, float]]], tolerance: float): + """Print comparison results. + + Args: + comparison_stats: Nested dictionary containing comparison statistics. + tolerance: Tolerance used for comparison. + """ + for pair_key, pair_stats in comparison_stats.items(): + if not pair_stats: # Skip if no comparable results + continue + + # Format the pair key for display + impl1, impl2 = pair_key.split("_vs_") + display_impl1 = impl1.replace("_", " ").title() + display_impl2 = impl2.replace("_", " ").title() + comparison_title = f"{display_impl1} vs {display_impl2}" + + # Check if all results match + all_match = all(stats["all_close"] for stats in pair_stats.values()) + + if all_match: + # Compact output when everything matches + print("\n" + "=" * 100) + print(f"RESULT COMPARISON: {comparison_title}") + print("=" * 100) + print(f"โœ“ All computed values match within tolerance ({tolerance})") + print("=" * 100) + else: + # Detailed output when there are mismatches + print("\n" + "=" * 100) + print(f"RESULT COMPARISON: {comparison_title}") + print("=" * 100) + print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") + print("-" * 100) + + for key, stats in pair_stats.items(): + # Format the key for display + display_key = key.replace("_", " ").title() + match_str = "โœ“ Yes" if stats["all_close"] else "โœ— No" + + print(f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") + + print("=" * 100) + print(f"\nโœ— Some results differ beyond tolerance ({tolerance})") + print(f" This may indicate implementation differences between {display_impl1} and {display_impl2}") + + print() + + +def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num_iterations: int): + """Print benchmark results in a formatted table. + + Args: + results_dict: Dictionary mapping implementation names to their timing results. + num_prims: Number of prims tested. + num_iterations: Number of iterations run. + """ + print("\n" + "=" * 100) + print(f"BENCHMARK RESULTS: {num_prims} prims, {num_iterations} iterations") + print("=" * 100) + + impl_names = list(results_dict.keys()) + # Format names for display + display_names = [name.replace("_", " ").title() for name in impl_names] + + # Calculate column width + col_width = 20 + + # Print header + header = f"{'Operation':<30}" + for display_name in display_names: + header += f" {display_name + ' (ms)':<{col_width}}" + print(header) + print("-" * 100) + + # Print each operation + operations = [ + ("Initialization", "init"), + ("Get World Poses", "get_world_poses"), + ("Set World Poses", "set_world_poses"), + ] + + for op_name, op_key in operations: + row = f"{op_name:<30}" + for impl_name in impl_names: + impl_time = results_dict[impl_name].get(op_key, 0) * 1000 # Convert to ms + row += f" {impl_time:>{col_width - 1}.4f}" + print(row) + + print("=" * 100) + + # Calculate and print total time (excluding N/A operations) + total_row = f"{'Total Time':<30}" + for impl_name in impl_names: + if impl_name == "physx_view": + # Exclude local pose operations for PhysX + total_time = ( + results_dict[impl_name].get("init", 0) * 1000 + + results_dict[impl_name].get("get_world_poses", 0) * 1000 + + results_dict[impl_name].get("set_world_poses", 0) * 1000 + ) + else: + total_time = sum(results_dict[impl_name].values()) * 1000 + total_row += f" {total_time:>{col_width - 1}.4f}" + print(f"\n{total_row}") + + # Calculate speedups relative to XformPrimView (USD baseline) + if "xform_view" in impl_names: + print("\n" + "=" * 100) + print("SPEEDUP vs XformPrimView (USD)") + print("=" * 100) + print(f"{'Operation':<30}", end="") + for impl_name, display_name in zip(impl_names, display_names): + if impl_name != "xform_view": + print(f" {display_name + ' Speedup':<{col_width}}", end="") + print() + print("-" * 100) + + xform_results = results_dict["xform_view"] + for op_name, op_key in operations: + print(f"{op_name:<30}", end="") + xform_time = xform_results.get(op_key, 0) + for impl_name, display_name in zip(impl_names, display_names): + if impl_name != "xform_view": + impl_time = results_dict[impl_name].get(op_key, 0) + if xform_time > 0 and impl_time > 0: + speedup = impl_time / xform_time + print(f" {speedup:>{col_width - 1}.2f}x", end="") + else: + print(f" {'N/A':>{col_width}}", end="") + print() + + # Overall speedup (only world pose operations) + print("=" * 100) + print(f"{'Overall Speedup (World Ops)':<30}", end="") + total_xform = ( + xform_results.get("init", 0) + + xform_results.get("get_world_poses", 0) + + xform_results.get("set_world_poses", 0) + ) + for impl_name, display_name in zip(impl_names, display_names): + if impl_name != "xform_view": + total_impl = ( + results_dict[impl_name].get("init", 0) + + results_dict[impl_name].get("get_world_poses", 0) + + results_dict[impl_name].get("set_world_poses", 0) + ) + if total_xform > 0 and total_impl > 0: + overall_speedup = total_impl / total_xform + print(f" {overall_speedup:>{col_width - 1}.2f}x", end="") + else: + print(f" {'N/A':>{col_width}}", end="") + print() + + print("\n" + "=" * 100) + print("\nNotes:") + print(" - Times are averaged over all iterations") + print(" - Speedup = (Implementation time) / (XformPrimView USD time)") + print(" - Speedup > 1.0 means USD XformPrimView is faster") + print(" - Speedup < 1.0 means the implementation is faster than USD") + print(" - PhysX View requires rigid body physics components") + print(" - XformPrimView works with any Xform prim (physics or non-physics)") + print(" - PhysX View does not support local pose operations directly") + print() + + +def main(): + """Main benchmark function.""" + print("=" * 100) + print("View Comparison Benchmark - XformPrimView vs PhysX RigidBodyView") + print("=" * 100) + print("Configuration:") + print(f" Number of environments: {args_cli.num_envs}") + print(f" Iterations per test: {args_cli.num_iterations}") + print(f" Device: {args_cli.device}") + print(f" Profiling: {'Enabled' if args_cli.profile else 'Disabled'}") + if args_cli.profile: + print(f" Profile directory: {args_cli.profile_dir}") + print() + + # Create profile directory if profiling is enabled + if args_cli.profile: + import os + + os.makedirs(args_cli.profile_dir, exist_ok=True) + + # Dictionary to store all results + all_timing_results = {} + all_computed_results = {} + profile_files = {} + + # Implementations to benchmark + implementations = [ + ("xform_view", "XformPrimView (USD)", "xform"), + ("xform_fabric_view", "XformPrimView (Fabric)", "xform_fabric"), + ("physx_view", "PhysX RigidBodyView", "physx"), + ] + + # Benchmark each implementation + for impl_key, impl_name, view_type in implementations: + print(f"Benchmarking {impl_name}...") + + if args_cli.profile: + profiler = cProfile.Profile() + profiler.enable() + + timing, computed = benchmark_view(view_type=view_type, num_iterations=args_cli.num_iterations) + + if args_cli.profile: + profiler.disable() + profile_file = f"{args_cli.profile_dir}/{impl_key}_benchmark.prof" + profiler.dump_stats(profile_file) + profile_files[impl_key] = profile_file + print(f" Profile saved to: {profile_file}") + + all_timing_results[impl_key] = timing + all_computed_results[impl_key] = computed + + print(" Done!") + print() + + # Print timing results + print_results(all_timing_results, args_cli.num_envs, args_cli.num_iterations) + + # Compare computed results + print("\nComparing computed results across implementations...") + comparison_stats = compare_results(all_computed_results, tolerance=1e-4) + print_comparison_results(comparison_stats, tolerance=1e-4) + + # Print profiling instructions if enabled + if args_cli.profile: + print("\n" + "=" * 100) + print("PROFILING RESULTS") + print("=" * 100) + print("Profile files have been saved. To visualize with snakeviz, run:") + for impl_key, profile_file in profile_files.items(): + impl_display = impl_key.replace("_", " ").title() + print(f" # {impl_display}") + print(f" snakeviz {profile_file}") + print("\nAlternatively, use pstats to analyze in terminal:") + print(" python -m pstats ") + print("=" * 100) + print() + + # Clean up + sim_utils.SimulationContext.clear_instance() + + +if __name__ == "__main__": + main() diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py new file mode 100644 index 000000000000..94a8dc743610 --- /dev/null +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -0,0 +1,631 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Benchmark script comparing XformPrimView implementations across different APIs. + +This script tests the performance of batched transform operations using: +- Isaac Lab's XformPrimView implementation with USD backend +- Isaac Lab's XformPrimView implementation with Fabric backend +- Isaac Sim's XformPrimView implementation (legacy) +- Isaac Sim Experimental's XformPrim implementation (latest) + +Usage: + # Basic benchmark (all APIs) + ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --device cuda:0 --headless + + # With profiling enabled (for snakeviz visualization) + ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --profile --headless + + # Then visualize with snakeviz: + snakeviz profile_results/isaaclab_usd_benchmark.prof + snakeviz profile_results/isaaclab_fabric_benchmark.prof + snakeviz profile_results/isaacsim_benchmark.prof + snakeviz profile_results/isaacsim_exp_benchmark.prof +""" + +from __future__ import annotations + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# parse the arguments +args_cli = argparse.Namespace() + +parser = argparse.ArgumentParser(description="This script can help you benchmark the performance of XformPrimView.") + +parser.add_argument("--num_envs", type=int, default=100, help="Number of environments to simulate.") +parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") +parser.add_argument( + "--profile", + action="store_true", + help="Enable profiling with cProfile. Results saved as .prof files for snakeviz visualization.", +) +parser.add_argument( + "--profile-dir", + type=str, + default="./profile_results", + help="Directory to save profile results. Default: ./profile_results", +) + +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import cProfile +import time +from typing import Literal + +import torch + +from isaacsim.core.prims import XFormPrim as IsaacSimXformPrimView +from isaacsim.core.utils.extensions import enable_extension + +# compare against latest Isaac Sim implementation +enable_extension("isaacsim.core.experimental.prims") +from isaacsim.core.experimental.prims import XformPrim as IsaacSimExperimentalXformPrimView + +import isaaclab.sim as sim_utils +from isaaclab.sim.views import XformPrimView as IsaacLabXformPrimView + + +@torch.no_grad() +def benchmark_xform_prim_view( # noqa: C901 + api: Literal["isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric", "isaacsim-exp"], + num_iterations: int, +) -> tuple[dict[str, float], dict[str, torch.Tensor]]: + """Benchmark the Xform view class from Isaac Lab, Isaac Sim, or Isaac Sim Experimental. + + Args: + api: Which API to benchmark: + - "isaaclab-usd": Isaac Lab XformPrimView with USD backend + - "isaaclab-fabric": Isaac Lab XformPrimView with Fabric backend + - "isaacsim-usd": Isaac Sim legacy XformPrimView with USD (usd=True) + - "isaacsim-fabric": Isaac Sim legacy XformPrimView with Fabric (usd=False) + - "isaacsim-exp": Isaac Sim Experimental XformPrim + num_iterations: Number of iterations to run. + + Returns: + A tuple of (timing_results, computed_results) where: + - timing_results: Dictionary containing timing results for various operations + - computed_results: Dictionary containing the computed values for validation + """ + timing_results = {} + computed_results = {} + + # Setup scene + print(" Setting up scene") + # Clear stage + sim_utils.create_new_stage() + # Create simulation context + start_time = time.perf_counter() + sim_cfg = sim_utils.SimulationCfg( + dt=0.01, + device=args_cli.device, + use_fabric=api in ("isaaclab-fabric", "isaacsim-fabric"), + ) + sim = sim_utils.SimulationContext(sim_cfg) + stage = sim_utils.get_current_stage() + + print(f" Time taken to create simulation context: {time.perf_counter() - start_time} seconds") + + # Create prims + prim_paths = [] + for i in range(args_cli.num_envs): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", stage=stage, translation=(i * 2.0, 0.0, 1.0)) + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", stage=stage, translation=(0.0, 0.0, 0.0)) + prim_paths.append(f"/World/Env_{i}/Object") + # Play simulation + sim.reset() + + # Pattern to match all prims + pattern = "/World/Env_.*/Object" + print(f" Pattern: {pattern}") + + # Create view + start_time = time.perf_counter() + if api == "isaaclab-usd" or api == "isaaclab-fabric": + xform_view = IsaacLabXformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) + elif api == "isaacsim-usd": + xform_view = IsaacSimXformPrimView(pattern, reset_xform_properties=False, usd=True) + elif api == "isaacsim-fabric": + xform_view = IsaacSimXformPrimView(pattern, reset_xform_properties=False, usd=False) + elif api == "isaacsim-exp": + xform_view = IsaacSimExperimentalXformPrimView(pattern) + else: + raise ValueError(f"Invalid API: {api}") + timing_results["init"] = time.perf_counter() - start_time + + if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): + num_prims = xform_view.count + elif api == "isaacsim-exp": + num_prims = len(xform_view.prims) + print(f" XformView managing {num_prims} prims") + + # Benchmark get_world_poses + # Warmup call to initialize Fabric (if needed) - excluded from timing + positions, orientations = xform_view.get_world_poses() + + # Now time the actual iterations (steady-state performance) + start_time = time.perf_counter() + for _ in range(num_iterations): + positions, orientations = xform_view.get_world_poses() + + # Ensure tensors are torch tensors (do this AFTER timing) + if not isinstance(positions, torch.Tensor): + positions = torch.tensor(positions, dtype=torch.float32) + if not isinstance(orientations, torch.Tensor): + orientations = torch.tensor(orientations, dtype=torch.float32) + + timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Store initial world poses + computed_results["initial_world_positions"] = positions.clone() + computed_results["initial_world_orientations"] = orientations.clone() + + # Benchmark set_world_poses + new_positions = positions.clone() + new_positions[:, 2] += 0.1 + start_time = time.perf_counter() + for _ in range(num_iterations): + if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): + xform_view.set_world_poses(new_positions, orientations) + elif api == "isaacsim-exp": + xform_view.set_world_poses(new_positions.cpu().numpy(), orientations.cpu().numpy()) + timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Get world poses after setting to verify + positions_after_set, orientations_after_set = xform_view.get_world_poses() + if not isinstance(positions_after_set, torch.Tensor): + positions_after_set = torch.tensor(positions_after_set, dtype=torch.float32) + if not isinstance(orientations_after_set, torch.Tensor): + orientations_after_set = torch.tensor(orientations_after_set, dtype=torch.float32) + computed_results["world_positions_after_set"] = positions_after_set.clone() + computed_results["world_orientations_after_set"] = orientations_after_set.clone() + + # Benchmark get_local_poses + # Warmup call (though local poses use USD, so minimal overhead) + translations, orientations_local = xform_view.get_local_poses() + + # Now time the actual iterations + start_time = time.perf_counter() + for _ in range(num_iterations): + translations, orientations_local = xform_view.get_local_poses() + # Ensure tensors are torch tensors (do this AFTER timing) + if not isinstance(translations, torch.Tensor): + translations = torch.tensor(translations, dtype=torch.float32, device=args_cli.device) + if not isinstance(orientations_local, torch.Tensor): + orientations_local = torch.tensor(orientations_local, dtype=torch.float32, device=args_cli.device) + + timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Store initial local poses + computed_results["initial_local_translations"] = translations.clone() + computed_results["initial_local_orientations"] = orientations_local.clone() + + # Benchmark set_local_poses + new_translations = translations.clone() + new_translations[:, 2] += 0.1 + start_time = time.perf_counter() + for _ in range(num_iterations): + if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): + xform_view.set_local_poses(new_translations, orientations_local) + elif api == "isaacsim-exp": + xform_view.set_local_poses(new_translations.cpu().numpy(), orientations_local.cpu().numpy()) + timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Get local poses after setting to verify + translations_after_set, orientations_local_after_set = xform_view.get_local_poses() + if not isinstance(translations_after_set, torch.Tensor): + translations_after_set = torch.tensor(translations_after_set, dtype=torch.float32) + if not isinstance(orientations_local_after_set, torch.Tensor): + orientations_local_after_set = torch.tensor(orientations_local_after_set, dtype=torch.float32) + computed_results["local_translations_after_set"] = translations_after_set.clone() + computed_results["local_orientations_after_set"] = orientations_local_after_set.clone() + + # Benchmark combined get operation + # Warmup call (Fabric should already be initialized by now, but for consistency) + positions, orientations = xform_view.get_world_poses() + translations, local_orientations = xform_view.get_local_poses() + + # Now time the actual iterations + start_time = time.perf_counter() + for _ in range(num_iterations): + positions, orientations = xform_view.get_world_poses() + translations, local_orientations = xform_view.get_local_poses() + timing_results["get_both"] = (time.perf_counter() - start_time) / num_iterations + + # Benchmark interleaved set/get (realistic workflow pattern) + # Pre-convert tensors for experimental API to avoid conversion overhead in loop + if api == "isaacsim-exp": + new_positions_np = new_positions.cpu().numpy() + orientations_np = orientations + + # Warmup + if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): + xform_view.set_world_poses(new_positions, orientations) + positions, orientations = xform_view.get_world_poses() + elif api == "isaacsim-exp": + xform_view.set_world_poses(new_positions_np, orientations_np) + positions, orientations = xform_view.get_world_poses() + positions = torch.tensor(positions, dtype=torch.float32) + orientations = torch.tensor(orientations, dtype=torch.float32) + + # Now time the actual interleaved iterations + start_time = time.perf_counter() + for _ in range(num_iterations): + # Write then immediately read (common pattern: set pose, verify/query result) + if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): + xform_view.set_world_poses(new_positions, orientations) + positions, orientations = xform_view.get_world_poses() + elif api == "isaacsim-exp": + xform_view.set_world_poses(new_positions_np, orientations_np) + positions, orientations = xform_view.get_world_poses() + + timing_results["interleaved_world_set_get"] = (time.perf_counter() - start_time) / num_iterations + + # close simulation + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + return timing_results, computed_results + + +def compare_results( + results_dict: dict[str, dict[str, torch.Tensor]], tolerance: float = 1e-4 +) -> dict[str, dict[str, dict[str, float]]]: + """Compare computed results across multiple implementations. + + Only compares implementations using the same data path: + - USD implementations (isaaclab-usd, isaacsim-usd) are compared with each other + - Fabric implementations (isaaclab-fabric, isaacsim-fabric) are compared with each other + + This is because Fabric is designed for write-first workflows and may not match + USD reads on initialization. + + Args: + results_dict: Dictionary mapping API names to their computed values. + tolerance: Tolerance for numerical comparison. + + Returns: + Nested dictionary: {comparison_pair: {metric: {stats}}}, e.g., + {"isaaclab-usd_vs_isaacsim-usd": {"initial_world_positions": {"max_diff": 0.001, ...}}} + """ + comparison_stats = {} + + # Group APIs by their data path (USD vs Fabric) + usd_apis = [api for api in results_dict.keys() if "usd" in api and "fabric" not in api] + fabric_apis = [api for api in results_dict.keys() if "fabric" in api] + + # Compare within USD group + for i, api1 in enumerate(usd_apis): + for api2 in usd_apis[i + 1 :]: + pair_key = f"{api1}_vs_{api2}" + comparison_stats[pair_key] = {} + + computed1 = results_dict[api1] + computed2 = results_dict[api2] + + for key in computed1.keys(): + if key not in computed2: + print(f" Warning: Key '{key}' not found in {api2} results") + continue + + val1 = computed1[key] + val2 = computed2[key] + + # Compute differences + diff = torch.abs(val1 - val2) + max_diff = torch.max(diff).item() + mean_diff = torch.mean(diff).item() + + # Check if within tolerance + all_close = torch.allclose(val1, val2, atol=tolerance, rtol=0) + + comparison_stats[pair_key][key] = { + "max_diff": max_diff, + "mean_diff": mean_diff, + "all_close": all_close, + } + + # Compare within Fabric group + for i, api1 in enumerate(fabric_apis): + for api2 in fabric_apis[i + 1 :]: + pair_key = f"{api1}_vs_{api2}" + comparison_stats[pair_key] = {} + + computed1 = results_dict[api1] + computed2 = results_dict[api2] + + for key in computed1.keys(): + if key not in computed2: + print(f" Warning: Key '{key}' not found in {api2} results") + continue + + val1 = computed1[key] + val2 = computed2[key] + + # Compute differences + diff = torch.abs(val1 - val2) + max_diff = torch.max(diff).item() + mean_diff = torch.mean(diff).item() + + # Check if within tolerance + all_close = torch.allclose(val1, val2, atol=tolerance, rtol=0) + + comparison_stats[pair_key][key] = { + "max_diff": max_diff, + "mean_diff": mean_diff, + "all_close": all_close, + } + + return comparison_stats + + +def print_comparison_results(comparison_stats: dict[str, dict[str, dict[str, float]]], tolerance: float): + """Print comparison results across implementations. + + Args: + comparison_stats: Nested dictionary containing comparison statistics for each API pair. + tolerance: Tolerance used for comparison. + """ + if not comparison_stats: + print("\n" + "=" * 100) + print("RESULT COMPARISON") + print("=" * 100) + print("โ„น๏ธ No comparisons performed.") + print(" USD and Fabric implementations are not compared because Fabric uses a") + print(" write-first workflow and may not match USD reads on initialization.") + print("=" * 100) + print() + return + + for pair_key, pair_stats in comparison_stats.items(): + # Format the pair key for display (e.g., "isaaclab_vs_isaacsim" -> "Isaac Lab vs Isaac Sim") + api1, api2 = pair_key.split("_vs_") + display_api1 = api1.replace("-", " ").title() + display_api2 = api2.replace("-", " ").title() + comparison_title = f"{display_api1} vs {display_api2}" + + # Check if all results match + all_match = all(stats["all_close"] for stats in pair_stats.values()) + + if all_match: + # Compact output when everything matches + print("\n" + "=" * 100) + print(f"RESULT COMPARISON: {comparison_title}") + print("=" * 100) + print(f"โœ“ All computed values match within tolerance ({tolerance})") + print("=" * 100) + else: + # Detailed output when there are mismatches + print("\n" + "=" * 100) + print(f"RESULT COMPARISON: {comparison_title}") + print("=" * 100) + print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") + print("-" * 100) + + for key, stats in pair_stats.items(): + # Format the key for display + display_key = key.replace("_", " ").title() + match_str = "โœ“ Yes" if stats["all_close"] else "โœ— No" + + print(f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") + + print("=" * 100) + print(f"\nโœ— Some results differ beyond tolerance ({tolerance})") + + # Special note for Isaac Sim Fabric local pose bug + if "isaacsim-fabric" in pair_key and any("local_translations_after_set" in k for k in pair_stats.keys()): + if not pair_stats.get("local_translations_after_set", {}).get("all_close", True): + print("\n โš ๏ธ Known Issue: Isaac Sim Fabric has a bug where get_local_poses() returns stale") + print(" values after set_local_poses(). Isaac Lab Fabric correctly returns updated values.") + print(" This is a correctness issue in Isaac Sim's implementation, not Isaac Lab's.") + else: + print(f" This may indicate implementation differences between {display_api1} and {display_api2}") + + print() + + +def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num_iterations: int): + """Print benchmark results in a formatted table. + + Args: + results_dict: Dictionary mapping API names to their timing results. + num_prims: Number of prims tested. + num_iterations: Number of iterations run. + """ + print("\n" + "=" * 100) + print(f"BENCHMARK RESULTS: {num_prims} prims, {num_iterations} iterations") + print("=" * 100) + + api_names = list(results_dict.keys()) + # Format API names for display + display_names = [name.replace("-", " ").replace("_", " ").title() for name in api_names] + + # Calculate column width based on number of APIs + col_width = 20 + + # Print header + header = f"{'Operation':<25}" + for display_name in display_names: + header += f" {display_name + ' (ms)':<{col_width}}" + print(header) + print("-" * 100) + + # Print each operation + operations = [ + ("Initialization", "init"), + ("Get World Poses", "get_world_poses"), + ("Set World Poses", "set_world_poses"), + ("Get Local Poses", "get_local_poses"), + ("Set Local Poses", "set_local_poses"), + ("Get Both (World+Local)", "get_both"), + ("Interleaved World Setโ†’Get", "interleaved_world_set_get"), + ] + + for op_name, op_key in operations: + row = f"{op_name:<25}" + for api_name in api_names: + api_time = results_dict[api_name].get(op_key, 0) * 1000 # Convert to ms + row += f" {api_time:>{col_width - 1}.4f}" + print(row) + + print("=" * 100) + + # Calculate and print total time + total_row = f"{'Total Time':<25}" + for api_name in api_names: + total_time = sum(results_dict[api_name].values()) * 1000 + total_row += f" {total_time:>{col_width - 1}.4f}" + print(f"\n{total_row}") + + # Calculate speedups relative to Isaac Lab USD (baseline) + if "isaaclab-usd" in api_names: + print("\n" + "=" * 100) + print("SPEEDUP vs Isaac Lab USD (Baseline)") + print("=" * 100) + print(f"{'Operation':<25}", end="") + for api_name, display_name in zip(api_names, display_names): + if api_name != "isaaclab-usd": + print(f" {display_name:<{col_width}}", end="") + print() + print("-" * 100) + + isaaclab_usd_results = results_dict["isaaclab-usd"] + for op_name, op_key in operations: + print(f"{op_name:<25}", end="") + isaaclab_usd_time = isaaclab_usd_results.get(op_key, 0) + for api_name, display_name in zip(api_names, display_names): + if api_name != "isaaclab-usd": + api_time = results_dict[api_name].get(op_key, 0) + if isaaclab_usd_time > 0 and api_time > 0: + speedup = isaaclab_usd_time / api_time + print(f" {speedup:>{col_width - 1}.2f}x", end="") + else: + print(f" {'N/A':>{col_width}}", end="") + print() + + # Overall speedup + print("=" * 100) + print(f"{'Overall Speedup':<25}", end="") + total_isaaclab_usd = sum(isaaclab_usd_results.values()) + for api_name, display_name in zip(api_names, display_names): + if api_name != "isaaclab-usd": + total_api = sum(results_dict[api_name].values()) + if total_isaaclab_usd > 0 and total_api > 0: + overall_speedup = total_isaaclab_usd / total_api + print(f" {overall_speedup:>{col_width - 1}.2f}x", end="") + else: + print(f" {'N/A':>{col_width}}", end="") + print() + + print("\n" + "=" * 100) + print("\nNotes:") + print(" - Times are averaged over all iterations") + print(" - Speedup = (Isaac Lab USD time) / (Other API time)") + print(" - Speedup > 1.0 means the other API is faster than Isaac Lab USD") + print(" - Speedup < 1.0 means the other API is slower than Isaac Lab USD") + print() + + +def main(): + """Main benchmark function.""" + print("=" * 100) + print("XformPrimView Benchmark - Comparing Multiple APIs") + print("=" * 100) + print("Configuration:") + print(f" Number of environments: {args_cli.num_envs}") + print(f" Iterations per test: {args_cli.num_iterations}") + print(f" Device: {args_cli.device}") + print(f" Profiling: {'Enabled' if args_cli.profile else 'Disabled'}") + if args_cli.profile: + print(f" Profile directory: {args_cli.profile_dir}") + print() + + # Create profile directory if profiling is enabled + if args_cli.profile: + import os + + os.makedirs(args_cli.profile_dir, exist_ok=True) + + # Dictionary to store all results + all_timing_results = {} + all_computed_results = {} + profile_files = {} + + # APIs to benchmark + apis_to_test = [ + ("isaaclab-usd", "Isaac Lab XformPrimView (USD)"), + ("isaaclab-fabric", "Isaac Lab XformPrimView (Fabric)"), + ("isaacsim-usd", "Isaac Sim XformPrimView (USD)"), + ("isaacsim-fabric", "Isaac Sim XformPrimView (Fabric)"), + ("isaacsim-exp", "Isaac Sim Experimental XformPrim"), + ] + + # Benchmark each API + for api_key, api_name in apis_to_test: + print(f"Benchmarking {api_name}...") + + if args_cli.profile: + profiler = cProfile.Profile() + profiler.enable() + + # Cast api_key to Literal type for type checker + timing, computed = benchmark_xform_prim_view( + api=api_key, # type: ignore[arg-type] + num_iterations=args_cli.num_iterations, + ) + + if args_cli.profile: + profiler.disable() + profile_file = f"{args_cli.profile_dir}/{api_key.replace('-', '_')}_benchmark.prof" + profiler.dump_stats(profile_file) + profile_files[api_key] = profile_file + print(f" Profile saved to: {profile_file}") + + all_timing_results[api_key] = timing + all_computed_results[api_key] = computed + + print(" Done!") + print() + + # Print timing results + print_results(all_timing_results, args_cli.num_envs, args_cli.num_iterations) + + # Compare computed results + print("\nComparing computed results across APIs...") + comparison_stats = compare_results(all_computed_results, tolerance=1e-6) + print_comparison_results(comparison_stats, tolerance=1e-4) + + # Print profiling instructions if enabled + if args_cli.profile: + print("\n" + "=" * 100) + print("PROFILING RESULTS") + print("=" * 100) + print("Profile files have been saved. To visualize with snakeviz, run:") + for api_key, profile_file in profile_files.items(): + api_display = api_key.replace("-", " ").title() + print(f" # {api_display}") + print(f" snakeviz {profile_file}") + print("\nAlternatively, use pstats to analyze in terminal:") + print(" python -m pstats ") + print("=" * 100) + print() + + # Clean up + sim_utils.SimulationContext.clear_instance() + + +if __name__ == "__main__": + main() diff --git a/scripts/benchmarks/utils.py b/scripts/benchmarks/utils.py index a7b8835bc999..8401320f4e50 100644 --- a/scripts/benchmarks/utils.py +++ b/scripts/benchmarks/utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,9 +7,10 @@ import glob import os +from tensorboard.backend.event_processing import event_accumulator + from isaacsim.benchmark.services import BaseIsaacBenchmark from isaacsim.benchmark.services.metrics.measurements import DictMeasurement, ListMeasurement, SingleMeasurement -from tensorboard.backend.event_processing import event_accumulator def parse_tf_logs(log_dir: str): diff --git a/scripts/demos/arms.py b/scripts/demos/arms.py index 7d80ae54efb1..92bd4499d6d5 100644 --- a/scripts/demos/arms.py +++ b/scripts/demos/arms.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,8 +35,6 @@ import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils from isaaclab.assets import Articulation from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -86,7 +84,7 @@ def design_scene() -> tuple[dict, list[list[float]]]: origins = define_origins(num_origins=6, spacing=2.0) # Origin 1 with Franka Panda - prim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) + sim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) # -- Table cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd") cfg.func("/World/Origin1/Table", cfg, translation=(0.55, 0.0, 1.05)) @@ -96,7 +94,7 @@ def design_scene() -> tuple[dict, list[list[float]]]: franka_panda = Articulation(cfg=franka_arm_cfg) # Origin 2 with UR10 - prim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) + sim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) # -- Table cfg = sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0) @@ -108,7 +106,7 @@ def design_scene() -> tuple[dict, list[list[float]]]: ur10 = Articulation(cfg=ur10_cfg) # Origin 3 with Kinova JACO2 (7-Dof) arm - prim_utils.create_prim("/World/Origin3", "Xform", translation=origins[2]) + sim_utils.create_prim("/World/Origin3", "Xform", translation=origins[2]) # -- Table cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/ThorlabsTable/table_instanceable.usd") cfg.func("/World/Origin3/Table", cfg, translation=(0.0, 0.0, 0.8)) @@ -118,7 +116,7 @@ def design_scene() -> tuple[dict, list[list[float]]]: kinova_j2n7s300 = Articulation(cfg=kinova_arm_cfg) # Origin 4 with Kinova JACO2 (6-Dof) arm - prim_utils.create_prim("/World/Origin4", "Xform", translation=origins[3]) + sim_utils.create_prim("/World/Origin4", "Xform", translation=origins[3]) # -- Table cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/ThorlabsTable/table_instanceable.usd") cfg.func("/World/Origin4/Table", cfg, translation=(0.0, 0.0, 0.8)) @@ -128,7 +126,7 @@ def design_scene() -> tuple[dict, list[list[float]]]: kinova_j2n6s300 = Articulation(cfg=kinova_arm_cfg) # Origin 5 with Sawyer - prim_utils.create_prim("/World/Origin5", "Xform", translation=origins[4]) + sim_utils.create_prim("/World/Origin5", "Xform", translation=origins[4]) # -- Table cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd") cfg.func("/World/Origin5/Table", cfg, translation=(0.55, 0.0, 1.05)) @@ -138,7 +136,7 @@ def design_scene() -> tuple[dict, list[list[float]]]: kinova_gen3n7 = Articulation(cfg=kinova_arm_cfg) # Origin 6 with Kinova Gen3 (7-Dof) arm - prim_utils.create_prim("/World/Origin6", "Xform", translation=origins[5]) + sim_utils.create_prim("/World/Origin6", "Xform", translation=origins[5]) # -- Table cfg = sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0) diff --git a/scripts/demos/bin_packing.py b/scripts/demos/bin_packing.py new file mode 100644 index 000000000000..a43cbf199b25 --- /dev/null +++ b/scripts/demos/bin_packing.py @@ -0,0 +1,354 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Demonstration of randomized bin-packing with Isaac Lab. + +This script tiles multiple environments, spawns a configurable set of grocery +objects, and continuously randomizes their poses, velocities, mass properties, +and active/cached state to mimic a bin filling workflow. It showcases how to +use ``RigidObjectCollection`` utilities for bulk pose resets, cache management, +and out-of-bounds recovery inside an interactive simulation loop. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p scripts/demos/bin_packing.py --num_envs 32 + +""" + +from __future__ import annotations + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Demo usage of RigidObjectCollection through bin packing example") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to spawn.") +# 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 math + +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.assets import AssetBaseCfg, RigidObjectCfg, RigidObjectCollection, RigidObjectCollectionCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sim import SimulationContext +from isaaclab.utils import Timer, configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Scene Configuration +## + +# Layout and spawn counts. +MAX_NUM_OBJECTS = 24 # Hard cap on objects managed per environment (active + cached). +MAX_OBJECTS_PER_BIN = 24 # Maximum active objects we plan to fit inside the bin. +MIN_OBJECTS_PER_BIN = 1 # Lower bound for randomized active object count. +NUM_OBJECTS_PER_LAYER = 4 # Number of groceries spawned on each layer of the active stack. + +# Cached staging area and grid spacing. +CACHE_HEIGHT = 2.5 # Height (m) at which inactive groceries wait out of view. +ACTIVE_LAYER_SPACING = 0.1 # Vertical spacing (m) between layers inside the bin. +CACHE_SPACING = 0.25 # XY spacing (m) between cached groceries. + +# Bin dimensions and bounds. +BIN_DIMENSIONS = (0.2, 0.3, 0.15) # Physical size (m) of the storage bin. +BIN_XY_BOUND = ((-0.2, -0.3), (0.2, 0.3)) # Valid XY region (min/max) for active groceries. + +# Randomization ranges (radians for rotations, m/s and rad/s for velocities). +POSE_RANGE = {"roll": (-3.14, 3.14), "pitch": (-3.14, 3.14), "yaw": (-3.14, 3.14)} +VELOCITY_RANGE = {"roll": (-0.2, 1.0), "pitch": (-0.2, 1.0), "yaw": (-0.2, 1.0)} + +# Object layout configuration + +GROCERIES = { + "OBJECT_A": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/004_sugar_box.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4), + ), + "OBJECT_B": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/003_cracker_box.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4), + ), + "OBJECT_C": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/005_tomato_soup_can.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4), + ), + "OBJECT_D": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/006_mustard_bottle.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4), + ), +} + + +@configclass +class MultiObjectSceneCfg(InteractiveSceneCfg): + """Configuration for a multi-object scene.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # rigid object + object: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/Object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/KLT_Bin/small_KLT.usd", + scale=(2.0, 2.0, 2.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, solver_velocity_iteration_count=0, kinematic_enabled=True + ), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.15)), + ) + + groceries: RigidObjectCollectionCfg = RigidObjectCollectionCfg( + # Instantiate four grocery variants per layer and replicate across all layers in each environment. + rigid_objects={ + f"Object_{label}_Layer{layer}": RigidObjectCfg( + prim_path=f"/World/envs/env_.*/Object_{label}_Layer{layer}", + init_state=RigidObjectCfg.InitialStateCfg(pos=(x, y, 0.2 + (layer) * 0.2)), + spawn=GROCERIES.get(f"OBJECT_{label}"), + ) + for layer in range(MAX_NUM_OBJECTS // NUM_OBJECTS_PER_LAYER) + for label, (x, y) in zip(["A", "B", "C", "D"], [(-0.035, -0.1), (-0.035, 0.1), (0.035, 0.1), (0.035, -0.1)]) + } + ) + + +def reset_object_collections( + scene: InteractiveScene, asset_name: str, view_states: torch.Tensor, view_ids: torch.Tensor, noise: bool = False +) -> None: + """Apply states to a subset of a collection, with optional noise. + + Updates ``view_states`` in-place for ``view_ids`` and writes transforms/velocities + to the PhysX view for the collection ``asset_name``. When ``noise`` is True, adds + uniform perturbations to pose (XYZ + Euler) and velocities using ``POSE_RANGE`` and + ``VELOCITY_RANGE``. + + Args: + scene: Interactive scene containing the collection. + asset_name: Key in the scene (e.g., ``"groceries"``) for the RigidObjectCollection. + view_states: Flat tensor (N, 13) with [x, y, z, qx, qy, qz, qw, lin(3), ang(3)] in world frame. + view_ids: 1D tensor of indices into ``view_states`` to update. + noise: If True, apply pose and velocity noise before writing. + + Returns: + None: This function updates ``view_states`` and the underlying PhysX view in-place. + """ + rigid_object_collection: RigidObjectCollection = scene[asset_name] + sel_view_states = view_states[view_ids] + positions = sel_view_states[:, :3] + orientations = sel_view_states[:, 3:7] + # poses + if noise: + 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=scene.device) + samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(view_ids), 6), device=scene.device) + positions += samples[..., 0:3] + + # Compose new orientations by applying the sampled euler noise in quaternion space. + orientations_delta = math_utils.quat_from_euler_xyz(samples[..., 3], samples[..., 4], samples[..., 5]) + orientations = math_utils.convert_quat(orientations, to="wxyz") + orientations = math_utils.quat_mul(orientations, orientations_delta) + orientations = math_utils.convert_quat(orientations, to="xyzw") + + # velocities + new_velocities = sel_view_states[:, 7:13] + if noise: + 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=scene.device) + samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(view_ids), 6), device=scene.device) + new_velocities += samples + else: + new_velocities[:] = 0.0 + + view_states[view_ids, :7] = torch.concat((positions, orientations), dim=-1) + view_states[view_ids, 7:] = new_velocities + + rigid_object_collection.root_physx_view.set_transforms(view_states[:, :7], indices=view_ids) + rigid_object_collection.root_physx_view.set_velocities(view_states[:, 7:], indices=view_ids) + + +def build_grocery_defaults( + num_envs: int, + device: str = "cpu", +) -> tuple[torch.Tensor, torch.Tensor]: + """Create default active/cached spawn poses for all environments. + + - Active poses: stacked 3D grid over the bin with ``ACTIVE_LAYER_SPACING`` per layer. + - Cached poses: 2D grid at ``CACHE_HEIGHT`` to park inactive objects out of view. + + Args: + num_envs: Number of environments to tile the poses for. + device: Torch device for allocation (e.g., ``"cuda:0"`` or ``"cpu"``). + + Returns: + tuple[torch.Tensor, torch.Tensor]: Active and cached spawn poses, each shaped + ``(num_envs, M, 7)`` with ``[x, y, z, qx, qy, qz, qw]`` where ``M`` equals + ``MAX_NUM_OBJECTS``. + """ + + # The bin has a size of 0.2 x 0.3 x 0.15 m + bin_x_dim, bin_y_dim, bin_z_dim = BIN_DIMENSIONS + # First, we calculate the number of layers and objects per layer + num_layers = math.ceil(MAX_OBJECTS_PER_BIN / NUM_OBJECTS_PER_LAYER) + num_x_objects = math.ceil(math.sqrt(NUM_OBJECTS_PER_LAYER)) + num_y_objects = math.ceil(NUM_OBJECTS_PER_LAYER / num_x_objects) + total_objects = num_x_objects * num_y_objects * num_layers + # Then, we create a 3D grid that allows for IxJxN objects to be placed on top of the bin. + x = torch.linspace(-bin_x_dim * (2 / 6), bin_x_dim * (2 / 6), num_x_objects, device=device) + y = torch.linspace(-bin_y_dim * (2 / 6), bin_y_dim * (2 / 6), num_y_objects, device=device) + z = torch.linspace(0, ACTIVE_LAYER_SPACING * (num_layers - 1), num_layers, device=device) + bin_z_dim * 2 + grid_z, grid_y, grid_x = torch.meshgrid(z, y, x, indexing="ij") # Note Z first, this stacks the layers. + # Using this grid plus a reference quaternion, create the poses for the groceries to be spawned above the bin. + ref_quat = torch.tensor([[0.0, 0.0, 0.0, 1.0]], device=device).repeat(total_objects, 1) + positions = torch.stack((grid_x.flatten(), grid_y.flatten(), grid_z.flatten()), dim=-1) + poses = torch.cat((positions, ref_quat), dim=-1) + # Duplicate across environments, cap at max_num_objects + active_spawn_poses = poses.unsqueeze(0).repeat(num_envs, 1, 1)[:, :MAX_NUM_OBJECTS, :] + + # We'll also create a buffer for the cached groceries. They'll be spawned below the bin so they can't be seen. + num_x_objects = math.ceil(math.sqrt(MAX_NUM_OBJECTS)) + num_y_objects = math.ceil(MAX_NUM_OBJECTS / num_x_objects) + # We create a XY grid only and fix the Z height for the cache. + x = CACHE_SPACING * torch.arange(num_x_objects, device=device) + y = CACHE_SPACING * torch.arange(num_y_objects, device=device) + grid_y, grid_x = torch.meshgrid(y, x, indexing="ij") + grid_z = CACHE_HEIGHT * torch.ones_like(grid_x) + # We can then create the poses for the cached groceries. + ref_quat = torch.tensor([[1.0, 0.0, 0.0, 0.0]], device=device).repeat(num_x_objects * num_y_objects, 1) + positions = torch.stack((grid_x.flatten(), grid_y.flatten(), grid_z.flatten()), dim=-1) + poses = torch.cat((positions, ref_quat), dim=-1) + # Duplicate across environments, cap at max_num_objects + cached_spawn_poses = poses.unsqueeze(0).repeat(num_envs, 1, 1)[:, :MAX_NUM_OBJECTS, :] + + return active_spawn_poses, cached_spawn_poses + + +## +# Simulation Loop +## + + +def run_simulator(sim: SimulationContext, scene: InteractiveScene) -> None: + """Runs the simulation loop that coordinates spawn randomization and stepping. + + Returns: + None: The simulator side-effects are applied through ``scene`` and ``sim``. + """ + # Extract scene entities + # note: we only do this here for readability. + groceries: RigidObjectCollection = scene["groceries"] + num_objects = groceries.num_objects + num_envs = scene.num_envs + device = scene.device + view_indices = torch.arange(num_envs * num_objects, device=device) + default_state_w = groceries.data.default_object_state.clone() + default_state_w[..., :3] = default_state_w[..., :3] + scene.env_origins.unsqueeze(1) + # Define simulation stepping + sim_dt = sim.get_physics_dt() + count = 0 + + # Pre-compute canonical spawn poses for each object both inside the bin and in the cache. + active_spawn_poses, cached_spawn_poses = build_grocery_defaults(num_envs, device) + # Offset poses into each environment's world frame. + active_spawn_poses[..., :3] += scene.env_origins.view(-1, 1, 3) + cached_spawn_poses[..., :3] += scene.env_origins.view(-1, 1, 3) + active_spawn_poses = groceries.reshape_data_to_view(active_spawn_poses) + cached_spawn_poses = groceries.reshape_data_to_view(cached_spawn_poses) + spawn_w = groceries.reshape_data_to_view(default_state_w).clone() + + groceries_mask_helper = torch.arange(num_objects * num_envs, device=device) % num_objects + # Precompute a helper mask to toggle objects between active and cached sets. + # Precompute XY bounds [[x_min,y_min],[x_max,y_max]] + bounds_xy = torch.as_tensor(BIN_XY_BOUND, device=device, dtype=spawn_w.dtype) + # Simulation loop + while simulation_app.is_running(): + # Reset + if count % 250 == 0: + # reset counter + count = 0 + # Randomly choose how many groceries stay active in each environment. + num_active_groceries = torch.randint(MIN_OBJECTS_PER_BIN, num_objects, (num_envs, 1), device=device) + groceries_mask = (groceries_mask_helper.view(num_envs, -1) < num_active_groceries).view(-1, 1) + spawn_w[:, :7] = cached_spawn_poses * (~groceries_mask) + active_spawn_poses * groceries_mask + # Retrieve positions + with Timer("[INFO] Time to reset scene: "): + reset_object_collections(scene, "groceries", spawn_w, view_indices[~groceries_mask.view(-1)]) + reset_object_collections(scene, "groceries", spawn_w, view_indices[groceries_mask.view(-1)], noise=True) + # Vary the mass and gravity settings so cached objects stay parked. + random_masses = torch.rand(groceries.num_instances * num_objects, device=device) * 0.2 + 0.2 + groceries.root_physx_view.set_masses(random_masses.cpu(), view_indices.cpu()) + groceries.root_physx_view.set_disable_gravities((~groceries_mask).cpu(), indices=view_indices.cpu()) + scene.reset() + + # Write data to sim + scene.write_data_to_sim() + # Perform step + sim.step() + + # Bring out-of-bounds objects back to the bin in one pass. + xy = groceries.reshape_data_to_view(groceries.data.object_pos_w - scene.env_origins.unsqueeze(1))[:, :2] + out_bound = torch.nonzero(~((xy >= bounds_xy[0]) & (xy <= bounds_xy[1])).all(dim=1), as_tuple=False).flatten() + if out_bound.numel(): + # Teleport stray objects back into the active stack to keep the bin tidy. + reset_object_collections(scene, "groceries", spawn_w, out_bound) + # Increment counter + count += 1 + # Update buffers + scene.update(sim_dt) + + +def main() -> None: + """Main function. + + Returns: + None: The function drives the simulation for its side-effects. + """ + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) + sim = SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view((2.5, 0.0, 4.0), (0.0, 0.0, 2.0)) + + # Design scene + scene_cfg = MultiObjectSceneCfg(num_envs=args_cli.num_envs, env_spacing=1.0, replicate_physics=False) + with Timer("[INFO] Time to create scene: "): + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + # run the main execution + main() + # close sim app + simulation_app.close() diff --git a/scripts/demos/bipeds.py b/scripts/demos/bipeds.py index a45d9511a908..91421c105ff6 100644 --- a/scripts/demos/bipeds.py +++ b/scripts/demos/bipeds.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -16,7 +16,6 @@ """Launch Isaac Sim Simulator first.""" import argparse -import torch from isaaclab.app import AppLauncher @@ -33,6 +32,8 @@ """Rest everything follows.""" +import torch + import isaaclab.sim as sim_utils from isaaclab.assets import Articulation from isaaclab.sim import SimulationContext @@ -55,11 +56,13 @@ def design_scene(sim: sim_utils.SimulationContext) -> tuple[list, torch.Tensor]: cfg.func("/World/Light", cfg) # Define origins - origins = torch.tensor([ - [0.0, -1.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - ]).to(device=sim.device) + origins = torch.tensor( + [ + [0.0, -1.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + ] + ).to(device=sim.device) # Robots cassie = Articulation(CASSIE_CFG.replace(prim_path="/World/Cassie")) diff --git a/scripts/demos/deformables.py b/scripts/demos/deformables.py index 13badc89aa4d..9b9a962c26d5 100644 --- a/scripts/demos/deformables.py +++ b/scripts/demos/deformables.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -31,8 +31,9 @@ """Rest everything follows.""" -import numpy as np import random + +import numpy as np import torch import tqdm diff --git a/scripts/demos/h1_locomotion.py b/scripts/demos/h1_locomotion.py index c849f3d372c1..734ca506a46f 100644 --- a/scripts/demos/h1_locomotion.py +++ b/scripts/demos/h1_locomotion.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -41,20 +41,22 @@ simulation_app = app_launcher.app """Rest everything follows.""" + import torch +from rsl_rl.runners import OnPolicyRunner import carb import omni from omni.kit.viewport.utility import get_viewport_from_window_name from omni.kit.viewport.utility.camera_state import ViewportCameraState from pxr import Gf, Sdf -from rsl_rl.runners import OnPolicyRunner from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.sim.utils.stage import get_current_stage from isaaclab.utils.math import quat_apply -from isaaclab.utils.pretrained_checkpoint import get_published_pretrained_checkpoint from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper +from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint from isaaclab_tasks.manager_based.locomotion.velocity.config.h1.rough_env_cfg import H1RoughEnvCfg_PLAY @@ -86,6 +88,7 @@ def __init__(self): # create envionrment env_cfg = H1RoughEnvCfg_PLAY() env_cfg.scene.num_envs = 25 + env_cfg.episode_length_s = 1000000 env_cfg.curriculum = None env_cfg.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) env_cfg.commands.base_velocity.ranges.heading = (-1.0, 1.0) @@ -109,7 +112,7 @@ def __init__(self): def create_camera(self): """Creates a camera to be used for third-person view.""" - stage = omni.usd.get_context().get_stage() + stage = get_current_stage() self.viewport = get_viewport_from_window_name("Viewport") # Create camera self.camera_path = "/World/Camera" @@ -143,7 +146,7 @@ def _on_keyboard_event(self, event): if event.type == carb.input.KeyboardEventType.KEY_PRESS: # Arrow keys map to pre-defined command vectors to control navigation of robot if event.input.name in self._key_to_control: - if self._selected_id: + if self._selected_id is not None: self.commands[self._selected_id] = self._key_to_control[event.input.name] # Escape key exits out of the current selected robot view elif event.input.name == "ESCAPE": @@ -157,7 +160,7 @@ def _on_keyboard_event(self, event): self.viewport.set_active_camera(self.camera_path) # On key release, the robot stops moving elif event.type == carb.input.KeyboardEventType.KEY_RELEASE: - if self._selected_id: + if self._selected_id is not None: self.commands[self._selected_id] = self._key_to_control["ZEROS"] def update_selected_object(self): diff --git a/scripts/demos/hands.py b/scripts/demos/hands.py index 1d360c923ebb..a0fa04e0fbfd 100644 --- a/scripts/demos/hands.py +++ b/scripts/demos/hands.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,8 +35,6 @@ import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils from isaaclab.assets import Articulation @@ -76,12 +74,12 @@ def design_scene() -> tuple[dict, list[list[float]]]: origins = define_origins(num_origins=2, spacing=0.5) # Origin 1 with Allegro Hand - prim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) + sim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) # -- Robot allegro = Articulation(ALLEGRO_HAND_CFG.replace(prim_path="/World/Origin1/Robot")) # Origin 2 with Shadow Hand - prim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) + sim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) # -- Robot shadow_hand = Articulation(SHADOW_HAND_CFG.replace(prim_path="/World/Origin2/Robot")) diff --git a/scripts/demos/haply_teleoperation.py b/scripts/demos/haply_teleoperation.py new file mode 100644 index 000000000000..b6d02900baf4 --- /dev/null +++ b/scripts/demos/haply_teleoperation.py @@ -0,0 +1,361 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Demonstration of Haply device teleoperation with a robotic arm. + +This script demonstrates how to use a Haply device (Inverse3 + VerseGrip) to +teleoperate a robotic arm in Isaac Lab. The Haply provides: +- Position tracking from the Inverse3 device +- Orientation and button inputs from the VerseGrip device +- Force feedback + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p scripts/demos/haply_teleoperation.py + + # With custom WebSocket URI + ./isaaclab.sh -p scripts/demos/haply_teleoperation.py --websocket_uri ws://localhost:10001 + + # With sensitivity adjustment + ./isaaclab.sh -p scripts/demos/haply_teleoperation.py --pos_sensitivity 2.0 + +Prerequisites: + 1. Install websockets package: pip install websockets + 2. Have Haply SDK running and accessible via WebSocket + 3. Connect Inverse3 and VerseGrip devices +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Demonstration of Haply device teleoperation with Isaac Lab.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.") +parser.add_argument( + "--websocket_uri", + type=str, + default="ws://localhost:10001", + help="WebSocket URI for Haply SDK connection.", +) +parser.add_argument( + "--pos_sensitivity", + type=float, + default=1.0, + help="Position sensitivity scaling factor.", +) + +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, AssetBaseCfg, RigidObject, RigidObjectCfg +from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg +from isaaclab.devices import HaplyDevice, HaplyDeviceCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import ContactSensor, ContactSensorCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from isaaclab_assets import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + +# Workspace mapping constants +HAPLY_Z_OFFSET = 0.35 +WORKSPACE_LIMITS = { + "x": (0.1, 0.9), + "y": (-0.50, 0.50), + "z": (1.05, 1.85), +} + + +def apply_haply_to_robot_mapping( + haply_pos: np.ndarray | torch.Tensor, + haply_initial_pos: np.ndarray | list, + robot_initial_pos: np.ndarray | torch.Tensor, +) -> np.ndarray: + """Apply coordinate mapping from Haply workspace to Franka Panda end-effector. + + Uses absolute position control: robot position = robot_initial_pos + haply_pos (transformed) + + Args: + haply_pos: Current Haply absolute position [x, y, z] in meters + haply_initial_pos: Haply's zero reference position [x, y, z] + robot_initial_pos: Base offset for robot end-effector + + Returns: + robot_pos: Target position for robot EE in world frame [x, y, z] + + """ + # Convert to numpy + if isinstance(haply_pos, torch.Tensor): + haply_pos = haply_pos.cpu().numpy() + if isinstance(robot_initial_pos, torch.Tensor): + robot_initial_pos = robot_initial_pos.cpu().numpy() + + haply_delta = haply_pos - haply_initial_pos + + # Coordinate system mapping: Haply (X, Y, Z) -> Robot (-Y, X, Z-offset) + robot_offset = np.array([-haply_delta[1], haply_delta[0], haply_delta[2] - HAPLY_Z_OFFSET]) + robot_pos = robot_initial_pos + robot_offset + + # Apply workspace limits for safety + robot_pos[0] = np.clip(robot_pos[0], WORKSPACE_LIMITS["x"][0], WORKSPACE_LIMITS["x"][1]) + robot_pos[1] = np.clip(robot_pos[1], WORKSPACE_LIMITS["y"][0], WORKSPACE_LIMITS["y"][1]) + robot_pos[2] = np.clip(robot_pos[2], WORKSPACE_LIMITS["z"][0], WORKSPACE_LIMITS["z"][1]) + + return robot_pos + + +@configclass +class FrankaHaplySceneCfg(InteractiveSceneCfg): + """Configuration for Franka scene with Haply teleoperation and contact sensors.""" + + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + ) + + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)), + ) + + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", + scale=(1.0, 1.0, 1.0), + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.50, 0.0, 1.05), rot=(0.707, 0, 0, 0.707)), + ) + + robot: Articulation = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + robot.init_state.pos = (-0.02, 0.0, 1.05) + robot.spawn.activate_contact_sensors = True + + cube = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.06, 0.06, 0.06), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.5, dynamic_friction=0.5), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.8, 0.2), metallic=0.2), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.60, 0.00, 1.15)), + ) + + left_finger_contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_leftfinger", + update_period=0.0, + history_length=3, + debug_vis=True, + track_pose=True, + ) + + right_finger_contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_rightfinger", + update_period=0.0, + history_length=3, + debug_vis=True, + track_pose=True, + ) + + +def run_simulator( + sim: sim_utils.SimulationContext, + scene: InteractiveScene, + haply_device: HaplyDevice, +): + """Runs the simulation loop with Haply teleoperation.""" + sim_dt = sim.get_physics_dt() + count = 1 + + robot: Articulation = scene["robot"] + cube: RigidObject = scene["cube"] + left_finger_sensor: ContactSensor = scene["left_finger_contact_sensor"] + right_finger_sensor: ContactSensor = scene["right_finger_contact_sensor"] + + ee_body_name = "panda_hand" + ee_body_idx = robot.body_names.index(ee_body_name) + + joint_pos = robot.data.default_joint_pos.clone() + joint_pos[0, :7] = torch.tensor([0.0, -0.569, 0.0, -2.81, 0.0, 3.037, 0.741], device=robot.device) + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_pos, joint_vel) + + for _ in range(10): + scene.write_data_to_sim() + sim.step() + scene.update(sim_dt) + + # Initialize the position of franka + robot_initial_pos = robot.data.body_pos_w[0, ee_body_idx].cpu().numpy() + haply_initial_pos = np.array([0.0, 0.0, 0.0], dtype=np.float32) + + ik_controller_cfg = DifferentialIKControllerCfg( + command_type="position", + use_relative_mode=False, + ik_method="dls", + ik_params={"lambda_val": 0.05}, + ) + + # IK joints control arms, buttons control ee rotation and gripper open/close + arm_joint_names = [ + "panda_joint1", + "panda_joint2", + "panda_joint3", + "panda_joint4", + "panda_joint5", + "panda_joint6", + ] + arm_joint_indices = [robot.joint_names.index(name) for name in arm_joint_names] + + # Initialize IK controller + ik_controller = DifferentialIKController(cfg=ik_controller_cfg, num_envs=scene.num_envs, device=sim.device) + initial_ee_quat = robot.data.body_quat_w[:, ee_body_idx] + ik_controller.set_command(command=torch.zeros(scene.num_envs, 3, device=sim.device), ee_quat=initial_ee_quat) + + prev_button_a = False + prev_button_b = False + prev_button_c = False + gripper_target = 0.04 + + # Initialize the rotation of franka end-effector + ee_rotation_angle = robot.data.joint_pos[0, 6].item() + rotation_step = np.pi / 3 + + print("\n[INFO] Teleoperation ready!") + print(" Move handler: Control pose of the end-effector") + print(" Button A: Open | Button B: Close | Button C: Rotate EE (60ยฐ)\n") + + while simulation_app.is_running(): + if count % 10000 == 0: + count = 1 + root_state = robot.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) + + joint_pos = robot.data.default_joint_pos.clone() + joint_pos[0, :7] = torch.tensor([0.0, -0.569, 0.0, -2.81, 0.0, 3.037, 0.741], device=robot.device) + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_pos, joint_vel) + + cube_state = cube.data.default_root_state.clone() + cube_state[:, :3] += scene.env_origins + cube.write_root_pose_to_sim(cube_state[:, :7]) + cube.write_root_velocity_to_sim(cube_state[:, 7:]) + + scene.reset() + haply_device.reset() + ik_controller.reset() + print("[INFO]: Resetting robot state...") + + # Get the data from Haply device + haply_data = haply_device.advance() + + haply_pos = haply_data[:3] + button_a = haply_data[7].item() > 0.5 + button_b = haply_data[8].item() > 0.5 + button_c = haply_data[9].item() > 0.5 + + if button_a and not prev_button_a: + gripper_target = 0.04 # Open gripper + if button_b and not prev_button_b: + gripper_target = 0.0 # Close gripper + if button_c and not prev_button_c: + joint_7_limit = 3.0 + ee_rotation_angle += rotation_step + + if ee_rotation_angle > joint_7_limit: + ee_rotation_angle = -joint_7_limit + (ee_rotation_angle - joint_7_limit) + elif ee_rotation_angle < -joint_7_limit: + ee_rotation_angle = joint_7_limit + (ee_rotation_angle + joint_7_limit) + + prev_button_a = button_a + prev_button_b = button_b + prev_button_c = button_c + + # Compute IK + target_pos = apply_haply_to_robot_mapping( + haply_pos, + haply_initial_pos, + robot_initial_pos, + ) + + target_pos_tensor = torch.tensor(target_pos, dtype=torch.float32, device=sim.device).unsqueeze(0) + + current_joint_pos = robot.data.joint_pos[:, arm_joint_indices] + ee_pos_w = robot.data.body_pos_w[:, ee_body_idx] + ee_quat_w = robot.data.body_quat_w[:, ee_body_idx] + + # get jacobian to IK controller + jacobian = robot.root_physx_view.get_jacobians()[:, ee_body_idx, :, arm_joint_indices] + ik_controller.set_command(command=target_pos_tensor, ee_quat=ee_quat_w) + joint_pos_des = ik_controller.compute(ee_pos_w, ee_quat_w, jacobian, current_joint_pos) + + joint_pos_target = robot.data.joint_pos[0].clone() + + # Update joints: 6 from IK + 1 from button control (correct by design) + joint_pos_target[arm_joint_indices] = joint_pos_des[0] # panda_joint1-6 from IK + joint_pos_target[6] = ee_rotation_angle # panda_joint7 - end-effector rotation (button C) + joint_pos_target[[-2, -1]] = gripper_target # gripper + + robot.set_joint_position_target(joint_pos_target.unsqueeze(0)) + + for _ in range(5): + scene.write_data_to_sim() + sim.step() + + scene.update(sim_dt) + count += 1 + + # get contact forces and apply force feedback + left_finger_forces = left_finger_sensor.data.net_forces_w[0, 0] + right_finger_forces = right_finger_sensor.data.net_forces_w[0, 0] + total_contact_force = (left_finger_forces + right_finger_forces) * 0.5 + haply_device.push_force(forces=total_contact_force.unsqueeze(0), position=torch.tensor([0])) + + +def main(): + """Main function to set up and run the Haply teleoperation demo.""" + sim_cfg = sim_utils.SimulationCfg(device=args_cli.device, dt=1 / 200) + sim = sim_utils.SimulationContext(sim_cfg) + + # set the simulation view + sim.set_camera_view([1.6, 1.0, 1.70], [0.4, 0.0, 1.0]) + + scene_cfg = FrankaHaplySceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + + # Create Haply device + haply_cfg = HaplyDeviceCfg( + websocket_uri=args_cli.websocket_uri, + pos_sensitivity=args_cli.pos_sensitivity, + sim_device=args_cli.device, + limit_force=2.0, + ) + haply_device = HaplyDevice(cfg=haply_cfg) + print(f"[INFO] Haply connected: {args_cli.websocket_uri}") + + sim.reset() + + run_simulator(sim, scene, haply_device) + + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/scripts/demos/markers.py b/scripts/demos/markers.py index 893d67c82c1e..6152dcf5226f 100644 --- a/scripts/demos/markers.py +++ b/scripts/demos/markers.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/scripts/demos/multi_asset.py b/scripts/demos/multi_asset.py index e0d1055c0e45..d104eb161d38 100644 --- a/scripts/demos/multi_asset.py +++ b/scripts/demos/multi_asset.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -37,7 +37,6 @@ import random -import omni.usd from pxr import Gf, Sdf import isaaclab.sim as sim_utils @@ -52,6 +51,7 @@ ) from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import SimulationContext +from isaaclab.sim.utils.stage import get_current_stage from isaaclab.utils import Timer, configclass from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR @@ -69,8 +69,8 @@ def randomize_shape_color(prim_path_expr: str): """Randomize the color of the geometry.""" - # acquire stage - stage = omni.usd.get_context().get_stage() + # get stage handle + stage = get_current_stage() # resolve prim paths for spawning and cloning prim_paths = sim_utils.find_matching_prim_paths(prim_path_expr) # manually clone prims if the source prim path is a regex expression diff --git a/scripts/demos/pick_and_place.py b/scripts/demos/pick_and_place.py new file mode 100644 index 000000000000..c98998de1242 --- /dev/null +++ b/scripts/demos/pick_and_place.py @@ -0,0 +1,427 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Keyboard control for Isaac Lab Pick and Place.") +parser.add_argument("--num_envs", type=int, default=32, help="Number of environments to spawn.") +# 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.""" + +from collections.abc import Sequence + +import torch + +import carb +import omni + +import isaaclab.sim as sim_utils +from isaaclab.assets import ( + Articulation, + ArticulationCfg, + RigidObject, + RigidObjectCfg, + SurfaceGripper, + SurfaceGripperCfg, +) +from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg +from isaaclab.markers import SPHERE_MARKER_CFG, VisualizationMarkers +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 + +from isaaclab_assets.robots.pick_and_place import PICK_AND_PLACE_CFG + + +@configclass +class PickAndPlaceEnvCfg(DirectRLEnvCfg): + """Example configuration for a PickAndPlace robot using suction-cups. + + This example follows what would be typically done in a DirectRL pipeline. + """ + + # env + decimation = 4 + episode_length_s = 240.0 + action_space = 4 + observation_space = 6 + state_space = 0 + + # Simulation cfg. Surface grippers are currently only supported on CPU. + # Surface grippers also require scene query support to function. + sim: SimulationCfg = SimulationCfg( + dt=1 / 60, + device="cpu", + render_interval=decimation, + use_fabric=True, + enable_scene_query_support=True, + ) + debug_vis = True + + # robot + robot_cfg: ArticulationCfg = PICK_AND_PLACE_CFG.replace(prim_path="/World/envs/env_.*/Robot") + x_dof_name = "x_axis" + y_dof_name = "y_axis" + z_dof_name = "z_axis" + + # We add a cube to pick-up + cube_cfg: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/Robot/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.4, 0.4, 0.4), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 0.0, 0.8)), + ), + init_state=RigidObjectCfg.InitialStateCfg(), + ) + + # Surface Gripper, the prim_expr need to point to a unique surface gripper per environment. + gripper = SurfaceGripperCfg( + prim_path="/World/envs/env_.*/Robot/picker_head/SurfaceGripper", + max_grip_distance=0.1, + shear_force_limit=500.0, + coaxial_force_limit=500.0, + retry_interval=0.2, + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1, env_spacing=12.0, replicate_physics=True) + + # reset logic + # Initial position of the robot + initial_x_pos_range = [-2.0, 2.0] + initial_y_pos_range = [-2.0, 2.0] + initial_z_pos_range = [0.0, 0.5] + + # Initial position of the cube + initial_object_x_pos_range = [-2.0, 2.0] + initial_object_y_pos_range = [-2.0, -0.5] + initial_object_z_pos = 0.2 + + # Target position of the cube + target_x_pos_range = [-2.0, 2.0] + target_y_pos_range = [2.0, 0.5] + target_z_pos = 0.2 + + +class PickAndPlaceEnv(DirectRLEnv): + """Example environment for a PickAndPlace robot using suction-cups. + + This example follows what would be typically done in a DirectRL pipeline. + Here we substitute the policy by keyboard inputs. + """ + + cfg: PickAndPlaceEnvCfg + + def __init__(self, cfg: PickAndPlaceEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + # Indices used to control the different axes of the gantry + self._x_dof_idx, _ = self.pick_and_place.find_joints(self.cfg.x_dof_name) + self._y_dof_idx, _ = self.pick_and_place.find_joints(self.cfg.y_dof_name) + self._z_dof_idx, _ = self.pick_and_place.find_joints(self.cfg.z_dof_name) + + # joints info + self.joint_pos = self.pick_and_place.data.joint_pos + self.joint_vel = self.pick_and_place.data.joint_vel + + # Buffers + self.go_to_cube = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + self.go_to_target = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + self.target_pos = torch.zeros((self.num_envs, 3), device=self.device, dtype=torch.float32) + self.instant_controls = torch.zeros((self.num_envs, 3), device=self.device, dtype=torch.float32) + self.permanent_controls = torch.zeros((self.num_envs, 1), device=self.device, dtype=torch.float32) + + # Visual marker for the target + self.set_debug_vis(self.cfg.debug_vis) + + # Sets up the keyboard callback and settings + self.set_up_keyboard() + + def set_up_keyboard(self): + """Sets up interface for keyboard input and registers the desired keys for control.""" + # Acquire keyboard interface + self._input = carb.input.acquire_input_interface() + self._keyboard = omni.appwindow.get_default_app_window().get_keyboard() + self._sub_keyboard = self._input.subscribe_to_keyboard_events(self._keyboard, self._on_keyboard_event) + # Open / Close / Idle commands for gripper + self._instant_key_controls = { + "Q": torch.tensor([0, 0, -1]), + "E": torch.tensor([0, 0, 1]), + "ZEROS": torch.tensor([0, 0, 0]), + } + # Move up or down + self._permanent_key_controls = { + "W": torch.tensor([-200.0], device=self.device), + "S": torch.tensor([100.0], device=self.device), + } + # Aiming manually is painful we can automate this. + self._auto_aim_cube = "A" + self._auto_aim_target = "D" + + # Task description: + print("Keyboard set up!") + print("The simulation is ready for you to try it out!") + print("Your goal is pick up the purple cube and to drop it on the red sphere!") + print(f"Number of environments: {self.num_envs}") + print("Use the following controls to interact with ALL environments simultaneously:") + print("Press the 'A' key to have all grippers track the cube position.") + print("Press the 'D' key to have all grippers track the target position") + print("Press the 'W' or 'S' keys to move all gantries UP or DOWN respectively") + print("Press 'Q' or 'E' to OPEN or CLOSE all grippers respectively") + + def _on_keyboard_event(self, event): + """Checks for a keyboard event and assign the corresponding command control depending on key pressed.""" + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + # Logic on key press - apply to ALL environments + if event.input.name == self._auto_aim_target: + self.go_to_target[:] = True + self.go_to_cube[:] = False + if event.input.name == self._auto_aim_cube: + self.go_to_cube[:] = True + self.go_to_target[:] = False + if event.input.name in self._instant_key_controls: + self.go_to_cube[:] = False + self.go_to_target[:] = False + self.instant_controls[:] = self._instant_key_controls[event.input.name] + if event.input.name in self._permanent_key_controls: + self.go_to_cube[:] = False + self.go_to_target[:] = False + self.permanent_controls[:] = self._permanent_key_controls[event.input.name] + # On key release, all robots stop moving + elif event.type == carb.input.KeyboardEventType.KEY_RELEASE: + self.go_to_cube[:] = False + self.go_to_target[:] = False + self.instant_controls[:] = self._instant_key_controls["ZEROS"] + + def _setup_scene(self): + self.pick_and_place = Articulation(self.cfg.robot_cfg) + self.cube = RigidObject(self.cfg.cube_cfg) + self.gripper = SurfaceGripper(self.cfg.gripper) + # 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["pick_and_place"] = self.pick_and_place + self.scene.rigid_objects["cube"] = self.cube + self.scene.surface_grippers["gripper"] = self.gripper + # 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: + # Store the actions + self.actions = actions.clone() + + def _apply_action(self) -> None: + # We use the keyboard outputs as an action. + # Process each environment independently + if self.go_to_cube.any(): + # Effort based proportional controller to track the cube position + head_pos_x = self.pick_and_place.data.joint_pos[self.go_to_cube, self._x_dof_idx[0]] + head_pos_y = self.pick_and_place.data.joint_pos[self.go_to_cube, self._y_dof_idx[0]] + cube_pos_x = self.cube.data.root_pos_w[self.go_to_cube, 0] - self.scene.env_origins[self.go_to_cube, 0] + cube_pos_y = self.cube.data.root_pos_w[self.go_to_cube, 1] - self.scene.env_origins[self.go_to_cube, 1] + d_cube_robot_x = cube_pos_x - head_pos_x + d_cube_robot_y = cube_pos_y - head_pos_y + self.instant_controls[self.go_to_cube] = torch.stack( + [d_cube_robot_x * 5.0, d_cube_robot_y * 5.0, torch.zeros_like(d_cube_robot_x)], dim=1 + ) + if self.go_to_target.any(): + # Effort based proportional controller to track the target position + head_pos_x = self.pick_and_place.data.joint_pos[self.go_to_target, self._x_dof_idx[0]] + head_pos_y = self.pick_and_place.data.joint_pos[self.go_to_target, self._y_dof_idx[0]] + target_pos_x = self.target_pos[self.go_to_target, 0] + target_pos_y = self.target_pos[self.go_to_target, 1] + d_target_robot_x = target_pos_x - head_pos_x + d_target_robot_y = target_pos_y - head_pos_y + self.instant_controls[self.go_to_target] = torch.stack( + [d_target_robot_x * 5.0, d_target_robot_y * 5.0, torch.zeros_like(d_target_robot_x)], dim=1 + ) + + # Set the joint effort targets for the picker + self.pick_and_place.set_joint_effort_target( + self.instant_controls[:, 0].unsqueeze(dim=1), joint_ids=self._x_dof_idx + ) + self.pick_and_place.set_joint_effort_target( + self.instant_controls[:, 1].unsqueeze(dim=1), joint_ids=self._y_dof_idx + ) + self.pick_and_place.set_joint_effort_target( + self.permanent_controls[:, 0].unsqueeze(dim=1), joint_ids=self._z_dof_idx + ) + # Set the gripper command + self.gripper.set_grippers_command(self.instant_controls[:, 2]) + + def _get_observations(self) -> dict: + # Get the observations + gripper_state = self.gripper.state.clone() + obs = torch.cat( + ( + self.joint_pos[:, self._x_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._x_dof_idx[0]].unsqueeze(dim=1), + self.joint_pos[:, self._y_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._y_dof_idx[0]].unsqueeze(dim=1), + self.joint_pos[:, self._z_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._z_dof_idx[0]].unsqueeze(dim=1), + self.target_pos[:, 0].unsqueeze(dim=1), + self.target_pos[:, 1].unsqueeze(dim=1), + gripper_state.unsqueeze(dim=1), + ), + dim=-1, + ) + + observations = {"policy": obs} + return observations + + def _get_rewards(self) -> torch.Tensor: + return torch.zeros_like(self.reset_terminated, dtype=torch.float32) + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + # Dones + self.joint_pos = self.pick_and_place.data.joint_pos + self.joint_vel = self.pick_and_place.data.joint_vel + # Check for time out + time_out = self.episode_length_buf >= self.max_episode_length - 1 + # Check if the cube reached the target + cube_to_target_x_dist = self.cube.data.root_pos_w[:, 0] - self.target_pos[:, 0] - self.scene.env_origins[:, 0] + cube_to_target_y_dist = self.cube.data.root_pos_w[:, 1] - self.target_pos[:, 1] - self.scene.env_origins[:, 1] + cube_to_target_z_dist = self.cube.data.root_pos_w[:, 2] - self.target_pos[:, 2] - self.scene.env_origins[:, 2] + cube_to_target_distance = torch.norm( + torch.stack((cube_to_target_x_dist, cube_to_target_y_dist, cube_to_target_z_dist), dim=1), dim=1 + ) + self.target_reached = cube_to_target_distance < 0.3 + # Check if the cube is out of bounds (that is outside of the picking area) + cube_to_origin_xy_diff = self.cube.data.root_pos_w[:, :2] - self.scene.env_origins[:, :2] + cube_to_origin_x_dist = torch.abs(cube_to_origin_xy_diff[:, 0]) + cube_to_origin_y_dist = torch.abs(cube_to_origin_xy_diff[:, 1]) + self.cube_out_of_bounds = (cube_to_origin_x_dist > 2.5) | (cube_to_origin_y_dist > 2.5) + + time_out = time_out | self.target_reached + return self.cube_out_of_bounds, time_out + + def _reset_idx(self, env_ids: Sequence[int] | None): + if env_ids is None: + env_ids = self.pick_and_place._ALL_INDICES + # Reset the environment, this must be done first! As it releases the objects held by the grippers. + # (And that's an operation that should be done before the gripper or the gripped objects are moved) + super()._reset_idx(env_ids) + num_resets = len(env_ids) + + # Set a target position for the cube + self.target_pos[env_ids, 0] = sample_uniform( + self.cfg.target_x_pos_range[0], + self.cfg.target_x_pos_range[1], + num_resets, + self.device, + ) + self.target_pos[env_ids, 1] = sample_uniform( + self.cfg.target_y_pos_range[0], + self.cfg.target_y_pos_range[1], + num_resets, + self.device, + ) + self.target_pos[env_ids, 2] = self.cfg.target_z_pos + + # Set the initial position of the cube + cube_pos = self.cube.data.default_root_state[env_ids, :7] + cube_pos[:, 0] = sample_uniform( + self.cfg.initial_object_x_pos_range[0], + self.cfg.initial_object_x_pos_range[1], + cube_pos[:, 0].shape, + self.device, + ) + cube_pos[:, 1] = sample_uniform( + self.cfg.initial_object_y_pos_range[0], + self.cfg.initial_object_y_pos_range[1], + cube_pos[:, 1].shape, + self.device, + ) + cube_pos[:, 2] = self.cfg.initial_object_z_pos + cube_pos[:, :3] += self.scene.env_origins[env_ids] + self.cube.write_root_pose_to_sim(cube_pos, env_ids) + + # Set the initial position of the robot + joint_pos = self.pick_and_place.data.default_joint_pos[env_ids] + joint_pos[:, self._x_dof_idx] += sample_uniform( + self.cfg.initial_x_pos_range[0], + self.cfg.initial_x_pos_range[1], + joint_pos[:, self._x_dof_idx].shape, + self.device, + ) + joint_pos[:, self._y_dof_idx] += sample_uniform( + self.cfg.initial_y_pos_range[0], + self.cfg.initial_y_pos_range[1], + joint_pos[:, self._y_dof_idx].shape, + self.device, + ) + joint_pos[:, self._z_dof_idx] += sample_uniform( + self.cfg.initial_z_pos_range[0], + self.cfg.initial_z_pos_range[1], + joint_pos[:, self._z_dof_idx].shape, + self.device, + ) + joint_vel = self.pick_and_place.data.default_joint_vel[env_ids] + + self.joint_pos[env_ids] = joint_pos + self.joint_vel[env_ids] = joint_vel + + self.pick_and_place.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 = SPHERE_MARKER_CFG.copy() + marker_cfg.markers["sphere"].radius = 0.25 + # -- 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.target_pos + self.scene.env_origins) + + +def main(): + """Main function.""" + # create environment configuration + env_cfg = PickAndPlaceEnvCfg() + env_cfg.scene.num_envs = args_cli.num_envs + # create environment + pick_and_place = PickAndPlaceEnv(env_cfg) + obs, _ = pick_and_place.reset() + while simulation_app.is_running(): + # check for selected robots + with torch.inference_mode(): + actions = torch.zeros((pick_and_place.num_envs, 4), device=pick_and_place.device, dtype=torch.float32) + pick_and_place.step(actions) + + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/scripts/demos/procedural_terrain.py b/scripts/demos/procedural_terrain.py index 6ca78958e11f..f0a2fb4e2ef7 100644 --- a/scripts/demos/procedural_terrain.py +++ b/scripts/demos/procedural_terrain.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -66,6 +66,7 @@ """Rest everything follows.""" import random + import torch import isaaclab.sim as sim_utils diff --git a/scripts/demos/quadcopter.py b/scripts/demos/quadcopter.py index 51d74384105c..bf42a04f8501 100644 --- a/scripts/demos/quadcopter.py +++ b/scripts/demos/quadcopter.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -16,7 +16,6 @@ """Launch Isaac Sim Simulator first.""" import argparse -import torch from isaaclab.app import AppLauncher @@ -33,6 +32,8 @@ """Rest everything follows.""" +import torch + import isaaclab.sim as sim_utils from isaaclab.assets import Articulation from isaaclab.sim import SimulationContext @@ -100,7 +101,11 @@ def main(): forces = torch.zeros(robot.num_instances, 4, 3, device=sim.device) torques = torch.zeros_like(forces) forces[..., 2] = robot_mass * gravity / 4.0 - robot.set_external_force_and_torque(forces, torques, body_ids=prop_body_ids) + robot.permanent_wrench_composer.set_forces_and_torques( + forces=forces, + torques=torques, + body_ids=prop_body_ids, + ) robot.write_data_to_sim() # perform step sim.step() diff --git a/scripts/demos/quadrupeds.py b/scripts/demos/quadrupeds.py index 78f7f69fcfe4..b9935de30dab 100644 --- a/scripts/demos/quadrupeds.py +++ b/scripts/demos/quadrupeds.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,8 +35,6 @@ import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils from isaaclab.assets import Articulation @@ -77,37 +75,37 @@ def design_scene() -> tuple[dict, list[list[float]]]: origins = define_origins(num_origins=7, spacing=1.25) # Origin 1 with Anymal B - prim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) + sim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) # -- Robot anymal_b = Articulation(ANYMAL_B_CFG.replace(prim_path="/World/Origin1/Robot")) # Origin 2 with Anymal C - prim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) + sim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) # -- Robot anymal_c = Articulation(ANYMAL_C_CFG.replace(prim_path="/World/Origin2/Robot")) # Origin 3 with Anymal D - prim_utils.create_prim("/World/Origin3", "Xform", translation=origins[2]) + sim_utils.create_prim("/World/Origin3", "Xform", translation=origins[2]) # -- Robot anymal_d = Articulation(ANYMAL_D_CFG.replace(prim_path="/World/Origin3/Robot")) # Origin 4 with Unitree A1 - prim_utils.create_prim("/World/Origin4", "Xform", translation=origins[3]) + sim_utils.create_prim("/World/Origin4", "Xform", translation=origins[3]) # -- Robot unitree_a1 = Articulation(UNITREE_A1_CFG.replace(prim_path="/World/Origin4/Robot")) # Origin 5 with Unitree Go1 - prim_utils.create_prim("/World/Origin5", "Xform", translation=origins[4]) + sim_utils.create_prim("/World/Origin5", "Xform", translation=origins[4]) # -- Robot unitree_go1 = Articulation(UNITREE_GO1_CFG.replace(prim_path="/World/Origin5/Robot")) # Origin 6 with Unitree Go2 - prim_utils.create_prim("/World/Origin6", "Xform", translation=origins[5]) + sim_utils.create_prim("/World/Origin6", "Xform", translation=origins[5]) # -- Robot unitree_go2 = Articulation(UNITREE_GO2_CFG.replace(prim_path="/World/Origin6/Robot")) # Origin 7 with Boston Dynamics Spot - prim_utils.create_prim("/World/Origin7", "Xform", translation=origins[6]) + sim_utils.create_prim("/World/Origin7", "Xform", translation=origins[6]) # -- Robot spot = Articulation(SPOT_CFG.replace(prim_path="/World/Origin7/Robot")) diff --git a/scripts/demos/sensors/cameras.py b/scripts/demos/sensors/cameras.py index 5baaf6c0d26a..83214f7e4cf2 100644 --- a/scripts/demos/sensors/cameras.py +++ b/scripts/demos/sensors/cameras.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -37,9 +37,10 @@ """Rest everything follows.""" +import os + import matplotlib.pyplot as plt import numpy as np -import os import torch import isaaclab.sim as sim_utils @@ -138,7 +139,10 @@ def save_images_grid( ncol = int(np.ceil(n_images / nrow)) fig, axes = plt.subplots(nrow, ncol, figsize=(ncol * 2, nrow * 2)) - axes = axes.flatten() + if isinstance(axes, np.ndarray): + axes = axes.flatten() + else: + axes = np.array([axes]) # plot images for idx, (img, ax) in enumerate(zip(images, axes)): diff --git a/scripts/demos/sensors/contact_sensor.py b/scripts/demos/sensors/contact_sensor.py index 7ca19f7e4359..0ee672ec16a6 100644 --- a/scripts/demos/sensors/contact_sensor.py +++ b/scripts/demos/sensors/contact_sensor.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -99,7 +99,6 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # Simulate physics while simulation_app.is_running(): - if count % 500 == 0: # reset counter count = 0 diff --git a/scripts/demos/sensors/frame_transformer_sensor.py b/scripts/demos/sensors/frame_transformer_sensor.py index 7aa748468939..8827b23cea71 100644 --- a/scripts/demos/sensors/frame_transformer_sensor.py +++ b/scripts/demos/sensors/frame_transformer_sensor.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -95,7 +95,6 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # Simulate physics while simulation_app.is_running(): - if count % 500 == 0: # reset counter count = 0 diff --git a/scripts/demos/sensors/imu_sensor.py b/scripts/demos/sensors/imu_sensor.py index cf2a285dbece..af649fd94a97 100644 --- a/scripts/demos/sensors/imu_sensor.py +++ b/scripts/demos/sensors/imu_sensor.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -66,7 +66,6 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # Simulate physics while simulation_app.is_running(): - if count % 500 == 0: # reset counter count = 0 diff --git a/scripts/demos/sensors/multi_mesh_raycaster.py b/scripts/demos/sensors/multi_mesh_raycaster.py new file mode 100644 index 000000000000..07b36573501b --- /dev/null +++ b/scripts/demos/sensors/multi_mesh_raycaster.py @@ -0,0 +1,303 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Example on using the Multi-Mesh Raycaster sensor. + +.. code-block:: bash + + # with allegro hand + python scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type allegro_hand + + # with anymal-D bodies + python scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type anymal_d + + # with random multiple objects + python scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type objects + +""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Example on using the multi-mesh raycaster sensor.") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to spawn.") +parser.add_argument( + "--asset_type", + type=str, + default="allegro_hand", + help="Asset type to use.", + choices=["allegro_hand", "anymal_d", "objects"], +) +# 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 random + +import torch + +import omni.usd +from pxr import Gf, Sdf + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, AssetBaseCfg, RigidObjectCfg +from isaaclab.markers.config import VisualizationMarkersCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors.ray_caster import MultiMeshRayCasterCfg, patterns +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Pre-defined configs +## +from isaaclab_assets.robots.allegro import ALLEGRO_HAND_CFG +from isaaclab_assets.robots.anymal import ANYMAL_D_CFG + +RAY_CASTER_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "hit": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, +) + + +if args_cli.asset_type == "allegro_hand": + asset_cfg = ALLEGRO_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + ray_caster_cfg = MultiMeshRayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot", + update_period=1 / 60, + offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, -0.1, 0.3)), + mesh_prim_paths=[ + "/World/Ground", + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/thumb_link_.*/visuals_xform"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/index_link.*/visuals_xform"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/middle_link_.*/visuals_xform"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/ring_link_.*/visuals_xform"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/palm_link/visuals_xform"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/allegro_mount/visuals_xform"), + ], + ray_alignment="world", + pattern_cfg=patterns.GridPatternCfg(resolution=0.005, size=(0.4, 0.4), direction=(0, 0, -1)), + debug_vis=not args_cli.headless, + visualizer_cfg=RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/RayCaster"), + ) + +elif args_cli.asset_type == "anymal_d": + asset_cfg = ANYMAL_D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + ray_caster_cfg = MultiMeshRayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + update_period=1 / 60, + offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, -0.1, 0.3)), + mesh_prim_paths=[ + "/World/Ground", + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/LF_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/RF_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/LH_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/RH_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/base/visuals"), + ], + ray_alignment="world", + pattern_cfg=patterns.GridPatternCfg(resolution=0.02, size=(2.5, 2.5), direction=(0, 0, -1)), + debug_vis=not args_cli.headless, + visualizer_cfg=RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/RayCaster"), + ) + +elif args_cli.asset_type == "objects": + asset_cfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + spawn=sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), metallic=0.2), + ), + sim_utils.SphereCfg( + radius=0.3, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), + ), + sim_utils.CylinderCfg( + radius=0.2, + height=0.5, + axis="Y", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), + ), + sim_utils.CapsuleCfg( + radius=0.15, + height=0.5, + axis="Z", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 1.0, 0.0), metallic=0.2), + ), + sim_utils.ConeCfg( + radius=0.2, + height=0.5, + axis="Z", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 1.0), metallic=0.2), + ), + ], + random_choice=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), + ) + ray_caster_cfg = MultiMeshRayCasterCfg( + prim_path="{ENV_REGEX_NS}/Object", + update_period=1 / 60, + offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, 0.0, 0.6)), + mesh_prim_paths=[ + "/World/Ground", + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Object"), + ], + ray_alignment="world", + pattern_cfg=patterns.GridPatternCfg(resolution=0.01, size=(0.6, 0.6), direction=(0, 0, -1)), + debug_vis=not args_cli.headless, + visualizer_cfg=RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/RayCaster"), + ) +else: + raise ValueError(f"Unknown asset type: {args_cli.asset_type}") + + +@configclass +class RaycasterSensorSceneCfg(InteractiveSceneCfg): + """Design the scene with sensors on the asset.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/Ground", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd", + scale=(1, 1, 1), + ), + ) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # asset + asset = asset_cfg + # ray caster + ray_caster = ray_caster_cfg + + +def randomize_shape_color(prim_path_expr: str): + """Randomize the color of the geometry.""" + + # acquire stage + stage = omni.usd.get_context().get_stage() + # resolve prim paths for spawning and cloning + prim_paths = sim_utils.find_matching_prim_paths(prim_path_expr) + # manually clone prims if the source prim path is a regex expression + + with Sdf.ChangeBlock(): + for prim_path in prim_paths: + print("Applying prim scale to:", prim_path) + # spawn single instance + prim_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_path) + + # DO YOUR OWN OTHER KIND OF RANDOMIZATION HERE! + # Note: Just need to acquire the right attribute about the property you want to set + # Here is an example on setting color randomly + color_spec = prim_spec.GetAttributeAtPath(prim_path + "/geometry/material/Shader.inputs:diffuseColor") + color_spec.default = Gf.Vec3f(random.random(), random.random(), random.random()) + + # randomize scale + scale_spec = prim_spec.GetAttributeAtPath(prim_path + ".xformOp:scale") + scale_spec.default = Gf.Vec3f(random.uniform(0.5, 1.5), random.uniform(0.5, 1.5), random.uniform(0.5, 1.5)) + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + """Run the simulator.""" + # Define simulation stepping + sim_dt = sim.get_physics_dt() + sim_time = 0.0 + count = 0 + + # Simulate physics + while simulation_app.is_running(): + if count % 500 == 0: + # reset counter + count = 0 + # reset the scene entities + # root state + root_state = scene["asset"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + scene["asset"].write_root_pose_to_sim(root_state[:, :7]) + scene["asset"].write_root_velocity_to_sim(root_state[:, 7:]) + + if isinstance(scene["asset"], Articulation): + # set joint positions with some noise + joint_pos, joint_vel = ( + scene["asset"].data.default_joint_pos.clone(), + scene["asset"].data.default_joint_vel.clone(), + ) + joint_pos += torch.rand_like(joint_pos) * 0.1 + scene["asset"].write_joint_state_to_sim(joint_pos, joint_vel) + # clear internal buffers + scene.reset() + print("[INFO]: Resetting Asset state...") + + if isinstance(scene["asset"], Articulation): + # -- generate actions/commands + targets = scene["asset"].data.default_joint_pos + 5 * ( + torch.rand_like(scene["asset"].data.default_joint_pos) - 0.5 + ) + # -- apply action to the asset + scene["asset"].set_joint_position_target(targets) + # -- write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + count += 1 + # update buffers + scene.update(sim_dt) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + # design scene + scene_cfg = RaycasterSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0, replicate_physics=False) + scene = InteractiveScene(scene_cfg) + + if args_cli.asset_type == "objects": + randomize_shape_color(scene_cfg.asset.prim_path.format(ENV_REGEX_NS="/World/envs/env_.*")) + + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/demos/sensors/multi_mesh_raycaster_camera.py b/scripts/demos/sensors/multi_mesh_raycaster_camera.py new file mode 100644 index 000000000000..8ef3a188f3d1 --- /dev/null +++ b/scripts/demos/sensors/multi_mesh_raycaster_camera.py @@ -0,0 +1,329 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Example on using the Multi-Mesh Raycaster Camera sensor. + +.. code-block:: bash + + # with allegro hand + python scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type allegro_hand + + # with anymal-D bodies + python scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type anymal_d + + # with random multiple objects + python scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type objects + +""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Example on using the multi-mesh raycaster sensor.") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to spawn.") +parser.add_argument( + "--asset_type", + type=str, + default="allegro_hand", + help="Asset type to use.", + choices=["allegro_hand", "anymal_d", "objects"], +) +# 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 random + +import torch + +import omni.usd +from pxr import Gf, Sdf + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, AssetBaseCfg, RigidObjectCfg +from isaaclab.markers.config import VisualizationMarkersCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors.ray_caster import MultiMeshRayCasterCameraCfg, patterns +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Pre-defined configs +## +from isaaclab_assets.robots.allegro import ALLEGRO_HAND_CFG +from isaaclab_assets.robots.anymal import ANYMAL_D_CFG + +RAY_CASTER_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "hit": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, +) + +if args_cli.asset_type == "allegro_hand": + asset_cfg = ALLEGRO_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + ray_caster_cfg = MultiMeshRayCasterCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot", + update_period=1 / 60, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg( + pos=(-0.70, -0.7, -0.25), rot=(0.268976, 0.268976, 0.653951, 0.653951) + ), + mesh_prim_paths=[ + "/World/Ground", + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/thumb_link_.*/visuals_xform"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/index_link.*/visuals_xform"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/middle_link_.*/visuals_xform"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/ring_link_.*/visuals_xform"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/palm_link/visuals_xform"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/allegro_mount/visuals_xform"), + ], + pattern_cfg=patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=120, + width=240, + ), + debug_vis=not args_cli.headless, + visualizer_cfg=RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/RayCaster"), + ) + +elif args_cli.asset_type == "anymal_d": + asset_cfg = ANYMAL_D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + ray_caster_cfg = MultiMeshRayCasterCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + update_period=1 / 60, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0, -0.1, 1.5), rot=(0.0, 1.0, 0.0, 0.0)), + mesh_prim_paths=[ + "/World/Ground", + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/LF_.*/visuals"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/RF_.*/visuals"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/LH_.*/visuals"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/RH_.*/visuals"), + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/base/visuals"), + ], + pattern_cfg=patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=120, + width=240, + ), + debug_vis=not args_cli.headless, + visualizer_cfg=RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/RayCaster"), + ) + +elif args_cli.asset_type == "objects": + asset_cfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + spawn=sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), metallic=0.2), + ), + sim_utils.SphereCfg( + radius=0.3, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), + ), + sim_utils.CylinderCfg( + radius=0.2, + height=0.5, + axis="Y", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), + ), + sim_utils.CapsuleCfg( + radius=0.15, + height=0.5, + axis="Z", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 1.0, 0.0), metallic=0.2), + ), + sim_utils.ConeCfg( + radius=0.2, + height=0.5, + axis="Z", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 1.0), metallic=0.2), + ), + ], + random_choice=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), + ) + ray_caster_cfg = MultiMeshRayCasterCameraCfg( + prim_path="{ENV_REGEX_NS}/Object", + update_period=1 / 60, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0, 0.0, 1.5), rot=(0.0, 1.0, 0.0, 0.0)), + mesh_prim_paths=[ + "/World/Ground", + MultiMeshRayCasterCameraCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Object"), + ], + pattern_cfg=patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=120, + width=240, + ), + debug_vis=not args_cli.headless, + visualizer_cfg=RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/RayCaster"), + ) +else: + raise ValueError(f"Unknown asset type: {args_cli.asset_type}") + + +@configclass +class RaycasterSensorSceneCfg(InteractiveSceneCfg): + """Design the scene with sensors on the asset.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/Ground", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd", + scale=(1, 1, 1), + ), + ) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # asset + asset = asset_cfg + # ray caster + ray_caster = ray_caster_cfg + + +def randomize_shape_color(prim_path_expr: str): + """Randomize the color of the geometry.""" + + # acquire stage + stage = omni.usd.get_context().get_stage() + # resolve prim paths for spawning and cloning + prim_paths = sim_utils.find_matching_prim_paths(prim_path_expr) + # manually clone prims if the source prim path is a regex expression + + with Sdf.ChangeBlock(): + for prim_path in prim_paths: + print("Applying prim scale to:", prim_path) + # spawn single instance + prim_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_path) + + # DO YOUR OWN OTHER KIND OF RANDOMIZATION HERE! + # Note: Just need to acquire the right attribute about the property you want to set + # Here is an example on setting color randomly + color_spec = prim_spec.GetAttributeAtPath(prim_path + "/geometry/material/Shader.inputs:diffuseColor") + color_spec.default = Gf.Vec3f(random.random(), random.random(), random.random()) + + # randomize scale + scale_spec = prim_spec.GetAttributeAtPath(prim_path + ".xformOp:scale") + scale_spec.default = Gf.Vec3f(random.uniform(0.5, 1.5), random.uniform(0.5, 1.5), random.uniform(0.5, 1.5)) + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + """Run the simulator.""" + # Define simulation stepping + sim_dt = sim.get_physics_dt() + sim_time = 0.0 + count = 0 + + triggered = True + countdown = 42 + + # Simulate physics + while simulation_app.is_running(): + if count % 500 == 0: + # reset counter + count = 0 + # reset the scene entities + # root state + root_state = scene["asset"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + scene["asset"].write_root_pose_to_sim(root_state[:, :7]) + scene["asset"].write_root_velocity_to_sim(root_state[:, 7:]) + + if isinstance(scene["asset"], Articulation): + # set joint positions with some noise + joint_pos, joint_vel = ( + scene["asset"].data.default_joint_pos.clone(), + scene["asset"].data.default_joint_vel.clone(), + ) + joint_pos += torch.rand_like(joint_pos) * 0.1 + scene["asset"].write_joint_state_to_sim(joint_pos, joint_vel) + # clear internal buffers + scene.reset() + print("[INFO]: Resetting Asset state...") + + if isinstance(scene["asset"], Articulation): + # -- generate actions/commands + targets = scene["asset"].data.default_joint_pos + 5 * ( + torch.rand_like(scene["asset"].data.default_joint_pos) - 0.5 + ) + # -- apply action to the asset + scene["asset"].set_joint_position_target(targets) + # -- write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + count += 1 + # update buffers + scene.update(sim_dt) + + if not triggered: + if countdown > 0: + countdown -= 1 + continue + + data = scene["ray_caster"].data.ray_hits_w.cpu().numpy() # noqa: F841 + triggered = True + else: + continue + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + # design scene + scene_cfg = RaycasterSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0, replicate_physics=False) + scene = InteractiveScene(scene_cfg) + + if args_cli.asset_type == "objects": + randomize_shape_color(scene_cfg.asset.prim_path.format(ENV_REGEX_NS="/World/envs/env_.*")) + + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/demos/sensors/raycaster_sensor.py b/scripts/demos/sensors/raycaster_sensor.py index 615df97aa007..02c55222e836 100644 --- a/scripts/demos/sensors/raycaster_sensor.py +++ b/scripts/demos/sensors/raycaster_sensor.py @@ -1,10 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import 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 @@ -63,7 +63,7 @@ class RaycasterSensorSceneCfg(InteractiveSceneCfg): update_period=1 / 60, offset=RayCasterCfg.OffsetCfg(pos=(0, 0, 0.5)), mesh_prim_paths=["/World/Ground"], - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.LidarPatternCfg( channels=100, vertical_fov_range=[-90, 90], horizontal_fov_range=[-90, 90], horizontal_res=1.0 ), @@ -83,7 +83,6 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # Simulate physics while simulation_app.is_running(): - if count % 500 == 0: # reset counter count = 0 diff --git a/scripts/demos/sensors/tacsl_sensor.py b/scripts/demos/sensors/tacsl_sensor.py new file mode 100644 index 000000000000..aa5f2253e293 --- /dev/null +++ b/scripts/demos/sensors/tacsl_sensor.py @@ -0,0 +1,427 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Example script demonstrating the TacSL tactile sensor implementation in IsaacLab. + +This script shows how to use the TactileSensor for both camera-based and force field +tactile sensing with the GelSight finger setup. + +.. code-block:: bash + + # Usage + python scripts/demos/sensors/tacsl_sensor.py \ + --use_tactile_rgb \ + --use_tactile_ff \ + --tactile_compliance_stiffness 100.0 \ + --num_envs 16 \ + --contact_object_type nut \ + --save_viz \ + --enable_cameras + +""" + +import argparse +import math +import os + +import cv2 +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# Add argparse arguments +parser = argparse.ArgumentParser(description="TacSL tactile sensor example.") +parser.add_argument("--num_envs", type=int, default=2, help="Number of environments to spawn.") +parser.add_argument("--normal_contact_stiffness", type=float, default=1.0, help="Tactile normal stiffness.") +parser.add_argument("--tangential_stiffness", type=float, default=0.1, help="Tactile tangential stiffness.") +parser.add_argument("--friction_coefficient", type=float, default=2.0, help="Tactile friction coefficient.") +parser.add_argument( + "--tactile_compliance_stiffness", + type=float, + default=None, + help="Optional: Override compliant contact stiffness (default: use USD asset values)", +) +parser.add_argument( + "--tactile_compliant_damping", + type=float, + default=None, + help="Optional: Override compliant contact damping (default: use USD asset values)", +) +parser.add_argument("--save_viz", action="store_true", help="Visualize tactile data.") +parser.add_argument("--save_viz_dir", type=str, default="tactile_record", help="Directory to save tactile data.") +parser.add_argument("--use_tactile_rgb", action="store_true", help="Use tactile RGB sensor data collection.") +parser.add_argument("--use_tactile_ff", action="store_true", help="Use tactile force field sensor data collection.") +parser.add_argument("--debug_sdf_closest_pts", action="store_true", help="Visualize closest SDF points.") +parser.add_argument("--debug_tactile_sensor_pts", action="store_true", help="Visualize tactile sensor points.") +parser.add_argument("--trimesh_vis_tactile_points", action="store_true", help="Visualize tactile points using trimesh.") +parser.add_argument( + "--contact_object_type", + type=str, + default="nut", + choices=["none", "cube", "nut"], + help="Type of contact object to use.", +) + +# 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 isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import TiledCameraCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.timer import Timer + +from isaaclab_contrib.sensors.tacsl_sensor import VisuoTactileSensorCfg +from isaaclab_contrib.sensors.tacsl_sensor.visuotactile_render import compute_tactile_shear_image +from isaaclab_contrib.sensors.tacsl_sensor.visuotactile_sensor_data import VisuoTactileSensorData + +from isaaclab_assets.sensors import GELSIGHT_R15_CFG + + +@configclass +class TactileSensorsSceneCfg(InteractiveSceneCfg): + """Design the scene with tactile sensors on the robot.""" + + # Ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # Lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Robot with tactile sensor + robot = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileWithCompliantContactCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + ), + compliant_contact_stiffness=args_cli.tactile_compliance_stiffness, + compliant_contact_damping=args_cli.tactile_compliant_damping, + physics_material_prim_path="elastomer", + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=12, + solver_velocity_iteration_count=1, + ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.001, rest_offset=-0.0005), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(math.sqrt(2) / 2, -math.sqrt(2) / 2, 0.0, 0.0), # 90ยฐ rotation + joint_pos={}, + joint_vel={}, + ), + actuators={}, + ) + + # Camera configuration for tactile sensing + + # TacSL Tactile Sensor + tactile_sensor = VisuoTactileSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer/tactile_sensor", + history_length=0, + debug_vis=args_cli.debug_tactile_sensor_pts or args_cli.debug_sdf_closest_pts, + # Sensor configuration + render_cfg=GELSIGHT_R15_CFG, + enable_camera_tactile=args_cli.use_tactile_rgb, + enable_force_field=args_cli.use_tactile_ff, + # Elastomer configuration + tactile_array_size=(20, 25), + tactile_margin=0.003, + # Contact object configuration + contact_object_prim_path_expr="{ENV_REGEX_NS}/contact_object", + # Force field physics parameters + normal_contact_stiffness=args_cli.normal_contact_stiffness, + friction_coefficient=args_cli.friction_coefficient, + tangential_stiffness=args_cli.tangential_stiffness, + # Camera configuration + # Note: the camera is already spawned in the scene, properties are set in the + # 'gelsight_r15_finger.usd' USD file + camera_cfg=TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer_tip/cam", + height=GELSIGHT_R15_CFG.image_height, + width=GELSIGHT_R15_CFG.image_width, + data_types=["distance_to_image_plane"], + spawn=None, + ), + # Debug Visualization + trimesh_vis_tactile_points=args_cli.trimesh_vis_tactile_points, + visualize_sdf_closest_pts=args_cli.debug_sdf_closest_pts, + ) + + +@configclass +class CubeTactileSceneCfg(TactileSensorsSceneCfg): + """Scene with cube contact object.""" + + # Cube contact object + contact_object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/contact_object", + spawn=sim_utils.CuboidCfg( + size=(0.01, 0.01, 0.01), + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=0.00327211), + collision_props=sim_utils.CollisionPropertiesCfg(), + physics_material=sim_utils.RigidBodyMaterialCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.1, 0.1)), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0 + 0.06776, 0.51), rot=(1.0, 0.0, 0.0, 0.0)), + ) + + +@configclass +class NutTactileSceneCfg(TactileSensorsSceneCfg): + """Scene with nut contact object.""" + + # Nut contact object + contact_object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/contact_object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_nut_m16.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + solver_position_iteration_count=12, + solver_velocity_iteration_count=1, + max_angular_velocity=180.0, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.1), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0), + articulation_props=sim_utils.ArticulationRootPropertiesCfg(articulation_enabled=False), + ), + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.0, 0.0 + 0.06776, 0.498), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + + +def mkdir_helper(dir_path: str) -> tuple[str, str]: + """Create directories for saving tactile sensor visualizations. + + Args: + dir_path: The base directory path where visualizations will be saved. + + Returns: + A tuple containing paths to the force field directory and RGB image directory. + """ + tactile_img_folder = dir_path + os.makedirs(tactile_img_folder, exist_ok=True) + # create a subdirectory for the force field data + tactile_force_field_dir = os.path.join(tactile_img_folder, "tactile_force_field") + os.makedirs(tactile_force_field_dir, exist_ok=True) + # create a subdirectory for the RGB image data + tactile_rgb_image_dir = os.path.join(tactile_img_folder, "tactile_rgb_image") + os.makedirs(tactile_rgb_image_dir, exist_ok=True) + + return tactile_force_field_dir, tactile_rgb_image_dir + + +def save_viz_helper( + dir_path_list: tuple[str, str], + count: int, + tactile_data: VisuoTactileSensorData, + num_envs: int, + nrows: int, + ncols: int, +): + """Save visualization of tactile sensor data. + + Args: + dir_path_list: A tuple containing paths to the force field directory and RGB image directory. + count: The current simulation step count, used for naming saved files. + tactile_data: The data object containing tactile sensor readings (forces, images). + num_envs: Number of environments in the simulation. + nrows: Number of rows in the tactile array. + ncols: Number of columns in the tactile array. + """ + # Only save the first 2 environments + + tactile_force_field_dir, tactile_rgb_image_dir = dir_path_list + + if tactile_data.tactile_shear_force is not None and tactile_data.tactile_normal_force is not None: + # visualize tactile forces + tactile_normal_force = tactile_data.tactile_normal_force.view((num_envs, nrows, ncols)) + tactile_shear_force = tactile_data.tactile_shear_force.view((num_envs, nrows, ncols, 2)) + + tactile_image = compute_tactile_shear_image( + tactile_normal_force[0, :, :].detach().cpu().numpy(), tactile_shear_force[0, :, :].detach().cpu().numpy() + ) + + if tactile_normal_force.shape[0] > 1: + tactile_image_1 = compute_tactile_shear_image( + tactile_normal_force[1, :, :].detach().cpu().numpy(), + tactile_shear_force[1, :, :].detach().cpu().numpy(), + ) + combined_image = np.vstack([tactile_image, tactile_image_1]) + cv2.imwrite( + os.path.join(tactile_force_field_dir, f"{count:04d}.png"), (combined_image * 255).astype(np.uint8) + ) + else: + cv2.imwrite( + os.path.join(tactile_force_field_dir, f"{count:04d}.png"), (tactile_image * 255).astype(np.uint8) + ) + + if tactile_data.tactile_rgb_image is not None: + tactile_rgb_data = tactile_data.tactile_rgb_image.cpu().numpy() + tactile_rgb_data = np.transpose(tactile_rgb_data, axes=(0, 2, 1, 3)) + tactile_rgb_data_first_2 = tactile_rgb_data[:2] if len(tactile_rgb_data) >= 2 else tactile_rgb_data + tactile_rgb_tiled = np.concatenate(tactile_rgb_data_first_2, axis=0) + # Convert to uint8 if not already + if tactile_rgb_tiled.dtype != np.uint8: + tactile_rgb_tiled = ( + (tactile_rgb_tiled * 255).astype(np.uint8) + if tactile_rgb_tiled.max() <= 1.0 + else tactile_rgb_tiled.astype(np.uint8) + ) + cv2.imwrite(os.path.join(tactile_rgb_image_dir, f"{count:04d}.png"), tactile_rgb_tiled) + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + """Run the simulator.""" + # Define simulation stepping + sim_dt = sim.get_physics_dt() + sim_time = 0.0 + count = 0 + + # Assign different masses to contact objects in different environments + num_envs = scene.num_envs + + if args_cli.save_viz: + # Create output directories for tactile data + print(f"[INFO]: Saving tactile data to: {args_cli.save_viz_dir}...") + dir_path_list = mkdir_helper(args_cli.save_viz_dir) + + # Create constant downward force + force_tensor = torch.zeros(scene.num_envs, 1, 3, device=sim.device) + torque_tensor = torch.zeros(scene.num_envs, 1, 3, device=sim.device) + force_tensor[:, 0, 2] = -1.0 + + nrows = scene["tactile_sensor"].cfg.tactile_array_size[0] + ncols = scene["tactile_sensor"].cfg.tactile_array_size[1] + + physics_timer = Timer() + physics_total_time = 0.0 + physics_total_count = 0 + + entity_list = ["robot"] + if "contact_object" in scene.keys(): + entity_list.append("contact_object") + + while simulation_app.is_running(): + if count == 122: + # Reset robot and contact object positions + count = 0 + for entity in entity_list: + root_state = scene[entity].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + scene[entity].write_root_state_to_sim(root_state) + + scene.reset() + print("[INFO]: Resetting robot and contact object state...") + + if "contact_object" in scene.keys(): + # rotation + if count > 20: + env_indices = torch.arange(scene.num_envs, device=sim.device) + odd_mask = env_indices % 2 == 1 + even_mask = env_indices % 2 == 0 + torque_tensor[odd_mask, 0, 2] = 10 # rotation for odd environments + torque_tensor[even_mask, 0, 2] = -10 # rotation for even environments + scene["contact_object"].permanent_wrench_composer.set_forces_and_torques(force_tensor, torque_tensor) + + # Step simulation + scene.write_data_to_sim() + physics_timer.start() + sim.step() + physics_timer.stop() + physics_total_time += physics_timer.total_run_time + physics_total_count += 1 + sim_time += sim_dt + count += 1 + scene.update(sim_dt) + + # Access tactile sensor data + tactile_data = scene["tactile_sensor"].data + + if args_cli.save_viz: + save_viz_helper(dir_path_list, count, tactile_data, num_envs, nrows, ncols) + + # Get timing summary from sensor and add physics timing + timing_summary = scene["tactile_sensor"].get_timing_summary() + + # Add physics timing to the summary + physics_avg = physics_total_time / (physics_total_count * scene.num_envs) if physics_total_count > 0 else 0.0 + timing_summary["physics_total"] = physics_total_time + timing_summary["physics_average"] = physics_avg + timing_summary["physics_fps"] = 1 / physics_avg if physics_avg > 0 else 0.0 + + print(timing_summary) + + +def main(): + """Main function.""" + # Initialize simulation + # Note: We set the gpu_collision_stack_size to prevent buffer overflow in contact-rich environments. + sim_cfg = sim_utils.SimulationCfg( + dt=0.005, + device=args_cli.device, + physx=sim_utils.PhysxCfg(gpu_collision_stack_size=2**30), + ) + sim = sim_utils.SimulationContext(sim_cfg) + + # Set main camera + sim.set_camera_view(eye=[0.5, 0.6, 1.0], target=[-0.1, 0.1, 0.5]) + + # Create scene based on contact object type + if args_cli.contact_object_type == "cube": + scene_cfg = CubeTactileSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + # disabled force field for cube contact object because a SDF collision mesh cannot + # be created for the Shape Prims + scene_cfg.tactile_sensor.enable_force_field = False + elif args_cli.contact_object_type == "nut": + scene_cfg = NutTactileSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + elif args_cli.contact_object_type == "none": + scene_cfg = TactileSensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + scene_cfg.tactile_sensor.contact_object_prim_path_expr = None + # this flag is to visualize the tactile sensor points + scene_cfg.tactile_sensor.debug_vis = True + else: + raise ValueError( + f"Invalid contact object type: '{args_cli.contact_object_type}'. Must be 'none', 'cube', or 'nut'." + ) + + scene = InteractiveScene(scene_cfg) + + # Initialize simulation + sim.reset() + print("[INFO]: Setup complete...") + + # Get initial render + scene["tactile_sensor"].get_initial_render() + # Run simulation + run_simulator(sim, scene) + + +if __name__ == "__main__": + # Run the main function + main() + # Close sim app + simulation_app.close() diff --git a/scripts/environments/export_IODescriptors.py b/scripts/environments/export_IODescriptors.py new file mode 100644 index 000000000000..3f515a166f98 --- /dev/null +++ b/scripts/environments/export_IODescriptors.py @@ -0,0 +1,101 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to an environment with random action agent.""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse +import os + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Random agent for Isaac Lab environments.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--output_dir", type=str, default=None, help="Path to the output directory.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +args_cli.headless = True + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import gymnasium as gym +import torch + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils import parse_env_cfg + +# PLACEHOLDER: Extension template (do not remove this comment) + + +def main(): + """Random actions agent with Isaac Lab environment.""" + # create environment configuration + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1, use_fabric=True) + # create environment + env = gym.make(args_cli.task, cfg=env_cfg) + + # print info (this is vectorized environment) + print(f"[INFO]: Gym observation space: {env.observation_space}") + print(f"[INFO]: Gym action space: {env.action_space}") + # reset environment + env.reset() + + outs = env.unwrapped.get_IO_descriptors + out_observations = outs["observations"] + out_actions = outs["actions"] + out_articulations = outs["articulations"] + out_scene = outs["scene"] + # Make a yaml file with the output + import yaml + + name = args_cli.task.lower().replace("-", "_") + name = name.replace(" ", "_") + + if not os.path.exists(args_cli.output_dir): + os.makedirs(args_cli.output_dir) + + with open(os.path.join(args_cli.output_dir, f"{name}_IO_descriptors.yaml"), "w") as f: + print(f"[INFO]: Exporting IO descriptors to {os.path.join(args_cli.output_dir, f'{name}_IO_descriptors.yaml')}") + yaml.safe_dump(outs, f) + + for k in out_actions: + print(f"--- Action term: {k['name']} ---") + k.pop("name") + for k1, v1 in k.items(): + print(f"{k1}: {v1}") + + for obs_group_name, obs_group in out_observations.items(): + print(f"--- Obs group: {obs_group_name} ---") + for k in obs_group: + print(f"--- Obs term: {k['name']} ---") + k.pop("name") + for k1, v1 in k.items(): + print(f"{k1}: {v1}") + + for articulation_name, articulation_data in out_articulations.items(): + print(f"--- Articulation: {articulation_name} ---") + for k1, v1 in articulation_data.items(): + print(f"{k1}: {v1}") + + for k1, v1 in out_scene.items(): + print(f"{k1}: {v1}") + + env.step(torch.zeros(env.action_space.shape, device=env.unwrapped.device)) + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/environments/list_envs.py b/scripts/environments/list_envs.py index 43393d52d488..0beb83e92131 100644 --- a/scripts/environments/list_envs.py +++ b/scripts/environments/list_envs.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -15,8 +15,16 @@ """Launch Isaac Sim Simulator first.""" +import argparse + from isaaclab.app import AppLauncher +# add argparse arguments +parser = argparse.ArgumentParser(description="List Isaac Lab environments.") +parser.add_argument("--keyword", type=str, default=None, help="Keyword to filter environments.") +# parse the arguments +args_cli = parser.parse_args() + # launch omniverse app app_launcher = AppLauncher(headless=True) simulation_app = app_launcher.app @@ -44,7 +52,7 @@ def main(): index = 0 # acquire all Isaac environments names for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id: + if "Isaac" in task_spec.id and (args_cli.keyword is None or args_cli.keyword in task_spec.id): # add details to table table.add_row([index + 1, task_spec.id, task_spec.entry_point, task_spec.kwargs["env_cfg_entry_point"]]) # increment count diff --git a/scripts/environments/random_agent.py b/scripts/environments/random_agent.py index 73d6f57ee2e1..6a40060d64b1 100644 --- a/scripts/environments/random_agent.py +++ b/scripts/environments/random_agent.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,6 +35,8 @@ import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils import parse_env_cfg +# PLACEHOLDER: Extension template (do not remove this comment) + def main(): """Random actions agent with Isaac Lab environment.""" diff --git a/scripts/environments/state_machine/lift_cube_sm.py b/scripts/environments/state_machine/lift_cube_sm.py index fa4d357f4c5a..6136e2e3a351 100644 --- a/scripts/environments/state_machine/lift_cube_sm.py +++ b/scripts/environments/state_machine/lift_cube_sm.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -38,10 +38,10 @@ """Rest everything else.""" -import gymnasium as gym -import torch from collections.abc import Sequence +import gymnasium as gym +import torch import warp as wp from isaaclab.assets.rigid_object.rigid_object_data import RigidObjectData diff --git a/scripts/environments/state_machine/lift_teddy_bear.py b/scripts/environments/state_machine/lift_teddy_bear.py index dbc30e611f99..2eb4ae710099 100644 --- a/scripts/environments/state_machine/lift_teddy_bear.py +++ b/scripts/environments/state_machine/lift_teddy_bear.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -33,12 +33,17 @@ app_launcher = AppLauncher(headless=args_cli.headless) simulation_app = app_launcher.app +# disable metrics assembler due to scene graph instancing +from isaacsim.core.utils.extensions import disable_extension + +disable_extension("omni.usd.metrics.assembler.ui") + """Rest everything else.""" -import gymnasium as gym -import torch from collections.abc import Sequence +import gymnasium as gym +import torch import warp as wp from isaaclab.assets.rigid_object.rigid_object_data import RigidObjectData diff --git a/scripts/environments/state_machine/open_cabinet_sm.py b/scripts/environments/state_machine/open_cabinet_sm.py index 0171fa2ad383..3cb88d31a1a2 100644 --- a/scripts/environments/state_machine/open_cabinet_sm.py +++ b/scripts/environments/state_machine/open_cabinet_sm.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -38,10 +38,10 @@ """Rest everything else.""" -import gymnasium as gym -import torch from collections.abc import Sequence +import gymnasium as gym +import torch import warp as wp from isaaclab.sensors import FrameTransformer @@ -206,7 +206,7 @@ def __init__(self, dt: float, num_envs: int, device: torch.device | str = "cpu", self.des_ee_pose = torch.zeros((self.num_envs, 7), device=self.device) self.des_gripper_state = torch.full((self.num_envs,), 0.0, device=self.device) - # approach infront of the handle + # approach in front of the handle self.handle_approach_offset = torch.zeros((self.num_envs, 7), device=self.device) self.handle_approach_offset[:, 0] = -0.1 self.handle_approach_offset[:, -1] = 1.0 # warp expects quaternion as (x, y, z, w) diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index eb3323a6ca38..8492ad77f3ce 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -1,34 +1,57 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""Script to run a keyboard teleoperation with Isaac Lab manipulation environments.""" +"""Script to run teleoperation with Isaac Lab manipulation environments. + +Supports multiple input devices (e.g., keyboard, spacemouse, gamepad) and devices +configured within the environment (including OpenXR-based hand tracking or motion +controllers).""" """Launch Isaac Sim Simulator first.""" import argparse -import os +from collections.abc import Callable from isaaclab.app import AppLauncher # add argparse arguments -parser = argparse.ArgumentParser(description="Keyboard teleoperation for Isaac Lab environments.") +parser = argparse.ArgumentParser(description="Teleoperation for Isaac Lab environments.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") parser.add_argument( - "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations." + "--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("--num_envs", type=int, default=1, help="Number of environments to simulate.") -parser.add_argument("--teleop_device", type=str, default="keyboard", help="Device for interacting with environment") 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.") +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments args_cli = parser.parse_args() app_launcher_args = vars(args_cli) -if args_cli.teleop_device.lower() == "handtracking": - app_launcher_args["experience"] = f'{os.environ["ISAACLAB_PATH"]}/apps/isaaclab.python.xr.openxr.kit' + +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 +if "handtracking" in args_cli.teleop_device.lower(): + app_launcher_args["xr"] = True + # launch omniverse app app_launcher = AppLauncher(app_launcher_args) simulation_app = app_launcher.app @@ -36,42 +59,47 @@ """Rest everything follows.""" +import logging + import gymnasium as gym import torch -import omni.log - -from isaaclab.devices import Se3Gamepad, Se3HandTracking, Se3Keyboard, Se3SpaceMouse -from isaaclab.envs import ViewerCfg -from isaaclab.envs.ui import ViewportCameraController +from isaaclab.devices import Se3Gamepad, Se3GamepadCfg, Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices.openxr import remove_camera_configs +from isaaclab.devices.teleop_device_factory import create_teleop_device +from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import TerminationTermCfg as DoneTerm import isaaclab_tasks # noqa: F401 from isaaclab_tasks.manager_based.manipulation.lift import mdp 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 -def pre_process_actions(delta_pose: torch.Tensor, gripper_command: bool) -> torch.Tensor: - """Pre-process actions for the environment.""" - # compute actions based on environment - if "Reach" in args_cli.task: - # note: reach is the only one that uses a different action space - # compute actions - return delta_pose - else: - # resolve gripper command - gripper_vel = torch.zeros(delta_pose.shape[0], 1, device=delta_pose.device) - gripper_vel[:] = -1.0 if gripper_command else 1.0 - # compute actions - return torch.concat([delta_pose, gripper_vel], dim=1) +# import logger +logger = logging.getLogger(__name__) -def main(): - """Running keyboard teleoperation with Isaac Lab manipulation environment.""" +def main() -> None: + """ + Run teleoperation with an Isaac Lab manipulation environment. + + Creates the environment, sets up teleoperation interfaces and callbacks, + and runs the main simulation loop until the application is closed. + + Returns: + None + """ # parse configuration - env_cfg = parse_env_cfg( - args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric - ) + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs) + env_cfg.env_name = args_cli.task + if not isinstance(env_cfg, ManagerBasedRLEnvCfg): + raise ValueError( + "Teleoperation is only supported for ManagerBasedRLEnv environments. " + f"Received environment config type: {type(env_cfg).__name__}" + ) # modify configuration env_cfg.terminations.time_out = None if "Lift" in args_cli.task: @@ -79,73 +107,172 @@ def main(): env_cfg.commands.object_pose.resampling_time_range = (1.0e9, 1.0e9) # add termination condition for reaching the goal otherwise the environment won't reset env_cfg.terminations.object_reached_goal = DoneTerm(func=mdp.object_reached_goal) - # create environment - env = gym.make(args_cli.task, cfg=env_cfg).unwrapped - # check environment name (for reach , we don't allow the gripper) - if "Reach" in args_cli.task: - omni.log.warn( - f"The environment '{args_cli.task}' does not support gripper control. The device command will be ignored." - ) - # create controller - if args_cli.teleop_device.lower() == "keyboard": - teleop_interface = Se3Keyboard( - pos_sensitivity=0.05 * args_cli.sensitivity, rot_sensitivity=0.05 * args_cli.sensitivity - ) - elif args_cli.teleop_device.lower() == "spacemouse": - teleop_interface = Se3SpaceMouse( - pos_sensitivity=0.05 * args_cli.sensitivity, rot_sensitivity=0.05 * args_cli.sensitivity - ) - elif args_cli.teleop_device.lower() == "gamepad": - teleop_interface = Se3Gamepad( - pos_sensitivity=0.1 * args_cli.sensitivity, rot_sensitivity=0.1 * args_cli.sensitivity - ) - elif args_cli.teleop_device.lower() == "handtracking": - from isaacsim.xr.openxr import OpenXRSpec + if args_cli.xr: + env_cfg = remove_camera_configs(env_cfg) + env_cfg.sim.render.antialiasing_mode = "DLSS" - teleop_interface = Se3HandTracking(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True) - teleop_interface.add_callback("RESET", env.reset) - viewer = ViewerCfg(eye=(-0.25, -0.3, 0.5), lookat=(0.6, 0, 0), asset_name="viewer") - ViewportCameraController(env, viewer) - else: - raise ValueError( - f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse''handtracking'." - ) + try: + # create environment + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + # check environment name (for reach , we don't allow the gripper) + if "Reach" in args_cli.task: + logger.warning( + f"The environment '{args_cli.task}' does not support gripper control. The device command will be" + " ignored." + ) + except Exception as e: + logger.error(f"Failed to create environment: {e}") + simulation_app.close() + return - # add teleoperation key for env reset + # Flags for controlling teleoperation flow should_reset_recording_instance = False + teleoperation_active = True + + # Callback handlers + def reset_recording_instance() -> None: + """ + Reset the environment to its initial state. - def reset_recording_instance(): + Sets a flag to reset the environment on the next simulation step. + + Returns: + None + """ nonlocal should_reset_recording_instance should_reset_recording_instance = True + print("Reset triggered - Environment will reset on next step") + + def start_teleoperation() -> None: + """ + Activate teleoperation control of the robot. + + Enables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = True + print("Teleoperation activated") + + def stop_teleoperation() -> None: + """ + Deactivate teleoperation control of the robot. + + Disables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = False + print("Teleoperation deactivated") - teleop_interface.add_callback("R", reset_recording_instance) - print(teleop_interface) + # Create device config if not already in env_cfg + teleoperation_callbacks: dict[str, Callable[[], None]] = { + "R": reset_recording_instance, + "START": start_teleoperation, + "STOP": stop_teleoperation, + "RESET": reset_recording_instance, + } + + # For hand tracking devices, add additional callbacks + if args_cli.xr: + # Default to inactive for hand tracking + teleoperation_active = False + else: + # Always active for other devices + teleoperation_active = True + + # Create teleop device from config if present, otherwise create manually + teleop_interface = None + try: + if hasattr(env_cfg, "teleop_devices") and args_cli.teleop_device in env_cfg.teleop_devices.devices: + teleop_interface = create_teleop_device( + args_cli.teleop_device, env_cfg.teleop_devices.devices, teleoperation_callbacks + ) + else: + logger.warning( + f"No teleop device '{args_cli.teleop_device}' found in environment config. Creating default." + ) + # Create fallback teleop device + sensitivity = args_cli.sensitivity + if args_cli.teleop_device.lower() == "keyboard": + teleop_interface = Se3Keyboard( + Se3KeyboardCfg(pos_sensitivity=0.05 * sensitivity, rot_sensitivity=0.05 * sensitivity) + ) + elif args_cli.teleop_device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse( + Se3SpaceMouseCfg(pos_sensitivity=0.05 * sensitivity, rot_sensitivity=0.05 * sensitivity) + ) + elif args_cli.teleop_device.lower() == "gamepad": + teleop_interface = Se3Gamepad( + Se3GamepadCfg(pos_sensitivity=0.1 * sensitivity, rot_sensitivity=0.1 * sensitivity) + ) + else: + logger.error(f"Unsupported teleop device: {args_cli.teleop_device}") + logger.error("Configure the teleop device in the environment config.") + env.close() + simulation_app.close() + return + + # Add callbacks to fallback device + for key, callback in teleoperation_callbacks.items(): + try: + teleop_interface.add_callback(key, callback) + except (ValueError, TypeError) as e: + logger.warning(f"Failed to add callback for key {key}: {e}") + except Exception as e: + logger.error(f"Failed to create teleop device: {e}") + env.close() + simulation_app.close() + return + + if teleop_interface is None: + logger.error("Failed to create teleop interface") + env.close() + simulation_app.close() + return + + print(f"Using teleop device: {teleop_interface}") # reset environment env.reset() teleop_interface.reset() + print("Teleoperation started. Press 'R' to reset the environment.") + # simulate environment while simulation_app.is_running(): - # run everything in inference mode - with torch.inference_mode(): - # get keyboard command - delta_pose, gripper_command = teleop_interface.advance() - delta_pose = delta_pose.astype("float32") - # convert to torch - delta_pose = torch.tensor(delta_pose, device=env.device).repeat(env.num_envs, 1) - # pre-process actions - actions = pre_process_actions(delta_pose, gripper_command) - # apply actions - env.step(actions) - - if should_reset_recording_instance: - env.reset() - should_reset_recording_instance = False + try: + # run everything in inference mode + with torch.inference_mode(): + # get device command + action = teleop_interface.advance() + + # Only apply teleop commands when active + if teleoperation_active: + # process actions + actions = action.repeat(env.num_envs, 1) + # apply actions + env.step(actions) + else: + env.sim.render() + + if should_reset_recording_instance: + env.reset() + teleop_interface.reset() + should_reset_recording_instance = False + print("Environment reset complete") + except Exception as e: + logger.error(f"Error during simulation step: {e}") + break # close the simulator env.close() + print("Environment closed") if __name__ == "__main__": diff --git a/scripts/environments/zero_agent.py b/scripts/environments/zero_agent.py index db93c8ba2a60..edd9317a6287 100644 --- a/scripts/environments/zero_agent.py +++ b/scripts/environments/zero_agent.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,6 +35,8 @@ import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils import parse_env_cfg +# PLACEHOLDER: Extension template (do not remove this comment) + def main(): """Zero actions agent with Isaac Lab environment.""" diff --git a/scripts/imitation_learning/isaaclab_mimic/annotate_demos.py b/scripts/imitation_learning/isaaclab_mimic/annotate_demos.py index e5d2667a130e..a60f7913549f 100644 --- a/scripts/imitation_learning/isaaclab_mimic/annotate_demos.py +++ b/scripts/imitation_learning/isaaclab_mimic/annotate_demos.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -7,12 +7,14 @@ Script to add mimic annotations to demos to be used as source demos for mimic dataset generation. """ -# Launching Isaac Sim Simulator first. - import argparse +import math from isaaclab.app import AppLauncher +# Launching Isaac Sim Simulator first. + + # add argparse arguments parser = argparse.ArgumentParser(description="Annotate demonstrations for Isaac Lab environments.") parser.add_argument("--task", type=str, default=None, help="Name of the task.") @@ -27,11 +29,16 @@ ) parser.add_argument("--auto", action="store_true", default=False, help="Automatically annotate subtasks.") parser.add_argument( - "--signals", - type=str, - nargs="+", - default=[], - help="Sequence of subtask termination signals for all except last subtask", + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) +parser.add_argument( + "--annotate_subtask_start_signals", + action="store_true", + default=False, + help="Enable annotating start points of subtasks.", ) # append AppLauncher cli args @@ -39,6 +46,12 @@ # parse the arguments 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 + # launch the simulator app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app @@ -46,27 +59,33 @@ """Rest everything follows.""" import contextlib -import gymnasium as gym import os + +import gymnasium as gym import torch import isaaclab_mimic.envs # noqa: F401 +if args_cli.enable_pinocchio: + import isaaclab_mimic.envs.pinocchio_envs # noqa: F401 + # Only enables inputs if this script is NOT headless mode if not args_cli.headless and not os.environ.get("HEADLESS", 0): - from isaaclab.devices import Se3Keyboard + from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg + from isaaclab.envs import ManagerBasedRLMimicEnv from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg -from isaaclab.managers import RecorderTerm, RecorderTermCfg +from isaaclab.managers import RecorderTerm, RecorderTermCfg, TerminationTermCfg from isaaclab.utils import configclass -from isaaclab.utils.datasets import HDF5DatasetFileHandler +from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg is_paused = False current_action_index = 0 -subtask_indices = [] +marked_subtask_action_indices = [] +skip_episode = False def play_cb(): @@ -79,10 +98,15 @@ def pause_cb(): is_paused = True +def skip_episode_cb(): + global skip_episode + skip_episode = True + + def mark_subtask_cb(): - global current_action_index, subtask_indices - subtask_indices.append(current_action_index) - print(f"Marked subtask at action index: {current_action_index}") + global current_action_index, marked_subtask_action_indices + marked_subtask_action_indices.append(current_action_index) + print(f"Marked a subtask signal at action index: {current_action_index}") class PreStepDatagenInfoRecorder(RecorderTerm): @@ -91,7 +115,7 @@ class PreStepDatagenInfoRecorder(RecorderTerm): def record_pre_step(self): eef_pose_dict = {} for eef_name in self._env.cfg.subtask_configs.keys(): - eef_pose_dict[eef_name] = self._env.get_robot_eef_pose(eef_name) + eef_pose_dict[eef_name] = self._env.get_robot_eef_pose(eef_name=eef_name) datagen_info = { "object_pose": self._env.get_object_poses(), @@ -108,6 +132,20 @@ class PreStepDatagenInfoRecorderCfg(RecorderTermCfg): class_type: type[RecorderTerm] = PreStepDatagenInfoRecorder +class PreStepSubtaskStartsObservationsRecorder(RecorderTerm): + """Recorder term that records the subtask start observations in each step.""" + + def record_pre_step(self): + return "obs/datagen_info/subtask_start_signals", self._env.get_subtask_start_signals() + + +@configclass +class PreStepSubtaskStartsObservationsRecorderCfg(RecorderTermCfg): + """Configuration for the subtask start observations recorder term.""" + + class_type: type[RecorderTerm] = PreStepSubtaskStartsObservationsRecorder + + class PreStepSubtaskTermsObservationsRecorder(RecorderTerm): """Recorder term that records the subtask completion observations in each step.""" @@ -127,16 +165,13 @@ class MimicRecorderManagerCfg(ActionStateRecorderManagerCfg): """Mimic specific recorder terms.""" record_pre_step_datagen_info = PreStepDatagenInfoRecorderCfg() + record_pre_step_subtask_start_signals = PreStepSubtaskStartsObservationsRecorderCfg() record_pre_step_subtask_term_signals = PreStepSubtaskTermsObservationsRecorderCfg() def main(): """Add Isaac Lab Mimic annotations to the given demo dataset file.""" - global is_paused, current_action_index, subtask_indices - - if not args_cli.auto and len(args_cli.signals) == 0: - if len(args_cli.signals) == 0: - raise ValueError("Subtask signals should be provided for manual mode.") + global is_paused, current_action_index, marked_subtask_action_indices # Load input dataset to be annotated if not os.path.exists(args_cli.input_file): @@ -148,7 +183,7 @@ def main(): if episode_count == 0: print("No episodes found in the dataset.") - exit() + return 0 # get output directory path and file name (without extension) from cli arguments output_dir = os.path.dirname(args_cli.output_file) @@ -158,13 +193,13 @@ def main(): os.makedirs(output_dir) if args_cli.task is not None: - env_name = args_cli.task + 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.env_name = args_cli.task + env_cfg.env_name = env_name # extract success checking function to invoke manually success_term = None @@ -178,35 +213,73 @@ def main(): env_cfg.terminations = None # Set up recorder terms for mimic annotations - env_cfg.recorders: MimicRecorderManagerCfg = MimicRecorderManagerCfg() + env_cfg.recorders = MimicRecorderManagerCfg() if not args_cli.auto: # disable subtask term signals recorder term if in manual mode env_cfg.recorders.record_pre_step_subtask_term_signals = None + + if not args_cli.auto or (args_cli.auto and not args_cli.annotate_subtask_start_signals): + # disable subtask start signals recorder term if in manual mode or no need for subtask start annotations + env_cfg.recorders.record_pre_step_subtask_start_signals = None + env_cfg.recorders.dataset_export_dir_path = output_dir env_cfg.recorders.dataset_filename = output_file_name # create environment from loaded config - env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + env: ManagerBasedRLMimicEnv = gym.make(args_cli.task, cfg=env_cfg).unwrapped - if not isinstance(env.unwrapped, ManagerBasedRLMimicEnv): + if not isinstance(env, ManagerBasedRLMimicEnv): raise ValueError("The environment should be derived from ManagerBasedRLMimicEnv") if args_cli.auto: - # check if the mimic API env.unwrapped.get_subtask_term_signals() is implemented - if env.unwrapped.get_subtask_term_signals.__func__ is ManagerBasedRLMimicEnv.get_subtask_term_signals: + # check if the mimic API env.get_subtask_term_signals() is implemented + if env.get_subtask_term_signals.__func__ is ManagerBasedRLMimicEnv.get_subtask_term_signals: raise NotImplementedError( "The environment does not implement the get_subtask_term_signals method required " "to run automatic annotations." ) + if ( + args_cli.annotate_subtask_start_signals + and env.get_subtask_start_signals.__func__ is ManagerBasedRLMimicEnv.get_subtask_start_signals + ): + raise NotImplementedError( + "The environment does not implement the get_subtask_start_signals method required " + "to run automatic annotations." + ) + else: + # get subtask termination signal names for each eef from the environment configs + subtask_term_signal_names = {} + subtask_start_signal_names = {} + for eef_name, eef_subtask_configs in env.cfg.subtask_configs.items(): + subtask_start_signal_names[eef_name] = ( + [subtask_config.subtask_term_signal for subtask_config in eef_subtask_configs] + if args_cli.annotate_subtask_start_signals + else [] + ) + subtask_term_signal_names[eef_name] = [ + subtask_config.subtask_term_signal for subtask_config in eef_subtask_configs + ] + # Validation: if annotating start signals, every subtask (including the last) must have a name + if args_cli.annotate_subtask_start_signals: + if any(name in (None, "") for name in subtask_start_signal_names[eef_name]): + raise ValueError( + f"Missing 'subtask_term_signal' for one or more subtasks in eef '{eef_name}'. When" + " '--annotate_subtask_start_signals' is enabled, each subtask (including the last) must" + " specify 'subtask_term_signal'. The last subtask's term signal name is used as the final" + " start signal name." + ) + # no need to annotate the last subtask term signal, so remove it from the list + subtask_term_signal_names[eef_name].pop() # reset environment env.reset() # Only enables inputs if this script is NOT headless mode if not args_cli.headless and not os.environ.get("HEADLESS", 0): - keyboard_interface = Se3Keyboard(pos_sensitivity=0.1, rot_sensitivity=0.1) + keyboard_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.1, rot_sensitivity=0.1)) keyboard_interface.add_callback("N", play_cb) keyboard_interface.add_callback("B", pause_cb) + keyboard_interface.add_callback("Q", skip_episode_cb) if not args_cli.auto: keyboard_interface.add_callback("S", mark_subtask_cb) keyboard_interface.reset() @@ -214,73 +287,31 @@ def main(): # simulate environment -- run everything in inference mode exported_episode_count = 0 processed_episode_count = 0 + successful_task_count = 0 # Counter for successful task completions with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): while simulation_app.is_running() and not simulation_app.is_exiting(): # Iterate over the episodes in the loaded dataset file for episode_index, episode_name in enumerate(dataset_file_handler.get_episode_names()): processed_episode_count += 1 - subtask_indices = [] print(f"\nAnnotating episode #{episode_index} ({episode_name})") - episode = dataset_file_handler.load_episode(episode_name, env.unwrapped.device) - episode_data = episode.data - - # read initial state from the loaded episode - initial_state = episode_data["initial_state"] - env.unwrapped.recorder_manager.reset() - env.unwrapped.reset_to(initial_state, None, is_relative=True) - - # replay actions from this episode - actions = episode_data["actions"] - first_action = True - for action_index, action in enumerate(actions): - current_action_index = action_index - if first_action: - first_action = False - else: - while is_paused: - env.unwrapped.sim.render() - continue - action_tensor = torch.Tensor(action).reshape([1, action.shape[0]]) - env.step(torch.Tensor(action_tensor)) + episode = dataset_file_handler.load_episode(episode_name, env.device) is_episode_annotated_successfully = False - if not args_cli.auto: - print(f"\tSubtasks marked at action indices: {subtask_indices}") - if len(args_cli.signals) != len(subtask_indices): - raise ValueError( - f"Number of annotated subtask signals {len(subtask_indices)} should be equal " - f" to number of subtasks {len(args_cli.signals)}" - ) - annotated_episode = env.unwrapped.recorder_manager.get_episode(0) - for subtask_index in range(len(args_cli.signals)): - # subtask termination signal is false until subtask is complete, and true afterwards - subtask_signals = torch.ones(len(actions), dtype=torch.bool) - subtask_signals[: subtask_indices[subtask_index]] = False - annotated_episode.add( - f"obs/datagen_info/subtask_term_signals/{args_cli.signals[subtask_index]}", subtask_signals - ) - is_episode_annotated_successfully = True + if args_cli.auto: + is_episode_annotated_successfully = annotate_episode_in_auto_mode(env, episode, success_term) else: - # check if all the subtask term signals are annotated - annotated_episode = env.unwrapped.recorder_manager.get_episode(0) - subtask_term_signal_dict = annotated_episode.data["obs"]["datagen_info"]["subtask_term_signals"] - is_episode_annotated_successfully = True - for signal_name, signal_flags in subtask_term_signal_dict.items(): - if not torch.any(signal_flags): - is_episode_annotated_successfully = False - print(f'\tDid not detect completion for the subtask "{signal_name}".') - - if not bool(success_term.func(env, **success_term.params)[0]): - is_episode_annotated_successfully = False - print("\tThe final task was not completed.") + is_episode_annotated_successfully = annotate_episode_in_manual_mode( + env, episode, success_term, subtask_term_signal_names, subtask_start_signal_names + ) - if is_episode_annotated_successfully: + if is_episode_annotated_successfully and not skip_episode: # set success to the recorded episode data and export to file - env.unwrapped.recorder_manager.set_success_to_episodes( - None, torch.tensor([[True]], dtype=torch.bool, device=env.unwrapped.device) + env.recorder_manager.set_success_to_episodes( + None, torch.tensor([[True]], dtype=torch.bool, device=env.device) ) - env.unwrapped.recorder_manager.export_episodes() + env.recorder_manager.export_episodes() exported_episode_count += 1 + successful_task_count += 1 # Increment successful task counter print("\tExported the annotated episode.") else: print("\tSkipped exporting the episode due to incomplete subtask annotations.") @@ -290,14 +321,219 @@ def main(): f"\nExported {exported_episode_count} (out of {processed_episode_count}) annotated" f" episode{'s' if exported_episode_count > 1 else ''}." ) + print( + f"Successful task completions: {successful_task_count}" + ) # This line is used by the dataset generation test case to check if the expected number of demos were annotated print("Exiting the app.") # Close environment after annotation is complete env.close() + return successful_task_count + + +def replay_episode( + env: ManagerBasedRLMimicEnv, + episode: EpisodeData, + success_term: TerminationTermCfg | None = None, +) -> bool: + """Replays an episode in the environment. + + This function replays the given recorded episode in the environment. It can optionally check if the task + was successfully completed using a success termination condition input. + + Args: + env: The environment to replay the episode in. + episode: The recorded episode data to replay. + success_term: Optional termination term to check for task success. + + Returns: + True if the episode was successfully replayed and the success condition was met (if provided), + False otherwise. + """ + global current_action_index, skip_episode, is_paused + # read initial state and actions from the loaded episode + initial_state = episode.data["initial_state"] + actions = episode.data["actions"] + env.sim.reset() + env.recorder_manager.reset() + env.reset_to(initial_state, None, is_relative=True) + first_action = True + for action_index, action in enumerate(actions): + current_action_index = action_index + if first_action: + first_action = False + else: + while is_paused or skip_episode: + env.sim.render() + if skip_episode: + return False + continue + action_tensor = torch.Tensor(action).reshape([1, action.shape[0]]) + env.step(torch.Tensor(action_tensor)) + if success_term is not None: + if not bool(success_term.func(env, **success_term.params)[0]): + return False + return True + + +def annotate_episode_in_auto_mode( + env: ManagerBasedRLMimicEnv, + episode: EpisodeData, + success_term: TerminationTermCfg | None = None, +) -> bool: + """Annotates an episode in automatic mode. + + This function replays the given episode in the environment and checks if the task was successfully completed. + If the task was not completed, it will print a message and return False. Otherwise, it will check if all the + subtask term signals are annotated and return True if they are, False otherwise. + + Args: + env: The environment to replay the episode in. + episode: The recorded episode data to replay. + success_term: Optional termination term to check for task success. + + Returns: + True if the episode was successfully annotated, False otherwise. + """ + global skip_episode + skip_episode = False + is_episode_annotated_successfully = replay_episode(env, episode, success_term) + if skip_episode: + print("\tSkipping the episode.") + return False + if not is_episode_annotated_successfully: + print("\tThe final task was not completed.") + else: + # check if all the subtask term signals are annotated + annotated_episode = env.recorder_manager.get_episode(0) + subtask_term_signal_dict = annotated_episode.data["obs"]["datagen_info"]["subtask_term_signals"] + for signal_name, signal_flags in subtask_term_signal_dict.items(): + signal_flags = torch.tensor(signal_flags, device=env.device) + if not torch.any(signal_flags): + is_episode_annotated_successfully = False + print(f'\tDid not detect completion for the subtask "{signal_name}".') + if args_cli.annotate_subtask_start_signals: + subtask_start_signal_dict = annotated_episode.data["obs"]["datagen_info"]["subtask_start_signals"] + for signal_name, signal_flags in subtask_start_signal_dict.items(): + if not torch.any(signal_flags): + is_episode_annotated_successfully = False + print(f'\tDid not detect start for the subtask "{signal_name}".') + return is_episode_annotated_successfully + + +def annotate_episode_in_manual_mode( + env: ManagerBasedRLMimicEnv, + episode: EpisodeData, + success_term: TerminationTermCfg | None = None, + subtask_term_signal_names: dict[str, list[str]] = {}, + subtask_start_signal_names: dict[str, list[str]] = {}, +) -> bool: + """Annotates an episode in manual mode. + + This function replays the given episode in the environment and allows for manual marking of subtask term signals. + It iterates over each eef and prompts the user to mark the subtask term signals for that eef. + + Args: + env: The environment to replay the episode in. + episode: The recorded episode data to replay. + success_term: Optional termination term to check for task success. + subtask_term_signal_names: Dictionary mapping eef names to lists of subtask term signal names. + subtask_start_signal_names: Dictionary mapping eef names to lists of subtask start signal names. + Returns: + True if the episode was successfully annotated, False otherwise. + """ + global is_paused, marked_subtask_action_indices, skip_episode + # iterate over the eefs for marking subtask term signals + subtask_term_signal_action_indices = {} + subtask_start_signal_action_indices = {} + for eef_name, eef_subtask_term_signal_names in subtask_term_signal_names.items(): + eef_subtask_start_signal_names = subtask_start_signal_names[eef_name] + # skip if no subtask annotation is needed for this eef + if len(eef_subtask_term_signal_names) == 0 and len(eef_subtask_start_signal_names) == 0: + continue + + while True: + is_paused = True + skip_episode = False + print(f'\tPlaying the episode for subtask annotations for eef "{eef_name}".') + print("\tSubtask signals to annotate:") + if len(eef_subtask_start_signal_names) > 0: + print(f"\t\t- Start:\t{eef_subtask_start_signal_names}") + print(f"\t\t- Termination:\t{eef_subtask_term_signal_names}") + + print('\n\tPress "N" to begin.') + print('\tPress "B" to pause.') + print('\tPress "S" to annotate subtask signals.') + print('\tPress "Q" to skip the episode.\n') + marked_subtask_action_indices = [] + task_success_result = replay_episode(env, episode, success_term) + if skip_episode: + print("\tSkipping the episode.") + return False + + print(f"\tSubtasks marked at action indices: {marked_subtask_action_indices}") + expected_subtask_signal_count = len(eef_subtask_term_signal_names) + len(eef_subtask_start_signal_names) + if task_success_result and expected_subtask_signal_count == len(marked_subtask_action_indices): + print(f'\tAll {expected_subtask_signal_count} subtask signals for eef "{eef_name}" were annotated.') + for marked_signal_index in range(expected_subtask_signal_count): + if args_cli.annotate_subtask_start_signals and marked_signal_index % 2 == 0: + subtask_start_signal_action_indices[ + eef_subtask_start_signal_names[int(marked_signal_index / 2)] + ] = marked_subtask_action_indices[marked_signal_index] + if not args_cli.annotate_subtask_start_signals: + # Direct mapping when only collecting termination signals + subtask_term_signal_action_indices[eef_subtask_term_signal_names[marked_signal_index]] = ( + marked_subtask_action_indices[marked_signal_index] + ) + elif args_cli.annotate_subtask_start_signals and marked_signal_index % 2 == 1: + # Every other signal is a termination when collecting both types + subtask_term_signal_action_indices[ + eef_subtask_term_signal_names[math.floor(marked_signal_index / 2)] + ] = marked_subtask_action_indices[marked_signal_index] + break + + if not task_success_result: + print("\tThe final task was not completed.") + return False + + if expected_subtask_signal_count != len(marked_subtask_action_indices): + print( + f"\tOnly {len(marked_subtask_action_indices)} out of" + f' {expected_subtask_signal_count} subtask signals for eef "{eef_name}" were' + " annotated." + ) + + print(f'\tThe episode will be replayed again for re-marking subtask signals for the eef "{eef_name}".\n') + + annotated_episode = env.recorder_manager.get_episode(0) + for ( + subtask_term_signal_name, + subtask_term_signal_action_index, + ) in subtask_term_signal_action_indices.items(): + # subtask termination signal is false until subtask is complete, and true afterwards + subtask_signals = torch.ones(len(episode.data["actions"]), dtype=torch.bool) + subtask_signals[:subtask_term_signal_action_index] = False + annotated_episode.add(f"obs/datagen_info/subtask_term_signals/{subtask_term_signal_name}", subtask_signals) + + if args_cli.annotate_subtask_start_signals: + for ( + subtask_start_signal_name, + subtask_start_signal_action_index, + ) in subtask_start_signal_action_indices.items(): + subtask_signals = torch.ones(len(episode.data["actions"]), dtype=torch.bool) + subtask_signals[:subtask_start_signal_action_index] = False + annotated_episode.add( + f"obs/datagen_info/subtask_start_signals/{subtask_start_signal_name}", subtask_signals + ) + + return True + if __name__ == "__main__": # run the main function - main() + successful_task_count = main() # close sim app simulation_app.close() + # exit with the number of successful task completions as return code + exit(successful_task_count) diff --git a/scripts/imitation_learning/isaaclab_mimic/consolidated_demo.py b/scripts/imitation_learning/isaaclab_mimic/consolidated_demo.py index 38e297d5daa5..d180dffd7ccf 100644 --- a/scripts/imitation_learning/isaaclab_mimic/consolidated_demo.py +++ b/scripts/imitation_learning/isaaclab_mimic/consolidated_demo.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -73,14 +73,15 @@ import asyncio import contextlib -import gymnasium as gym -import numpy as np import os import random import time + +import gymnasium as gym +import numpy as np import torch -from isaaclab.devices import Se3Keyboard, Se3SpaceMouse +from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg from isaaclab.envs import ManagerBasedRLMimicEnv from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg from isaaclab.managers import DatasetExportMode, RecorderTerm, RecorderTermCfg @@ -198,9 +199,9 @@ async def run_teleop_robot( # create controller if needed if teleop_interface is None: if args_cli.teleop_device.lower() == "keyboard": - teleop_interface = Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5) + teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) elif args_cli.teleop_device.lower() == "spacemouse": - teleop_interface = Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5) + teleop_interface = Se3SpaceMouse(Se3SpaceMouseCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) else: raise ValueError( f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse'." @@ -301,7 +302,6 @@ def env_loop(env, env_action_queue, shared_datagen_info_pool, asyncio_event_loop is_first_print = True with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): while True: - actions = torch.zeros(env.unwrapped.action_space.shape) # get actions from all the data generators @@ -361,7 +361,7 @@ def main(): # get the environment name if args_cli.task is not None: - env_name = args_cli.task + env_name = args_cli.task.split(":")[-1] elif args_cli.input_file: # if the environment name is not specified, try to get it from the dataset file dataset_file_handler = HDF5DatasetFileHandler() @@ -401,7 +401,7 @@ def main(): env_cfg.recorders.dataset_export_mode = DatasetExportMode.EXPORT_SUCCEEDED_ONLY # create environment - env = gym.make(env_name, cfg=env_cfg) + env = gym.make(args_cli.task, cfg=env_cfg) if not isinstance(env.unwrapped, ManagerBasedRLMimicEnv): raise ValueError("The environment should be derived from ManagerBasedRLMimicEnv") diff --git a/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py b/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py index 37e993751f8c..527792ea9038 100644 --- a/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py +++ b/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -7,7 +7,7 @@ Main data generation script. """ -# Launching Isaac Sim Simulator first. +"""Launch Isaac Sim Simulator first.""" import argparse @@ -32,11 +32,29 @@ action="store_true", help="pause after every subtask during generation for debugging - only useful with render flag", ) +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) +parser.add_argument( + "--use_skillgen", + action="store_true", + default=False, + help="use skillgen to generate motion trajectories", +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments 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 + # launch the simulator app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app @@ -44,24 +62,39 @@ """Rest everything follows.""" import asyncio +import inspect +import logging +import random + import gymnasium as gym import numpy as np -import random import torch +from isaaclab.envs import ManagerBasedRLMimicEnv + import isaaclab_mimic.envs # noqa: F401 + +if args_cli.enable_pinocchio: + import isaaclab_mimic.envs.pinocchio_envs # noqa: F401 + from isaaclab_mimic.datagen.generation import env_loop, setup_async_generation, setup_env_config from isaaclab_mimic.datagen.utils import get_env_name_from_dataset, setup_output_paths import isaaclab_tasks # noqa: F401 +# import logger +logger = logging.getLogger(__name__) + def main(): num_envs = args_cli.num_envs # Setup output paths and get env name output_dir, output_file_name = setup_output_paths(args_cli.output_file) - env_name = args_cli.task or get_env_name_from_dataset(args_cli.input_file) + task_name = args_cli.task + if task_name: + task_name = args_cli.task.split(":")[-1] + env_name = task_name or get_env_name_from_dataset(args_cli.input_file) # Configure environment env_cfg, success_term = setup_env_config( @@ -73,17 +106,55 @@ def main(): generation_num_trials=args_cli.generation_num_trials, ) - # create environment - env = gym.make(env_name, cfg=env_cfg) + # Create environment + env = gym.make(env_name, cfg=env_cfg).unwrapped + + if not isinstance(env, ManagerBasedRLMimicEnv): + raise ValueError("The environment should be derived from ManagerBasedRLMimicEnv") - # set seed for generation - random.seed(env.unwrapped.cfg.datagen_config.seed) - np.random.seed(env.unwrapped.cfg.datagen_config.seed) - torch.manual_seed(env.unwrapped.cfg.datagen_config.seed) + # Check if the mimic API from this environment contains decprecated signatures + if "action_noise_dict" not in inspect.signature(env.target_eef_pose_to_action).parameters: + logger.warning( + f'The "noise" parameter in the "{env_name}" environment\'s mimic API "target_eef_pose_to_action", ' + "is deprecated. Please update the API to take action_noise_dict instead." + ) - # reset before starting + # Set seed for generation + random.seed(env.cfg.datagen_config.seed) + np.random.seed(env.cfg.datagen_config.seed) + torch.manual_seed(env.cfg.datagen_config.seed) + + # Reset before starting env.reset() + motion_planners = None + if args_cli.use_skillgen: + from isaaclab_mimic.motion_planners.curobo.curobo_planner import CuroboPlanner + from isaaclab_mimic.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg + + # Create one motion planner per environment + motion_planners = {} + for env_id in range(num_envs): + print(f"Initializing motion planner for environment {env_id}") + # Create a config instance from the task name + planner_config = CuroboPlannerCfg.from_task_name(env_name) + + # Ensure visualization is only enabled for the first environment + # If not, sphere and plan visualization will be too slow in isaac lab + # It is efficient to visualize the spheres and plan for the first environment in rerun + if env_id != 0: + planner_config.visualize_spheres = False + planner_config.visualize_plan = False + + motion_planners[env_id] = CuroboPlanner( + env=env, + robot=env.scene["robot"], + config=planner_config, # Pass the config object + env_id=env_id, # Pass environment ID + ) + + env.cfg.datagen_config.use_skillgen = True + # Setup and run async data generation async_components = setup_async_generation( env=env, @@ -91,13 +162,38 @@ def main(): input_file=args_cli.input_file, success_term=success_term, pause_subtask=args_cli.pause_subtask, + motion_planners=motion_planners, # Pass the motion planners dictionary ) try: - asyncio.ensure_future(asyncio.gather(*async_components["tasks"])) - env_loop(env, async_components["action_queue"], async_components["info_pool"], async_components["event_loop"]) + data_gen_tasks = asyncio.ensure_future(asyncio.gather(*async_components["tasks"])) + env_loop( + env, + async_components["reset_queue"], + async_components["action_queue"], + async_components["info_pool"], + async_components["event_loop"], + ) except asyncio.CancelledError: print("Tasks were cancelled.") + finally: + # Cancel all async tasks when env_loop finishes + data_gen_tasks.cancel() + try: + # Wait for tasks to be cancelled + async_components["event_loop"].run_until_complete(data_gen_tasks) + except asyncio.CancelledError: + print("Remaining async tasks cancelled and cleaned up.") + except Exception as e: + print(f"Error cancelling remaining async tasks: {e}") + # Cleanup of motion planners and their visualizers + if motion_planners is not None: + for env_id, planner in motion_planners.items(): + if getattr(planner, "plan_visualizer", None) is not None: + print(f"Closing plan visualizer for environment {env_id}") + planner.plan_visualizer.close() + planner.plan_visualizer = None + motion_planners.clear() if __name__ == "__main__": @@ -105,5 +201,5 @@ def main(): main() except KeyboardInterrupt: print("\nProgram interrupted by user. Exiting...") - # close sim app + # Close sim app simulation_app.close() 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 000000000000..4999f2d3fefc --- /dev/null +++ b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py @@ -0,0 +1,774 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 random + +import gymnasium as gym +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 000000000000..e65059d7d65a --- /dev/null +++ b/scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py @@ -0,0 +1,110 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 os + +import h5py +import matplotlib.pyplot as plt + + +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 31d8d3ae7343..f663bc3acb2b 100644 --- a/scripts/imitation_learning/robomimic/play.py +++ b/scripts/imitation_learning/robomimic/play.py @@ -1,12 +1,25 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""Script to play and evaluate a trained policy from robomimic.""" +"""Script to play and evaluate a trained policy from robomimic. + +This script loads a robomimic policy and plays it in an Isaac Lab environment. + +Args: + task: Name of the environment. + checkpoint: Path to the robomimic policy checkpoint. + horizon: If provided, override the step horizon of each rollout. + num_rollouts: If provided, override the number of rollouts. + seed: If provided, overeride the default random seed. + norm_factor_min: If provided, minimum value of the action space normalization factor. + norm_factor_max: If provided, maximum value of the action space normalization factor. +""" """Launch Isaac Sim Simulator first.""" + import argparse from isaaclab.app import AppLauncher @@ -21,41 +34,94 @@ parser.add_argument("--horizon", type=int, default=800, help="Step horizon of each rollout.") parser.add_argument("--num_rollouts", type=int, default=1, help="Number of rollouts.") parser.add_argument("--seed", type=int, default=101, help="Random seed.") +parser.add_argument( + "--norm_factor_min", type=float, default=None, help="Optional: minimum value of the normalization factor." +) +parser.add_argument( + "--norm_factor_max", type=float, default=None, help="Optional: maximum value of the normalization factor." +) +parser.add_argument("--enable_pinocchio", default=False, action="store_true", help="Enable Pinocchio.") + # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments 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 + # launch omniverse app app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app """Rest everything follows.""" -import gymnasium as gym -import torch +import copy +import random +import gymnasium as gym +import numpy as np import robomimic.utils.file_utils as FileUtils import robomimic.utils.torch_utils as TorchUtils +import torch + +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 from isaaclab_tasks.utils import parse_env_cfg -def rollout(policy, env, horizon, device): - policy.start_episode +def rollout(policy, env, success_term, horizon, device): + """Perform a single rollout of the policy in the environment. + + Args: + policy: The robomimicpolicy to play. + env: The environment to play in. + horizon: The step horizon of each rollout. + device: The device to run the policy on. + + Returns: + terminated: Whether the rollout terminated. + traj: The trajectory of the rollout. + """ + policy.start_episode() obs_dict, _ = env.reset() traj = dict(actions=[], obs=[], next_obs=[]) for i in range(horizon): # Prepare observations - obs = obs_dict["policy"] + obs = copy.deepcopy(obs_dict["policy"]) for ob in obs: obs[ob] = torch.squeeze(obs[ob]) + + # Check if environment image observations + if hasattr(env.cfg, "image_obs_list"): + # Process image observations for robomimic inference + for image_name in env.cfg.image_obs_list: + if image_name in obs_dict["policy"].keys(): + # Convert from chw uint8 to hwc normalized float + image = torch.squeeze(obs_dict["policy"][image_name]) + image = image.permute(2, 0, 1).clone().float() + image = image / 255.0 + image = image.clip(0.0, 1.0) + obs[image_name] = image + traj["obs"].append(obs) # Compute actions actions = policy(obs) + + # Unnormalize actions + if args_cli.norm_factor_min is not None and args_cli.norm_factor_max is not None: + actions = ( + (actions + 1) * (args_cli.norm_factor_max - args_cli.norm_factor_min) + ) / 2 + args_cli.norm_factor_min + actions = torch.from_numpy(actions).to(device=device).view(1, env.action_space.shape[1]) # Apply actions @@ -66,9 +132,10 @@ def rollout(policy, env, horizon, device): traj["actions"].append(actions.tolist()) traj["next_obs"].append(obs) - if terminated: + # Check if rollout was successful + if bool(success_term.func(env, **success_term.params)[0]): return True, traj - elif truncated: + elif terminated or truncated: return False, traj return False, traj @@ -88,24 +155,28 @@ def main(): # Disable recorder env_cfg.recorders = None + # Extract success checking function + success_term = env_cfg.terminations.success + env_cfg.terminations.success = None + # Create environment env = gym.make(args_cli.task, cfg=env_cfg).unwrapped # Set seed torch.manual_seed(args_cli.seed) + np.random.seed(args_cli.seed) + random.seed(args_cli.seed) env.seed(args_cli.seed) # Acquire device device = TorchUtils.get_torch_device(try_to_use_cuda=True) - # Load policy - policy, _ = FileUtils.policy_from_checkpoint(ckpt_path=args_cli.checkpoint, device=device, verbose=True) - # Run policy results = [] for trial in range(args_cli.num_rollouts): print(f"[INFO] Starting trial {trial}") - terminated, traj = rollout(policy, env, args_cli.horizon, device) + policy, _ = FileUtils.policy_from_checkpoint(ckpt_path=args_cli.checkpoint, device=device) + terminated, traj = rollout(policy, env, success_term, args_cli.horizon, device) results.append(terminated) print(f"[INFO] Trial {trial}: {terminated}\n") diff --git a/scripts/imitation_learning/robomimic/robust_eval.py b/scripts/imitation_learning/robomimic/robust_eval.py new file mode 100644 index 000000000000..0e1e9014ba92 --- /dev/null +++ b/scripts/imitation_learning/robomimic/robust_eval.py @@ -0,0 +1,335 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to evaluate a trained policy from robomimic across multiple evaluation settings. + +This script loads a trained robomimic policy and evaluates it in an Isaac Lab environment +across multiple evaluation settings (lighting, textures, etc.) and seeds. It saves the results +to a specified output directory. + +Args: + task: Name of the environment. + input_dir: Directory containing the model checkpoints to evaluate. + horizon: Step horizon of each rollout. + num_rollouts: Number of rollouts per model per setting. + num_seeds: Number of random seeds to evaluate. + seeds: Optional list of specific seeds to use instead of random ones. + log_dir: Directory to write results to. + log_file: Name of the output file. + output_vis_file: File path to export recorded episodes. + norm_factor_min: If provided, minimum value of the action space normalization factor. + norm_factor_max: If provided, maximum value of the action space normalization factor. + disable_fabric: Whether to disable fabric and use USD I/O operations. +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Evaluate robomimic policy for Isaac Lab environment.") +parser.add_argument( + "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations." +) +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--input_dir", type=str, default=None, help="Directory containing models to evaluate.") +parser.add_argument( + "--start_epoch", type=int, default=100, help="Epoch of the checkpoint to start the evaluation from." +) +parser.add_argument("--horizon", type=int, default=400, help="Step horizon of each rollout.") +parser.add_argument("--num_rollouts", type=int, default=15, help="Number of rollouts for each setting.") +parser.add_argument("--num_seeds", type=int, default=3, help="Number of random seeds to evaluate.") +parser.add_argument("--seeds", nargs="+", type=int, default=None, help="List of specific seeds to use.") +parser.add_argument( + "--log_dir", type=str, default="/tmp/policy_evaluation_results", help="Directory to write results to." +) +parser.add_argument("--log_file", type=str, default="results", help="Name of output file.") +parser.add_argument( + "--output_vis_file", type=str, default="visuals.hdf5", help="File path to export recorded episodes." +) +parser.add_argument( + "--norm_factor_min", type=float, default=None, help="Optional: minimum value of the normalization factor." +) +parser.add_argument( + "--norm_factor_max", type=float, default=None, help="Optional: maximum value of the normalization factor." +) +parser.add_argument("--enable_pinocchio", default=False, action="store_true", help="Enable Pinocchio.") + +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +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 + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import copy +import os +import pathlib +import random + +import gymnasium as gym +import robomimic.utils.file_utils as FileUtils +import robomimic.utils.torch_utils as TorchUtils +import torch + +from isaaclab_tasks.utils import parse_env_cfg + + +def rollout(policy, env: gym.Env, success_term, horizon: int, device: torch.device) -> tuple[bool, dict]: + """Perform a single rollout of the policy in the environment. + + Args: + policy: The robomimic policy to evaluate. + env: The environment to evaluate in. + horizon: The step horizon of each rollout. + device: The device to run the policy on. + args_cli: Command line arguments containing normalization factors. + + Returns: + terminated: Whether the rollout terminated successfully. + traj: The trajectory of the rollout. + """ + policy.start_episode() + obs_dict, _ = env.reset() + traj = dict(actions=[], obs=[], next_obs=[]) + + for _ in range(horizon): + # Prepare policy observations + obs = copy.deepcopy(obs_dict["policy"]) + for ob in obs: + obs[ob] = torch.squeeze(obs[ob]) + + # Check if environment image observations + if hasattr(env.cfg, "image_obs_list"): + # Process image observations for robomimic inference + for image_name in env.cfg.image_obs_list: + if image_name in obs_dict["policy"].keys(): + # Convert from chw uint8 to hwc normalized float + image = torch.squeeze(obs_dict["policy"][image_name]) + image = image.permute(2, 0, 1).clone().float() + image = image / 255.0 + image = image.clip(0.0, 1.0) + obs[image_name] = image + + traj["obs"].append(obs) + + # Compute actions + actions = policy(obs) + + # Unnormalize actions if normalization factors are provided + if args_cli.norm_factor_min is not None and args_cli.norm_factor_max is not None: + actions = ( + (actions + 1) * (args_cli.norm_factor_max - args_cli.norm_factor_min) + ) / 2 + args_cli.norm_factor_min + + actions = torch.from_numpy(actions).to(device=device).view(1, env.action_space.shape[1]) + + # Apply actions + obs_dict, _, terminated, truncated, _ = env.step(actions) + obs = obs_dict["policy"] + + # Record trajectory + traj["actions"].append(actions.tolist()) + traj["next_obs"].append(obs) + + if bool(success_term.func(env, **success_term.params)[0]): + return True, traj + elif terminated or truncated: + return False, traj + + return False, traj + + +def evaluate_model( + model_path: str, + env: gym.Env, + device: torch.device, + success_term, + num_rollouts: int, + horizon: int, + seed: int, + output_file: str, +) -> float: + """Evaluate a single model checkpoint across multiple rollouts. + + Args: + model_path: Path to the model checkpoint. + env: The environment to evaluate in. + device: The device to run the policy on. + num_rollouts: Number of rollouts to perform. + horizon: Step horizon of each rollout. + seed: Random seed to use. + output_file: File to write results to. + + Returns: + float: Success rate of the model + """ + # Set seed + torch.manual_seed(seed) + env.seed(seed) + random.seed(seed) + + # Load policy + policy, _ = FileUtils.policy_from_checkpoint(ckpt_path=model_path, device=device, verbose=False) + + # Run policy + results = [] + for trial in range(num_rollouts): + print(f"[Model: {os.path.basename(model_path)}] Starting trial {trial}") + terminated, _ = rollout(policy, env, success_term, horizon, device) + results.append(terminated) + with open(output_file, "a") as file: + file.write(f"[Model: {os.path.basename(model_path)}] Trial {trial}: {terminated}\n") + print(f"[Model: {os.path.basename(model_path)}] Trial {trial}: {terminated}") + + # Calculate and log results + success_rate = results.count(True) / len(results) + with open(output_file, "a") as file: + file.write( + f"[Model: {os.path.basename(model_path)}] Successful trials: {results.count(True)}, out of" + f" {len(results)} trials\n" + ) + file.write(f"[Model: {os.path.basename(model_path)}] Success rate: {success_rate}\n") + file.write(f"[Model: {os.path.basename(model_path)}] Results: {results}\n") + file.write("-" * 80 + "\n\n") + + print( + f"\n[Model: {os.path.basename(model_path)}] Successful trials: {results.count(True)}, out of" + f" {len(results)} trials" + ) + print(f"[Model: {os.path.basename(model_path)}] Success rate: {success_rate}\n") + print(f"[Model: {os.path.basename(model_path)}] Results: {results}\n") + + return success_rate + + +def main() -> None: + """Run evaluation of trained policies from robomimic with Isaac Lab environment.""" + # Parse configuration + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1, use_fabric=not args_cli.disable_fabric) + + # Set observations to dictionary mode for Robomimic + env_cfg.observations.policy.concatenate_terms = False + + # Set termination conditions + env_cfg.terminations.time_out = None + + # Disable recorder + env_cfg.recorders = None + + # Extract success checking function + success_term = env_cfg.terminations.success + env_cfg.terminations.success = None + + # Set evaluation settings + env_cfg.eval_mode = True + + # Create environment + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + + # Acquire device + device = TorchUtils.get_torch_device(try_to_use_cuda=False) + + # Get model checkpoints + model_checkpoints = [f.name for f in os.scandir(args_cli.input_dir) if f.is_file()] + + # Set up seeds + seeds = random.sample(range(0, 10000), args_cli.num_seeds) if args_cli.seeds is None else args_cli.seeds + + # Define evaluation settings + settings = ["vanilla", "light_intensity", "light_color", "light_texture", "table_texture", "robot_texture", "all"] + + # Create log directory if it doesn't exist + os.makedirs(args_cli.log_dir, exist_ok=True) + + # Evaluate each seed + for seed in seeds: + output_path = os.path.join(args_cli.log_dir, f"{args_cli.log_file}_seed_{seed}") + path = pathlib.Path(output_path) + path.parent.mkdir(parents=True, exist_ok=True) + + # Initialize results summary + results_summary = dict() + results_summary["overall"] = {} + for setting in settings: + results_summary[setting] = {} + + with open(output_path, "w") as file: + # Evaluate each setting + for setting in settings: + env.cfg.eval_type = setting + + file.write(f"Evaluation setting: {setting}\n") + file.write("=" * 80 + "\n\n") + + print(f"Evaluation setting: {setting}") + print("=" * 80) + + # Evaluate each model + for model in model_checkpoints: + # Skip early checkpoints + model_epoch = int(model.split(".")[0].split("_")[-1]) + if model_epoch < args_cli.start_epoch: + continue + + model_path = os.path.join(args_cli.input_dir, model) + success_rate = evaluate_model( + model_path=model_path, + env=env, + device=device, + success_term=success_term, + num_rollouts=args_cli.num_rollouts, + horizon=args_cli.horizon, + seed=seed, + output_file=output_path, + ) + + # Store results + results_summary[setting][model] = success_rate + if model not in results_summary["overall"].keys(): + results_summary["overall"][model] = 0.0 + results_summary["overall"][model] += success_rate + + env.reset() + + file.write("=" * 80 + "\n\n") + env.reset() + + # Calculate overall success rates + for model in results_summary["overall"].keys(): + results_summary["overall"][model] /= len(settings) + + # Write final summary + file.write("\nResults Summary (success rate):\n") + for setting in results_summary.keys(): + file.write(f"\nSetting: {setting}\n") + for model in results_summary[setting].keys(): + file.write(f"{model}: {results_summary[setting][model]}\n") + max_key = max(results_summary[setting], key=results_summary[setting].get) + file.write( + f"\nBest model for setting {setting} is {max_key} with success rate" + f" {results_summary[setting][max_key]}\n" + ) + + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/imitation_learning/robomimic/train.py b/scripts/imitation_learning/robomimic/train.py index 3c0a17f0df3f..11dd9814de6d 100644 --- a/scripts/imitation_learning/robomimic/train.py +++ b/scripts/imitation_learning/robomimic/train.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -28,14 +28,19 @@ """ The main entry point for training policies from pre-collected data. -Args: - algo: name of the algorithm to run. - task: name of the environment. - name: if provided, override the experiment name defined in the config - dataset: if provided, override the dataset path defined in the config - -This file has been modified from the original version in the following ways: +This script loads dataset(s), creates a model based on the algorithm specified, +and trains the model. It supports training on various environments with multiple +algorithms from robomimic. +Args: + algo: Name of the algorithm to run. + task: Name of the environment. + name: If provided, override the experiment name defined in the config. + dataset: If provided, override the dataset path defined in the config. + log_dir: Directory to save logs. + normalize_training_actions: Whether to normalize actions in the training data. + +This file has been modified from the original robomimic version to integrate with IsaacLab. """ """Launch Isaac Sim Simulator first.""" @@ -49,33 +54,91 @@ """Rest everything follows.""" import argparse -import gymnasium as gym +import importlib import json -import numpy as np import os +import shutil import sys import time -import torch import traceback from collections import OrderedDict -from torch.utils.data import DataLoader +import gymnasium as gym +import h5py +import numpy as np import psutil import robomimic.utils.env_utils as EnvUtils import robomimic.utils.file_utils as FileUtils import robomimic.utils.obs_utils as ObsUtils import robomimic.utils.torch_utils as TorchUtils import robomimic.utils.train_utils as TrainUtils +import torch from robomimic.algo import algo_factory -from robomimic.config import config_factory +from robomimic.config import Config, config_factory from robomimic.utils.log_utils import DataLogger, PrintLogger +from torch.utils.data import DataLoader -# Needed so that environment is registered import isaaclab_tasks # noqa: F401 - - -def train(config, device): - """Train a model using the algorithm.""" +import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 +import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + + +def normalize_hdf5_actions(config: Config, log_dir: str) -> str: + """Normalizes actions in hdf5 dataset to [-1, 1] range. + + Args: + config: The configuration object containing dataset path. + log_dir: Directory to save normalization parameters. + + Returns: + Path to the normalized dataset. + """ + base, ext = os.path.splitext(config.train.data) + normalized_path = base + "_normalized" + ext + + # Copy the original dataset + print(f"Creating normalized dataset at {normalized_path}") + shutil.copyfile(config.train.data, normalized_path) + + # Open the new dataset and normalize the actions + with h5py.File(normalized_path, "r+") as f: + dataset_paths = [f"/data/demo_{str(i)}/actions" for i in range(len(f["data"].keys()))] + + # Compute the min and max of the dataset + dataset = np.array(f[dataset_paths[0]]).flatten() + for i, path in enumerate(dataset_paths): + if i != 0: + data = np.array(f[path]).flatten() + dataset = np.append(dataset, data) + + max = np.max(dataset) + min = np.min(dataset) + + # Normalize the actions + for i, path in enumerate(dataset_paths): + data = np.array(f[path]) + normalized_data = 2 * ((data - min) / (max - min)) - 1 # Scale to [-1, 1] range + del f[path] + f[path] = normalized_data + + # Save the min and max values to log directory + with open(os.path.join(log_dir, "normalization_params.txt"), "w") as f: + f.write(f"min: {min}\n") + f.write(f"max: {max}\n") + + return normalized_path + + +def train(config: Config, device: str, log_dir: str, ckpt_dir: str, video_dir: str): + """Train a model using the algorithm specified in config. + + Args: + config: Configuration object. + device: PyTorch device to use for training. + log_dir: Directory to save logs. + ckpt_dir: Directory to save checkpoints. + video_dir: Directory to save videos. + """ # first set seeds np.random.seed(config.train.seed) torch.manual_seed(config.train.seed) @@ -83,7 +146,6 @@ def train(config, device): print("\n============= New Training Run with Config =============") print(config) print("") - log_dir, ckpt_dir, video_dir = TrainUtils.get_exp_dir(config) print(f">>> Saving logs into directory: {log_dir}") print(f">>> Saving checkpoints into directory: {ckpt_dir}") @@ -219,7 +281,8 @@ def train(config, device): and (epoch % config.experiment.save.every_n_epochs == 0) ) epoch_list_check = epoch in config.experiment.save.epochs - should_save_ckpt = time_check or epoch_check or epoch_list_check + last_epoch_check = epoch == config.train.num_epochs + should_save_ckpt = time_check or epoch_check or epoch_list_check or last_epoch_check ckpt_reason = None if should_save_ckpt: last_ckpt_time = time.time() @@ -278,25 +341,41 @@ def train(config, device): data_logger.close() -def main(args): - """Train a model on a task using a specified algorithm.""" +def main(args: argparse.Namespace): + """Train a model on a task using a specified algorithm. + + Args: + args: Command line arguments. + """ # load config if args.task is not None: # obtain the configuration entry point cfg_entry_point_key = f"robomimic_{args.algo}_cfg_entry_point" + task_name = args.task.split(":")[-1] - print(f"Loading configuration for task: {args.task}") + print(f"Loading configuration for task: {task_name}") print(gym.envs.registry.keys()) print(" ") - cfg_entry_point_file = gym.spec(args.task).kwargs.pop(cfg_entry_point_key) + cfg_entry_point_file = gym.spec(task_name).kwargs.pop(cfg_entry_point_key) # check if entry point exists if cfg_entry_point_file is None: raise ValueError( - f"Could not find configuration for the environment: '{args.task}'." + f"Could not find configuration for the environment: '{task_name}'." f" Please check that the gym registry has the entry point: '{cfg_entry_point_key}'." ) - with open(cfg_entry_point_file) as f: + # resolve module path if needed + if ":" in cfg_entry_point_file: + mod_name, file_name = cfg_entry_point_file.split(":") + mod = importlib.import_module(mod_name) + if mod.__file__ is None: + raise ValueError(f"Could not find module file for: '{mod_name}'") + mod_path = os.path.dirname(mod.__file__) + config_file = os.path.join(mod_path, file_name) + else: + config_file = cfg_entry_point_file + + with open(config_file) as f: ext_cfg = json.load(f) config = config_factory(ext_cfg["algo_name"]) # update config with external json - this will throw errors if @@ -312,9 +391,17 @@ def main(args): if args.name is not None: config.experiment.name = args.name + if args.epochs is not None: + config.train.num_epochs = args.epochs + # change location of experiment directory config.train.output_dir = os.path.abspath(os.path.join("./logs", args.log_dir, args.task)) + log_dir, ckpt_dir, video_dir = TrainUtils.get_exp_dir(config) + + if args.normalize_training_actions: + config.train.data = normalize_hdf5_actions(config, log_dir) + # get torch device device = TorchUtils.get_torch_device(try_to_use_cuda=config.train.cuda) @@ -323,7 +410,7 @@ def main(args): # catch error during training and print it res_str = "finished run successfully!" try: - train(config, device=device) + train(config, device, log_dir, ckpt_dir, video_dir) except Exception as e: res_str = f"run failed with error:\n{e}\n\n{traceback.format_exc()}" print(res_str) @@ -351,6 +438,16 @@ def main(args): parser.add_argument("--task", type=str, default=None, help="Name of the task.") parser.add_argument("--algo", type=str, default=None, help="Name of the algorithm.") parser.add_argument("--log_dir", type=str, default="robomimic", help="Path to log directory") + parser.add_argument("--normalize_training_actions", action="store_true", default=False, help="Normalize actions") + parser.add_argument( + "--epochs", + type=int, + default=None, + help=( + "Optional: Number of training epochs. If specified, overrides the number of epochs from the JSON training" + " config." + ), + ) args = parser.parse_args() diff --git a/scripts/reinforcement_learning/ray/grok_cluster_with_kubectl.py b/scripts/reinforcement_learning/ray/grok_cluster_with_kubectl.py index fda628788b46..b7b3c5cf89e2 100644 --- a/scripts/reinforcement_learning/ray/grok_cluster_with_kubectl.py +++ b/scripts/reinforcement_learning/ray/grok_cluster_with_kubectl.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -164,11 +164,12 @@ def process_cluster(cluster_info: dict, ray_head_name: str = "head") -> str: For each cluster, check that it is running, and get the Ray head address that will accept jobs. Args: - cluster_info (dict): A dictionary containing cluster information with keys 'cluster', 'pods', and 'namespace'. - ray_head_name (str, optional): The name of the ray head container. Defaults to "head". + cluster_info: A dictionary containing cluster information with keys 'cluster', 'pods', and 'namespace'. + ray_head_name: The name of the ray head container. Defaults to "head". Returns: - str: A string containing the cluster name and its Ray head address, or an error message if the head pod or Ray address is not found. + A string containing the cluster name and its Ray head address, or an error message if + the head pod or Ray address is not found. """ cluster, pods, namespace = cluster_info head_pod = None diff --git a/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cartpole_cfg.py b/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cartpole_cfg.py index 7490bf69ec3b..f43ae7ecaaaa 100644 --- a/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cartpole_cfg.py +++ b/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cartpole_cfg.py @@ -1,9 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import pathlib import sys +from typing import Any # Allow for import of items from the ray workflow. CUR_DIR = pathlib.Path(__file__).parent @@ -12,6 +13,8 @@ import util import vision_cfg from ray import tune +from ray.tune.progress_reporter import CLIReporter +from ray.tune.stopper import Stopper class CartpoleRGBNoTuneJobCfg(vision_cfg.CameraJobCfg): @@ -47,3 +50,36 @@ def __init__(self, cfg: dict = {}): cfg = util.populate_isaac_ray_cfg_args(cfg) cfg["runner_args"]["--task"] = tune.choice(["Isaac-Cartpole-RGB-TheiaTiny-v0"]) super().__init__(cfg) + + +class CustomCartpoleProgressReporter(CLIReporter): + def __init__(self): + super().__init__( + metric_columns={ + "training_iteration": "iter", + "time_total_s": "total time (s)", + "Episode/Episode_Reward/alive": "alive", + "Episode/Episode_Reward/cart_vel": "cart velocity", + "rewards/time": "rewards/time", + }, + max_report_frequency=5, + sort_by_metric=True, + ) + + +class CartpoleEarlyStopper(Stopper): + def __init__(self): + self._bad_trials = set() + + def __call__(self, trial_id: str, result: dict[str, Any]) -> bool: + iter = result.get("training_iteration", 0) + out_of_bounds = result.get("Episode/Episode_Termination/cart_out_of_bounds") + + # Mark the trial for stopping if conditions are met + if iter >= 20 and out_of_bounds is not None and out_of_bounds > 0.85: + self._bad_trials.add(trial_id) + + return trial_id in self._bad_trials + + def stop_all(self) -> bool: + return False # only stop individual trials diff --git a/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cfg.py b/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cfg.py index cefb0090e54f..cb59a993368d 100644 --- a/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cfg.py +++ b/scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -85,12 +85,14 @@ def get_cnn_layers(_): if next_size <= 0: break - layers.append({ - "filters": tune.randint(16, 32).sample(), - "kernel_size": str(kernel), - "strides": str(stride), - "padding": str(padding), - }) + layers.append( + { + "filters": tune.randint(16, 32).sample(), + "kernel_size": str(kernel), + "strides": str(stride), + "padding": str(padding), + } + ) size = next_size return layers @@ -98,7 +100,6 @@ def get_cnn_layers(_): cfg["hydra_args"]["agent.params.network.cnn.convs"] = tune.sample_from(get_cnn_layers) if vary_mlp: # Vary the MLP structure; neurons (units) per layer, number of layers, - max_num_layers = 6 max_neurons_per_layer = 128 if "env.observations.policy.image.params.model_name" in cfg["hydra_args"]: @@ -140,12 +141,14 @@ class TheiaCameraJob(CameraJobCfg): def __init__(self, cfg: dict = {}): cfg = util.populate_isaac_ray_cfg_args(cfg) - cfg["hydra_args"]["env.observations.policy.image.params.model_name"] = tune.choice([ - "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", - ]) + cfg["hydra_args"]["env.observations.policy.image.params.model_name"] = tune.choice( + [ + "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", + ] + ) super().__init__(cfg, vary_env_count=True, vary_cnn=False, vary_mlp=True) diff --git a/scripts/reinforcement_learning/ray/launch.py b/scripts/reinforcement_learning/ray/launch.py index 10b15353eb9c..3a3be716702e 100644 --- a/scripts/reinforcement_learning/ray/launch.py +++ b/scripts/reinforcement_learning/ray/launch.py @@ -1,17 +1,8 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import argparse -import pathlib -import subprocess -import yaml - -import util -from jinja2 import Environment, FileSystemLoader -from kubernetes import config - """This script helps create one or more KubeRay clusters. Usage: @@ -34,6 +25,18 @@ --num_workers 1 2 --num_clusters 1 \ --worker_accelerator nvidia-l4 nvidia-tesla-t4 --gpu_per_worker 1 2 4 """ + +import argparse +import pathlib +import subprocess + +import yaml +from jinja2 import Environment, FileSystemLoader +from kubernetes import config + +# Local imports +import util # isort: skip + RAY_DIR = pathlib.Path(__file__).parent diff --git a/scripts/reinforcement_learning/ray/mlflow_to_local_tensorboard.py b/scripts/reinforcement_learning/ray/mlflow_to_local_tensorboard.py index 5511c88b0e4f..2c45f1cd0a8d 100644 --- a/scripts/reinforcement_learning/ray/mlflow_to_local_tensorboard.py +++ b/scripts/reinforcement_learning/ray/mlflow_to_local_tensorboard.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,10 +9,10 @@ import os import sys from concurrent.futures import ProcessPoolExecutor, as_completed -from torch.utils.tensorboard import SummaryWriter import mlflow from mlflow.tracking import MlflowClient +from torch.utils.tensorboard import SummaryWriter def setup_logging(level=logging.INFO): @@ -66,6 +66,7 @@ def process_run(args): def download_experiment_tensorboard_logs(uri: str, experiment_name: str, download_dir: str) -> None: """Download MLflow experiment logs and convert to TensorBoard format.""" + # import logger logger = logging.getLogger(__name__) try: diff --git a/scripts/reinforcement_learning/ray/submit_job.py b/scripts/reinforcement_learning/ray/submit_job.py index b97c99492e3e..21fb6a3d9b39 100644 --- a/scripts/reinforcement_learning/ray/submit_job.py +++ b/scripts/reinforcement_learning/ray/submit_job.py @@ -1,15 +1,8 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import argparse -import os -import time -from concurrent.futures import ThreadPoolExecutor - -from ray import job_submission - """ This script submits aggregate job(s) to cluster(s) described in a config file containing ``name: address: http://:`` on @@ -26,7 +19,11 @@ creates several individual jobs when started on a cluster. Alternatively, an aggregate job could be a :file:'../wrap_resources.py` resource-wrapped job, which may contain several individual sub-jobs separated by -the + delimiter. +the + delimiter. An aggregate job could also be a :file:`../task_runner.py` multi-task submission job, +where each sub-job and its resource requirements are defined in a YAML configuration file. +In this mode, :file:`../task_runner.py` will read the YAML file (via --task_cfg), and +submit all defined sub-tasks to the Ray cluster, supporting per-job resource specification and +real-time streaming of sub-job outputs. If there are more aggregate jobs than cluster(s), aggregate jobs will be submitted as clusters become available via the defined relation above. If there are less aggregate job(s) @@ -48,9 +45,22 @@ # Example: Submitting resource wrapped job python3 scripts/reinforcement_learning/ray/submit_job.py --aggregate_jobs wrap_resources.py --test + # Example: submitting tasks with specific resources, and supporting pip packages and py_modules + # You may use relative paths for task_cfg and py_modules, placing them in the + # "scripts/reinforcement_learning/ray" directory, which will be uploaded to the cluster. + python3 scripts/reinforcement_learning/ray/submit_job.py --aggregate_jobs task_runner.py --task_cfg tasks.yaml + # For all command line arguments python3 scripts/reinforcement_learning/ray/submit_job.py -h """ + +import argparse +import os +import time +from concurrent.futures import ThreadPoolExecutor + +from ray import job_submission + script_directory = os.path.dirname(os.path.abspath(__file__)) CONFIG = {"working_dir": script_directory, "executable": "/workspace/isaaclab/isaaclab.sh -p"} diff --git a/scripts/reinforcement_learning/ray/task_runner.py b/scripts/reinforcement_learning/ray/task_runner.py new file mode 100644 index 000000000000..7bb3596fc918 --- /dev/null +++ b/scripts/reinforcement_learning/ray/task_runner.py @@ -0,0 +1,298 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script dispatches one or more user-defined Python tasks to workers in a Ray cluster. +Each task, along with its resource requirements and execution parameters, is specified in a YAML configuration file. +Users may define the number of CPUs, GPUs, and the amount of memory to allocate per task via the config file. + +Key features: +------------- +- Fine-grained, per-task resource management via config fields (`num_gpus`, `num_cpus`, `memory`). +- Parallel execution of multiple tasks using available resources across the Ray cluster. +- Option to specify node affinity for tasks, e.g., by hostname, node ID, or any node. +- Optional batch (simultaneous) or independent scheduling of tasks. + +Task scheduling and distribution are handled via Rayโ€™s built-in resource manager. + +YAML configuration fields: +-------------------------- +- `pip`: List of extra pip packages to install before running any tasks. +- `py_modules`: List of additional Python module paths (directories or files) to include in the runtime environment. +- `concurrent`: (bool) It determines task dispatch semantics: + - If `concurrent: true`, **all tasks are scheduled as a batch**. The script waits until + sufficient resources are available for every task in the batch, then launches all tasks + together. If resources are insufficient, all tasks remain blocked until the cluster can + support the full batch. + - If `concurrent: false`, tasks are launched as soon as resources are available for each + individual task, and Ray independently schedules them. This may result in non-simultaneous + task start times. +- `tasks`: List of task specifications, each with: + - `name`: String identifier for the task. + - `py_args`: Arguments to the Python interpreter (e.g., script/module, flags, user arguments). + - `num_gpus`: Number of GPUs to allocate (float or string arithmetic, e.g., "2*2"). + - `num_cpus`: Number of CPUs to allocate (float or string). + - `memory`: Amount of RAM in bytes (int or string). + - `node` (optional): Node placement constraints. + - `specific` (str): Type of node placement, support `hostname`, `node_id`, or `any`. + - `any`: Place the task on any available node. + - `hostname`: Place the task on a specific hostname. `hostname` must be specified + in the node field. + - `node_id`: Place the task on a specific node ID. `node_id` must be specified in + the node field. + - `hostname` (str): Specific hostname to place the task on. + - `node_id` (str): Specific node ID to place the task on. + + +Typical usage: +-------------- + +.. code-block:: bash + + # Print help and argument details: + python task_runner.py -h + + # Submit tasks defined in a YAML file to the Ray cluster (auto-detects Ray head address): + python task_runner.py --task_cfg /path/to/tasks.yaml + +YAML configuration example-1: +----------------------------- + +.. code-block:: yaml + + pip: ["xxx"] + py_modules: ["my_package/my_package"] + concurrent: false + tasks: + - name: "Isaac-Cartpole-v0" + py_args: "-m torch.distributed.run --nnodes=1 --nproc_per_node=2 --rdzv_endpoint=localhost:29501 /workspace/isaaclab/scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Cartpole-v0 --max_iterations 200 --headless --distributed" + num_gpus: 2 + num_cpus: 10 + memory: 10737418240 + - name: "script need some dependencies" + py_args: "script.py --option arg" + num_gpus: 0 + num_cpus: 1 + memory: 10*1024*1024*1024 + +YAML configuration example-2: +----------------------------- + +.. code-block:: yaml + + pip: ["xxx"] + py_modules: ["my_package/my_package"] + concurrent: true + tasks: + - name: "Isaac-Cartpole-v0-multi-node-train-1" + py_args: "-m torch.distributed.run --nproc_per_node=1 --nnodes=2 --node_rank=0 --rdzv_id=123 --rdzv_backend=c10d --rdzv_endpoint=localhost:5555 /workspace/isaaclab/scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Cartpole-v0 --headless --distributed --max_iterations 1000" + num_gpus: 1 + num_cpus: 10 + memory: 10*1024*1024*1024 + node: + specific: "hostname" + hostname: "xxx" + - name: "Isaac-Cartpole-v0-multi-node-train-2" + py_args: "-m torch.distributed.run --nproc_per_node=1 --nnodes=2 --node_rank=1 --rdzv_id=123 --rdzv_backend=c10d --rdzv_endpoint=x.x.x.x:5555 /workspace/isaaclab/scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Cartpole-v0 --headless --distributed --max_iterations 1000" + num_gpus: 1 + num_cpus: 10 + memory: 10*1024*1024*1024 + node: + specific: "hostname" + hostname: "xxx" + +To stop all tasks early, press Ctrl+C; the script will cancel all running Ray tasks. +""" # noqa: E501 + +import argparse +import ast +import operator +from datetime import datetime + +import yaml + +# Local imports +import util # isort: skip + +# Safe operators for arithmetic expression evaluation +_SAFE_OPERATORS = { + ast.Add: operator.add, + ast.Sub: operator.sub, + ast.Mult: operator.mul, + ast.Div: operator.truediv, + ast.FloorDiv: operator.floordiv, + ast.Pow: operator.pow, + ast.Mod: operator.mod, + ast.USub: operator.neg, + ast.UAdd: operator.pos, +} + + +def safe_eval_arithmetic(expr: str) -> int | float: + """ + Safely evaluate a string containing only arithmetic expressions. + + Supports: +, -, *, /, //, **, % and numeric literals. + Raises ValueError for any non-arithmetic expressions. + + Args: + expr: A string containing an arithmetic expression (e.g., "10*1024*1024"). + + Returns: + The numeric result of the expression. + + Raises: + ValueError: If the expression contains non-arithmetic operations. + """ + + def _eval_node(node: ast.AST) -> int | float: + if isinstance(node, ast.Expression): + return _eval_node(node.body) + elif isinstance(node, ast.Constant) and isinstance(node.value, (int, float)): + return node.value + elif isinstance(node, ast.BinOp) and type(node.op) in _SAFE_OPERATORS: + left = _eval_node(node.left) + right = _eval_node(node.right) + return _SAFE_OPERATORS[type(node.op)](left, right) + elif isinstance(node, ast.UnaryOp) and type(node.op) in _SAFE_OPERATORS: + operand = _eval_node(node.operand) + return _SAFE_OPERATORS[type(node.op)](operand) + else: + raise ValueError(f"Unsafe expression: {ast.dump(node)}") + + try: + tree = ast.parse(expr.strip(), mode="eval") + return _eval_node(tree) + except (SyntaxError, TypeError) as e: + raise ValueError(f"Invalid arithmetic expression: {expr}") from e + + +def parse_args() -> argparse.Namespace: + """ + Parse command-line arguments for the Ray task runner. + + Returns: + A namespace containing parsed CLI arguments: + - task_cfg (str): Path to the YAML task file. + - ray_address (str): Ray cluster address. + - test (bool): Whether to run a GPU resource isolation sanity check. + """ + parser = argparse.ArgumentParser(description="Run tasks from a YAML config file.") + parser.add_argument("--task_cfg", type=str, required=True, help="Path to the YAML task file.") + parser.add_argument("--ray_address", type=str, default="auto", help="the Ray address.") + parser.add_argument( + "--test", + action="store_true", + help=( + "Run nvidia-smi test instead of the arbitrary job," + "can use as a sanity check prior to any jobs to check " + "that GPU resources are correctly isolated." + ), + ) + return parser.parse_args() + + +def parse_task_resource(task: dict) -> util.JobResource: + """ + Parse task resource requirements from the YAML configuration. + + Args: + task (dict): Dictionary representing a single task's configuration. + Keys may include `num_gpus`, `num_cpus`, and `memory`, each either + as a number or evaluatable string expression. + + Returns: + util.JobResource: Resource object with the parsed values. + """ + resource = util.JobResource() + if "num_gpus" in task: + value = task["num_gpus"] + resource.num_gpus = safe_eval_arithmetic(value) if isinstance(value, str) else value + if "num_cpus" in task: + value = task["num_cpus"] + resource.num_cpus = safe_eval_arithmetic(value) if isinstance(value, str) else value + if "memory" in task: + value = task["memory"] + resource.memory = safe_eval_arithmetic(value) if isinstance(value, str) else value + return resource + + +def run_tasks( + tasks: list[dict], args: argparse.Namespace, runtime_env: dict | None = None, concurrent: bool = False +) -> None: + """ + Submit tasks to the Ray cluster for execution. + + Args: + tasks (list[dict]): A list of task configuration dictionaries. + args (argparse.Namespace): Parsed command-line arguments. + runtime_env (dict | None): Ray runtime environment configuration containing: + - pip (list[str] | None): Additional pip packages to install. + - py_modules (list[str] | None): Python modules to include in the environment. + concurrent (bool): Whether to launch tasks simultaneously as a batch, + or independently as resources become available. + + Returns: + None + """ + job_objs = [] + util.ray_init(ray_address=args.ray_address, runtime_env=runtime_env, log_to_driver=False) + for task in tasks: + resource = parse_task_resource(task) + print(f"[INFO] Creating job {task['name']} with resource={resource}") + job = util.Job( + name=task["name"], + py_args=task["py_args"], + resources=resource, + node=util.JobNode( + specific=task.get("node", {}).get("specific"), + hostname=task.get("node", {}).get("hostname"), + node_id=task.get("node", {}).get("node_id"), + ), + ) + job_objs.append(job) + start = datetime.now() + print(f"[INFO] Creating {len(job_objs)} jobs at {start.strftime('%H:%M:%S.%f')} with runtime env={runtime_env}") + # submit jobs + util.submit_wrapped_jobs( + jobs=job_objs, + test_mode=args.test, + concurrent=concurrent, + ) + end = datetime.now() + print( + f"[INFO] All jobs completed at {end.strftime('%H:%M:%S.%f')}, took {(end - start).total_seconds():.2f} seconds." + ) + + +def main() -> None: + """ + Main entry point for the Ray task runner script. + + Reads the YAML task configuration file, parses CLI arguments, + and dispatches tasks to the Ray cluster. + + Returns: + None + """ + args = parse_args() + with open(args.task_cfg) as f: + config = yaml.safe_load(f) + tasks = config["tasks"] + runtime_env = { + "pip": None if not config.get("pip") else config["pip"], + "py_modules": None if not config.get("py_modules") else config["py_modules"], + } + concurrent = config.get("concurrent", False) + run_tasks( + tasks=tasks, + args=args, + runtime_env=runtime_env, + concurrent=concurrent, + ) + + +if __name__ == "__main__": + main() diff --git a/scripts/reinforcement_learning/ray/tuner.py b/scripts/reinforcement_learning/ray/tuner.py index 09ceb06a5d42..99dc7e8d08f5 100644 --- a/scripts/reinforcement_learning/ray/tuner.py +++ b/scripts/reinforcement_learning/ray/tuner.py @@ -1,18 +1,23 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import argparse import importlib.util import os +import random +import subprocess import sys -from time import sleep +from time import sleep, time import ray import util from ray import air, tune +from ray.tune import Callback +from ray.tune.progress_reporter import ProgressReporter from ray.tune.search.optuna import OptunaSearch from ray.tune.search.repeater import Repeater +from ray.tune.stopper import CombinedStopper """ This script breaks down an aggregate tuning job, as defined by a hyperparameter sweep configuration, @@ -44,6 +49,11 @@ ./isaaclab.sh -p scripts/reinforcement_learning/ray/tuner.py --run_mode local \ --cfg_file scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cartpole_cfg.py \ --cfg_class CartpoleTheiaJobCfg + # Local with a custom progress reporter + ./isaaclab.sh -p scripts/reinforcement_learning/ray/tuner.py \ + --cfg_file scripts/reinforcement_learning/ray/hyperparameter_tuning/vision_cartpole_cfg.py \ + --cfg_class CartpoleTheiaJobCfg \ + --progress_reporter CustomCartpoleProgressReporter # Remote (run grok cluster or create config file mentioned in :file:`submit_job.py`) ./isaaclab.sh -p scripts/reinforcement_learning/ray/submit_job.py \ --aggregate_jobs tuner.py \ @@ -57,6 +67,9 @@ PYTHON_EXEC = "./isaaclab.sh -p" WORKFLOW = "scripts/reinforcement_learning/rl_games/train.py" NUM_WORKERS_PER_NODE = 1 # needed for local parallelism +PROCESS_RESPONSE_TIMEOUT = 200.0 # seconds to wait before killing the process when it stops responding +MAX_LINES_TO_SEARCH_EXPERIMENT_LOGS = 1000 # maximum number of lines to read from the training process logs +MAX_LOG_EXTRACTION_ERRORS = 10 # maximum allowed LogExtractionErrors before we abort the whole training class IsaacLabTuneTrainable(tune.Trainable): @@ -70,12 +83,13 @@ class IsaacLabTuneTrainable(tune.Trainable): def setup(self, config: dict) -> None: """Get the invocation command, return quick for easy scheduling.""" self.data = None + self.time_since_last_proc_response = 0.0 self.invoke_cmd = util.get_invocation_command_from_cfg(cfg=config, python_cmd=PYTHON_EXEC, workflow=WORKFLOW) print(f"[INFO]: Recovered invocation with {self.invoke_cmd}") self.experiment = None def reset_config(self, new_config: dict): - """Allow environments to be re-used by fetching a new invocation command""" + """Allow environments to be reused by fetching a new invocation command""" self.setup(new_config) return True @@ -84,12 +98,21 @@ def step(self) -> dict: # When including this as first step instead of setup, experiments get scheduled faster # Don't want to block the scheduler while the experiment spins up print(f"[INFO]: Invoking experiment as first step with {self.invoke_cmd}...") - experiment = util.execute_job( - self.invoke_cmd, - identifier_string="", - extract_experiment=True, - persistent_dir=BASE_DIR, - ) + try: + experiment = util.execute_job( + self.invoke_cmd, + identifier_string="", + extract_experiment=True, # Keep this as True to return a valid dictionary + persistent_dir=BASE_DIR, + max_lines_to_search_logs=MAX_LINES_TO_SEARCH_EXPERIMENT_LOGS, + max_time_to_search_logs=PROCESS_RESPONSE_TIMEOUT, + ) + except util.LogExtractionError: + self.data = { + "LOG_EXTRACTION_ERROR_STOPPER_FLAG": True, + "done": True, + } + return self.data self.experiment = experiment print(f"[INFO]: Tuner recovered experiment info {experiment}") self.proc = experiment["proc"] @@ -109,11 +132,35 @@ def step(self) -> dict: while data is None: data = util.load_tensorboard_logs(self.tensorboard_logdir) + proc_status = self.proc.poll() + if proc_status is not None: + break sleep(2) # Lazy report metrics to avoid performance overhead if self.data is not None: - while util._dicts_equal(data, self.data): + data_ = {k: v for k, v in data.items() if k != "done"} + self_data_ = {k: v for k, v in self.data.items() if k != "done"} + unresponsiveness_start_time = time() + while util._dicts_equal(data_, self_data_): + self.time_since_last_proc_response = time() - unresponsiveness_start_time data = util.load_tensorboard_logs(self.tensorboard_logdir) + data_ = {k: v for k, v in data.items() if k != "done"} + proc_status = self.proc.poll() + if proc_status is not None: + break + if self.time_since_last_proc_response > PROCESS_RESPONSE_TIMEOUT: + self.time_since_last_proc_response = 0.0 + print("[WARNING]: Training workflow process is not responding, terminating...") + self.proc.terminate() + try: + self.proc.wait(timeout=20) + except subprocess.TimeoutExpired: + print("[ERROR]: The process did not terminate within timeout duration.") + self.proc.kill() + self.proc.wait() + self.data = data + self.data["done"] = True + return self.data sleep(2) # Lazy report metrics to avoid performance overhead self.data = data @@ -132,13 +179,73 @@ def default_resource_request(self): ) -def invoke_tuning_run(cfg: dict, args: argparse.Namespace) -> None: +class LogExtractionErrorStopper(tune.Stopper): + """Stopper that stops all trials if multiple LogExtractionErrors occur. + + Args: + max_errors: The maximum number of LogExtractionErrors allowed before terminating the experiment. + """ + + def __init__(self, max_errors: int): + self.max_errors = max_errors + self.error_count = 0 + + def __call__(self, trial_id, result): + """Increments the error count if trial has encountered a LogExtractionError. + + It does not stop the trial based on the metrics, always returning False. + """ + if result.get("LOG_EXTRACTION_ERROR_STOPPER_FLAG", False): + self.error_count += 1 + print( + f"[ERROR]: Encountered LogExtractionError {self.error_count} times. " + f"Maximum allowed is {self.max_errors}." + ) + return False + + def stop_all(self): + """Returns true if number of LogExtractionErrors exceeds the maximum allowed, terminating the experiment.""" + if self.error_count > self.max_errors: + print("[FATAL]: Encountered LogExtractionError more than allowed, aborting entire tuning run... ") + return True + else: + return False + + +class ProcessCleanupCallback(Callback): + """Callback to clean up processes when trials are stopped.""" + + def on_trial_error(self, iteration, trials, trial, error, **info): + """Called when a trial encounters an error.""" + self._cleanup_trial(trial) + + def on_trial_complete(self, iteration, trials, trial, **info): + """Called when a trial completes.""" + self._cleanup_trial(trial) + + def _cleanup_trial(self, trial): + """Clean up processes for a trial using SIGKILL.""" + try: + subprocess.run(["pkill", "-9", "-f", f"rid {trial.config['runner_args']['-rid']}"], check=False) + sleep(5) + except Exception as e: + print(f"[ERROR]: Failed to cleanup trial {trial.trial_id}: {e}") + + +def invoke_tuning_run( + cfg: dict, + args: argparse.Namespace, + progress_reporter: ProgressReporter | None = None, + stopper: tune.Stopper | None = None, +) -> None: """Invoke an Isaac-Ray tuning run. Log either to a local directory or to MLFlow. Args: cfg: Configuration dictionary extracted from job setup args: Command-line arguments related to tuning. + progress_reporter: Custom progress reporter. Defaults to CLIReporter or JupyterNotebookReporter if not provided. + stopper: Custom stopper, optional. """ # Allow for early exit os.environ["TUNE_DISABLE_STRICT_METRIC_CHECKING"] = "1" @@ -146,17 +253,17 @@ def invoke_tuning_run(cfg: dict, args: argparse.Namespace) -> None: print("[WARNING]: Not saving checkpoints, just running experiment...") print("[INFO]: Model parameters and metrics will be preserved.") print("[WARNING]: For homogeneous cluster resources only...") + + # Initialize Ray + util.ray_init( + ray_address=args.ray_address, + log_to_driver=True, + ) + # Get available resources resources = util.get_gpu_node_resources() print(f"[INFO]: Available resources {resources}") - if not ray.is_initialized(): - ray.init( - address=args.ray_address, - log_to_driver=True, - num_gpus=len(resources), - ) - print(f"[INFO]: Using config {cfg}") # Configure the search algorithm and the repeater @@ -166,15 +273,37 @@ def invoke_tuning_run(cfg: dict, args: argparse.Namespace) -> None: ) repeat_search = Repeater(searcher, repeat=args.repeat_run_count) + # Configure the stoppers + stoppers: CombinedStopper = CombinedStopper( + *[ + LogExtractionErrorStopper(max_errors=MAX_LOG_EXTRACTION_ERRORS), + *([stopper] if stopper is not None else []), + ] + ) + + if progress_reporter is not None: + os.environ["RAY_AIR_NEW_OUTPUT"] = "0" + if ( + getattr(progress_reporter, "_metric", None) is not None + or getattr(progress_reporter, "_mode", None) is not None + ): + raise ValueError( + "Do not set or directly in the custom progress reporter class, " + "provide them as arguments to tuner.py instead." + ) + if args.run_mode == "local": # Standard config, to file run_config = air.RunConfig( storage_path="/tmp/ray", name=f"IsaacRay-{args.cfg_class}-tune", + callbacks=[ProcessCleanupCallback()], verbose=1, checkpoint_config=air.CheckpointConfig( checkpoint_frequency=0, # Disable periodic checkpointing checkpoint_at_end=False, # Disable final checkpoint ), + stop=stoppers, + progress_reporter=progress_reporter, ) elif args.run_mode == "remote": # MLFlow, to MLFlow server @@ -188,17 +317,22 @@ def invoke_tuning_run(cfg: dict, args: argparse.Namespace) -> None: run_config = ray.train.RunConfig( name="mlflow", storage_path="/tmp/ray", - callbacks=[mlflow_callback], + callbacks=[ProcessCleanupCallback(), mlflow_callback], checkpoint_config=ray.train.CheckpointConfig(checkpoint_frequency=0, checkpoint_at_end=False), + stop=stoppers, + progress_reporter=progress_reporter, ) else: raise ValueError("Unrecognized run mode.") - + # RID isn't optimized as it is sampled from, but useful for cleanup later + cfg["runner_args"]["-rid"] = tune.sample_from(lambda _: str(random.randint(int(1e9), int(1e10) - 1))) # Configure the tuning job tuner = tune.Tuner( IsaacLabTuneTrainable, param_space=cfg, tune_config=tune.TuneConfig( + metric=args.metric, + mode=args.mode, search_alg=repeat_search, num_samples=args.num_samples, reuse_actors=True, @@ -306,8 +440,55 @@ def __init__(self, cfg: dict): default=3, help="How many times to repeat each hyperparameter config.", ) + parser.add_argument( + "--process_response_timeout", + type=float, + default=PROCESS_RESPONSE_TIMEOUT, + help="Training workflow process response timeout.", + ) + parser.add_argument( + "--max_lines_to_search_experiment_logs", + type=float, + default=MAX_LINES_TO_SEARCH_EXPERIMENT_LOGS, + help="Max number of lines to search for experiment logs before terminating the training workflow process.", + ) + parser.add_argument( + "--max_log_extraction_errors", + type=float, + default=MAX_LOG_EXTRACTION_ERRORS, + help="Max number number of LogExtractionError failures before we abort the whole tuning run.", + ) + parser.add_argument( + "--progress_reporter", + type=str, + default=None, + help=( + "Optional: name of a custom reporter class defined in the cfg_file. " + "Must subclass ray.tune.ProgressReporter " + "(e.g., CustomCartpoleProgressReporter)." + ), + ) + parser.add_argument( + "--stopper", + type=str, + default=None, + help="A stop criteria in the cfg_file, must be a tune.Stopper instance.", + ) args = parser.parse_args() + PROCESS_RESPONSE_TIMEOUT = args.process_response_timeout + MAX_LINES_TO_SEARCH_EXPERIMENT_LOGS = int(args.max_lines_to_search_experiment_logs) + print( + "[INFO]: The max number of lines to search for experiment logs before (early) terminating the training " + f"workflow process is set to {MAX_LINES_TO_SEARCH_EXPERIMENT_LOGS}.\n" + "[INFO]: The process response timeout, used while updating tensorboard scalars and searching for " + f"experiment logs, is set to {PROCESS_RESPONSE_TIMEOUT} seconds." + ) + MAX_LOG_EXTRACTION_ERRORS = int(args.max_log_extraction_errors) + print( + "[INFO]: Max number of LogExtractionError failures before we abort the whole tuning run is " + f"set to {MAX_LOG_EXTRACTION_ERRORS}.\n" + ) NUM_WORKERS_PER_NODE = args.num_workers_per_node print(f"[INFO]: Using {NUM_WORKERS_PER_NODE} workers per node.") if args.run_mode == "remote": @@ -351,7 +532,25 @@ def __init__(self, cfg: dict): print(f"[INFO]: Successfully instantiated class '{class_name}' from {file_path}") cfg = instance.cfg print(f"[INFO]: Grabbed the following hyperparameter sweep config: \n {cfg}") - invoke_tuning_run(cfg, args) + # Load optional stopper config + stopper = None + if args.stopper and hasattr(module, args.stopper): + stopper = getattr(module, args.stopper) + if isinstance(stopper, type) and issubclass(stopper, tune.Stopper): + stopper = stopper() + else: + raise TypeError(f"[ERROR]: Unsupported stop criteria type: {type(stopper)}") + print(f"[INFO]: Loaded custom stop criteria from '{args.stopper}'") + # Load optional progress reporter config + progress_reporter = None + if args.progress_reporter and hasattr(module, args.progress_reporter): + progress_reporter = getattr(module, args.progress_reporter) + if isinstance(progress_reporter, type) and issubclass(progress_reporter, tune.ProgressReporter): + progress_reporter = progress_reporter() + else: + raise TypeError(f"[ERROR]: {args.progress_reporter} is not a valid ProgressReporter.") + print(f"[INFO]: Loaded custom progress reporter from '{args.progress_reporter}'") + invoke_tuning_run(cfg, args, progress_reporter=progress_reporter, stopper=stopper) else: raise AttributeError(f"[ERROR]:Class '{class_name}' not found in {file_path}") diff --git a/scripts/reinforcement_learning/ray/util.py b/scripts/reinforcement_learning/ray/util.py index 144fbf4ee3bd..a73ebdf493dc 100644 --- a/scripts/reinforcement_learning/ray/util.py +++ b/scripts/reinforcement_learning/ray/util.py @@ -1,16 +1,23 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import argparse import os import re +import select import subprocess +import sys import threading +from collections.abc import Sequence +from dataclasses import dataclass from datetime import datetime from math import isclose +from time import time +from typing import Any import ray +from ray.util.scheduling_strategies import NodeAffinitySchedulingStrategy from tensorboard.backend.event_processing.directory_watcher import DirectoryDeletedError from tensorboard.backend.event_processing.event_accumulator import EventAccumulator @@ -26,6 +33,12 @@ def load_tensorboard_logs(directory: str) -> dict: The latest available scalar values. """ + # replace any non-alnum/underscore/dot with "_", then collapse runs of "_" + def replace_invalid_chars(t): + t2 = re.sub(r"[^0-9A-Za-z_./]", "_", t) + t2 = re.sub(r"_+", "_", t2) + return t2.strip("_") + # Initialize the event accumulator with a size guidance for only the latest entry def get_latest_scalars(path: str) -> dict: event_acc = EventAccumulator(path, size_guidance={"scalars": 1}) @@ -33,7 +46,7 @@ def get_latest_scalars(path: str) -> dict: event_acc.Reload() if event_acc.Tags()["scalars"]: return { - tag: event_acc.Scalars(tag)[-1].value + replace_invalid_chars(tag): event_acc.Scalars(tag)[-1].value for tag in event_acc.Tags()["scalars"] if event_acc.Scalars(tag) } @@ -58,7 +71,7 @@ def process_args(args, target_list, is_hydra=False): if not is_hydra: if key.endswith("_singleton"): target_list.append(value) - elif key.startswith("--"): + elif key.startswith("--") or key.startswith("-"): target_list.append(f"{key} {value}") # Space instead of = for runner args else: target_list.append(f"{value}") @@ -98,6 +111,12 @@ def remote_execute_job( ) +class LogExtractionError(Exception): + """Raised when we cannot extract experiment_name/logdir from the trainer output.""" + + pass + + def execute_job( job_cmd: str, identifier_string: str = "job 0", @@ -105,6 +124,8 @@ def execute_job( extract_experiment: bool = False, persistent_dir: str | None = None, log_all_output: bool = False, + max_lines_to_search_logs: int = 1000, + max_time_to_search_logs: float = 200.0, ) -> str | dict: """Issue a job (shell command). @@ -117,6 +138,8 @@ def execute_job( persistent_dir: When supplied, change to run the directory in a persistent directory. Can be used to avoid losing logs in the /tmp directory. Defaults to None. log_all_output: When true, print all output to the console. Defaults to False. + max_lines_to_search_logs: Maximum number of lines to search for experiment info. Defaults to 1000. + max_time_to_search_logs: Maximum time to wait for experiment info before giving up. Defaults to 200.0 seconds. Raises: ValueError: If the job is unable to start, or throws an error. Most likely to happen due to running out of memory. @@ -190,6 +213,8 @@ def execute_job( process = subprocess.Popen( job_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True, bufsize=1 ) + process_file_descriptor = process.stdout.fileno() + if persistent_dir: os.chdir(og_dir) experiment_name = None @@ -205,48 +230,80 @@ def stream_reader(stream, identifier_string, result_details): if log_all_output: print(f"{identifier_string}: {line}") - # Read stdout until we find experiment info + # Read stdout until we find exp. info, up to max_lines_to_search_logs lines, max_time_to_search_logs, or EOF. # Do some careful handling prevent overflowing the pipe reading buffer with error 141 - for line in iter(process.stdout.readline, ""): - line = line.strip() - result_details.append(f"{identifier_string}: {line} \n") - if log_all_output: - print(f"{identifier_string}: {line}") - - if extract_experiment: - exp_match = experiment_info_pattern.search(line) - log_match = logdir_pattern.search(line) - err_match = err_pattern.search(line) - - if err_match: - raise ValueError(f"Encountered an error during trial run. {' '.join(result_details)}") - - if exp_match: - experiment_name = exp_match.group(1) - if log_match: - logdir = log_match.group(1) - - if experiment_name and logdir: - # Start stderr reader after finding experiment info - stderr_thread = threading.Thread( - target=stream_reader, args=(process.stderr, identifier_string, result_details) - ) - stderr_thread.daemon = True - stderr_thread.start() - - # Start stdout reader to continue reading to flush buffer - stdout_thread = threading.Thread( - target=stream_reader, args=(process.stdout, identifier_string, result_details) - ) - stdout_thread.daemon = True - stdout_thread.start() - - return { - "experiment_name": experiment_name, - "logdir": logdir, - "proc": process, - "result": " ".join(result_details), - } + lines_read = 0 + search_duration = 0.0 + search_start_time = time() + while True: + new_line_ready, _, _ = select.select([process_file_descriptor], [], [], 1.0) # Wait up to 1s for stdout + if new_line_ready: + line = process.stdout.readline() + if not line: # EOF + break + + lines_read += 1 + line = line.strip() + result_details.append(f"{identifier_string}: {line} \n") + + if log_all_output: + print(f"{identifier_string}: {line}") + + if extract_experiment: + exp_match = experiment_info_pattern.search(line) + log_match = logdir_pattern.search(line) + err_match = err_pattern.search(line) + + if err_match: + raise ValueError(f"Encountered an error during trial run. {' '.join(result_details)}") + + if exp_match: + experiment_name = exp_match.group(1) + if log_match: + logdir = log_match.group(1) + + if experiment_name and logdir: + # Start stderr reader after finding experiment info + stderr_thread = threading.Thread( + target=stream_reader, args=(process.stderr, identifier_string, result_details) + ) + stderr_thread.daemon = True + stderr_thread.start() + + # Start stdout reader to continue reading to flush buffer + stdout_thread = threading.Thread( + target=stream_reader, args=(process.stdout, identifier_string, result_details) + ) + stdout_thread.daemon = True + stdout_thread.start() + + return { + "experiment_name": experiment_name, + "logdir": logdir, + "proc": process, + "result": " ".join(result_details), + } + + if extract_experiment: # if we are looking for experiment info, check for timeouts and line limits + search_duration = time() - search_start_time + if search_duration > max_time_to_search_logs: + print(f"[ERROR]: Could not find experiment logs within {max_time_to_search_logs} seconds.") + break + if lines_read >= max_lines_to_search_logs: + print(f"[ERROR]: Could not find experiment logs within first {max_lines_to_search_logs} lines.") + break + + # If we reach here, we didn't find experiment info in the output + if extract_experiment and not (experiment_name and logdir): + error_msg = ( + "Could not extract experiment_name/logdir from trainer output " + f"(experiment_name={experiment_name!r}, logdir={logdir!r}).\n" + "\tMake sure your training script prints the following correctly:\n" + "\t\tExact experiment name requested from command line: \n" + "\t\t[INFO] Logging experiment in directory: \n\n" + ) + print(f"[ERROR]: {error_msg}") + raise LogExtractionError("Could not extract experiment_name/logdir from training workflow output.") process.wait() now = datetime.now().strftime("%H:%M:%S.%f") completion_info = f"\n[INFO]: {identifier_string}: Job Started at {start_time}, completed at {now}\n" @@ -255,12 +312,23 @@ def stream_reader(stream, identifier_string, result_details): return " ".join(result_details) +def ray_init(ray_address: str = "auto", runtime_env: dict[str, Any] | None = None, log_to_driver: bool = False): + """Initialize Ray with the given address and runtime environment.""" + if not ray.is_initialized(): + print( + f"[INFO] Initializing Ray with address {ray_address}, log_to_driver={log_to_driver}," + f" runtime_env={runtime_env}" + ) + ray.init(address=ray_address, runtime_env=runtime_env, log_to_driver=log_to_driver) + else: + print("[WARNING]: Attempting to initialize Ray but it is already initialized!") + + def get_gpu_node_resources( total_resources: bool = False, one_node_only: bool = False, include_gb_ram: bool = False, include_id: bool = False, - ray_address: str = "auto", ) -> list[dict] | dict: """Get information about available GPU node resources. @@ -277,8 +345,7 @@ def get_gpu_node_resources( or simply the resource for a single node if requested. """ if not ray.is_initialized(): - ray.init(address=ray_address) - + raise RuntimeError("Ray must be initialized before calling get_gpu_node_resources().") nodes = ray.nodes() node_resources = [] total_cpus = 0 @@ -429,3 +496,225 @@ def _dicts_equal(d1: dict, d2: dict, tol=1e-9) -> bool: elif d1[key] != d2[key]: return False return True + + +@dataclass +class JobResource: + """A dataclass to represent a resource request for a job.""" + + num_gpus: float | None = None + num_cpus: float | None = None + memory: int | None = None # in bytes + + def to_opt(self) -> dict[str, Any]: + """Convert the resource request to a dictionary.""" + opt = {} + if self.num_gpus is not None: + opt["num_gpus"] = self.num_gpus + if self.num_cpus is not None: + opt["num_cpus"] = self.num_cpus + if self.memory is not None: + opt["memory"] = self.memory + return opt + + def to_pg_resources(self) -> dict[str, Any]: + """Convert the resource request to a dictionary suitable for placement groups.""" + res = {} + if self.num_gpus is not None: + res["GPU"] = self.num_gpus + if self.num_cpus is not None: + res["CPU"] = self.num_cpus + if self.memory is not None: + res["memory"] = self.memory + return res + + +@dataclass +class JobNode: + """A dataclass to represent a node for job affinity.""" + + specific: str | None = None + hostname: str | None = None + node_id: str | None = None + + def to_opt(self, nodes: list[dict[str, Any]]) -> dict[str, Any]: + """ + Convert node affinity settings into a dictionary of Ray actor scheduling options. + + Args: + nodes (list[dict[str, Any]]): List of node metadata from `ray.nodes()` which looks like this: + [{ + 'NodeID': 'xxx', + 'Alive': True, + 'NodeManagerAddress': 'x.x.x.x', + 'NodeManagerHostname': 'ray-head-mjzzf', + 'NodeManagerPort': 44039, + 'ObjectManagerPort': 35689, + 'ObjectStoreSocketName': '/tmp/ray/session_xxx/sockets/plasma_store', + 'RayletSocketName': '/tmp/ray/session_xxx/sockets/raylet', + 'MetricsExportPort': 8080, + 'NodeName': 'x.x.x.x', + 'RuntimeEnvAgentPort': 63725, + 'DeathReason': 0, + 'DeathReasonMessage': '', + 'alive': True, + 'Resources': { + 'node:__internal_head__': 1.0, + 'object_store_memory': 422449279795.0, + 'memory': 1099511627776.0, + 'GPU': 8.0, + 'node:x.x.x.x': 1.0, + 'CPU': 192.0, + 'accelerator_type:H20': 1.0 + }, + 'Labels': { + 'ray.io/node_id': 'xxx' + } + },...] + + Returns: + dict[str, Any]: A dictionary with possible scheduling options: + - Empty if no specific placement requirement. + - "scheduling_strategy" key set to `NodeAffinitySchedulingStrategy` + if hostname or node_id placement is specified. + + Raises: + ValueError: If hostname/node_id is specified but not found in the cluster + or the node is not alive. + """ + opt = {} + if self.specific is None or self.specific == "any": + return opt + elif self.specific == "hostname": + if self.hostname is None: + raise ValueError("Hostname must be specified when specific is 'hostname'") + for node in nodes: + if node["NodeManagerHostname"] == self.hostname: + if node["alive"] is False: + raise ValueError(f"Node {node['NodeID']} is not alive") + opt["scheduling_strategy"] = NodeAffinitySchedulingStrategy(node_id=node["NodeID"], soft=False) + return opt + raise ValueError(f"Hostname {self.hostname} not found in nodes: {nodes}") + elif self.specific == "node_id": + if self.node_id is None: + raise ValueError("Node ID must be specified when specific is 'node_id'") + for node in nodes: + if node["NodeID"] == self.node_id: + if node["alive"] is False: + raise ValueError(f"Node {node['NodeID']} is not alive") + opt["scheduling_strategy"] = NodeAffinitySchedulingStrategy(node_id=node["NodeID"], soft=False) + return opt + raise ValueError(f"Node ID {self.node_id} not found in nodes: {nodes}") + else: + raise ValueError(f"Invalid specific value: {self.specific}. Must be 'any', 'hostname', or 'node_id'.") + + +@dataclass +class Job: + """A dataclass to represent a job to be submitted to Ray.""" + + # job command + cmd: str | None = None + py_args: str | None = None + # identifier string for the job, e.g., "job 0" + name: str = "" + # job resources, e.g., {"CPU": 4, "GPU": 1} + resources: JobResource | None = None + # specify the node to run the job on, if needed to run on a specific node + node: JobNode | None = None + + def to_opt(self, nodes: list[dict[str, Any]]) -> dict[str, Any]: + """ + Convert the job definition into a dictionary of Ray scheduling options. + + Args: + nodes (list[dict[str, Any]]): Node information from `ray.nodes()`. + + Returns: + dict[str, Any]: Combined scheduling options from: + - `JobResource.to_opt()` for resource requirements + - `JobNode.to_opt()` for node placement constraints + """ + opt = {} + if self.resources is not None: + opt.update(self.resources.to_opt()) + if self.node is not None: + opt.update(self.node.to_opt(nodes)) + return opt + + +@ray.remote +class JobActor: + """Actor to run job in Ray cluster.""" + + def __init__(self, job: Job, test_mode: bool, log_all_output: bool, extract_experiment: bool = False): + self.job = job + self.test_mode = test_mode + self.log_all_output = log_all_output + self.extract_experiment = extract_experiment + self.done = True + + def ready(self) -> bool: + """Check if the job is ready to run.""" + return self.done + + def run(self): + """Run the job.""" + cmd = self.job.cmd if self.job.cmd else " ".join([sys.executable, *self.job.py_args.split()]) + return execute_job( + job_cmd=cmd, + identifier_string=self.job.name, + test_mode=self.test_mode, + extract_experiment=self.extract_experiment, + log_all_output=self.log_all_output, + ) + + +def submit_wrapped_jobs( + jobs: Sequence[Job], + log_realtime: bool = True, + test_mode: bool = False, + concurrent: bool = False, +) -> None: + """ + Submit a list of jobs to the Ray cluster and manage their execution. + + Args: + jobs (Sequence[Job]): A sequence of Job objects to execute on Ray. + log_realtime (bool): Whether to log stdout/stderr in real-time. Defaults to True. + test_mode (bool): If True, run in GPU sanity-check mode instead of actual jobs. Defaults to False. + concurrent (bool): Whether to launch tasks simultaneously as a batch, + or independently as resources become available. Defaults to False. + + Returns: + None + """ + if jobs is None or len(jobs) == 0: + print("[WARNING]: No jobs to submit") + return + if not ray.is_initialized(): + raise Exception("Ray is not initialized. Please initialize Ray before submitting jobs.") + nodes = ray.nodes() + actors = [] + for i, job in enumerate(jobs): + opts = job.to_opt(nodes) + name = job.name or f"job_{i + 1}" + print(f"[INFO] Create {name} with opts={opts}") + job_actor = JobActor.options(**opts).remote(job, test_mode, log_realtime) + actors.append(job_actor) + try: + if concurrent: + ray.get([actor.ready.remote() for actor in actors]) + print("[INFO] All actors are ready to run.") + future = [actor.run.remote() for actor in actors] + while future: + ready, not_ready = ray.wait(future, timeout=5) + for result in ray.get(ready): + print(f"\n{result}\n") + future = not_ready + print("[INFO] all jobs completed.") + except KeyboardInterrupt: + print("[INFO] KeyboardInterrupt received, cancelling โ€ฆ") + for actor in actors: + ray.cancel(actor, force=True) + sys.exit(0) diff --git a/scripts/reinforcement_learning/ray/wrap_resources.py b/scripts/reinforcement_learning/ray/wrap_resources.py index 4dfebcec984b..158bd0d82460 100644 --- a/scripts/reinforcement_learning/ray/wrap_resources.py +++ b/scripts/reinforcement_learning/ray/wrap_resources.py @@ -1,14 +1,8 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import argparse - -import ray -import util -from ray.util.scheduling_strategies import NodeAffinitySchedulingStrategy - """ This script dispatches sub-job(s) (individual jobs, use :file:`tuner.py` for tuning jobs) to worker(s) on GPU-enabled node(s) of a specific cluster as part of an resource-wrapped aggregate @@ -64,6 +58,10 @@ ./isaaclab.sh -p scripts/reinforcement_learning/ray/wrap_resources.py -h """ +import argparse + +import util + def wrap_resources_to_jobs(jobs: list[str], args: argparse.Namespace) -> None: """ @@ -75,9 +73,14 @@ def wrap_resources_to_jobs(jobs: list[str], args: argparse.Namespace) -> None: args: The arguments for resource allocation """ - if not ray.is_initialized(): - ray.init(address=args.ray_address, log_to_driver=True) - job_results = [] + job_objs = [] + util.ray_init( + ray_address=args.ray_address, + runtime_env={ + "py_modules": None if not args.py_modules else args.py_modules, + }, + log_to_driver=False, + ) gpu_node_resources = util.get_gpu_node_resources(include_id=True, include_gb_ram=True) if any([args.gpu_per_worker, args.cpu_per_worker, args.ram_gb_per_worker]) and args.num_workers: @@ -97,7 +100,7 @@ def wrap_resources_to_jobs(jobs: list[str], args: argparse.Namespace) -> None: jobs = ["nvidia-smi"] * num_nodes for i, job in enumerate(jobs): gpu_node = gpu_node_resources[i % num_nodes] - print(f"[INFO]: Submitting job {i + 1} of {len(jobs)} with job '{job}' to node {gpu_node}") + print(f"[INFO]: Creating job {i + 1} of {len(jobs)} with job '{job}' to node {gpu_node}") print( f"[INFO]: Resource parameters: GPU: {args.gpu_per_worker[i]}" f" CPU: {args.cpu_per_worker[i]} RAM {args.ram_gb_per_worker[i]}" @@ -106,19 +109,19 @@ def wrap_resources_to_jobs(jobs: list[str], args: argparse.Namespace) -> None: num_gpus = args.gpu_per_worker[i] / args.num_workers[i] num_cpus = args.cpu_per_worker[i] / args.num_workers[i] memory = (args.ram_gb_per_worker[i] * 1024**3) / args.num_workers[i] - print(f"[INFO]: Requesting {num_gpus=} {num_cpus=} {memory=} id={gpu_node['id']}") - job = util.remote_execute_job.options( - num_gpus=num_gpus, - num_cpus=num_cpus, - memory=memory, - scheduling_strategy=NodeAffinitySchedulingStrategy(gpu_node["id"], soft=False), - ).remote(job, f"Job {i}", args.test) - job_results.append(job) - - results = ray.get(job_results) - for i, result in enumerate(results): - print(f"[INFO]: Job {i} result: {result}") - print("[INFO]: All jobs completed.") + job_objs.append( + util.Job( + cmd=job, + name=f"Job-{i + 1}", + resources=util.JobResource(num_gpus=num_gpus, num_cpus=num_cpus, memory=memory), + node=util.JobNode( + specific="node_id", + node_id=gpu_node["id"], + ), + ) + ) + # submit jobs + util.submit_wrapped_jobs(jobs=job_objs, test_mode=args.test, concurrent=False) if __name__ == "__main__": @@ -134,6 +137,15 @@ def wrap_resources_to_jobs(jobs: list[str], args: argparse.Namespace) -> None: "that GPU resources are correctly isolated." ), ) + parser.add_argument( + "--py_modules", + type=str, + nargs="*", + default=[], + help=( + "List of python modules or paths to add before running the job. Example: --py_modules my_package/my_package" + ), + ) parser.add_argument( "--sub_jobs", type=str, diff --git a/scripts/reinforcement_learning/rl_games/play.py b/scripts/reinforcement_learning/rl_games/play.py index c13bc8b5d7a5..ee2dbcdbb149 100644 --- a/scripts/reinforcement_learning/rl_games/play.py +++ b/scripts/reinforcement_learning/rl_games/play.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,6 +8,7 @@ """Launch Isaac Sim Simulator first.""" import argparse +import sys from isaaclab.app import AppLauncher @@ -20,7 +21,11 @@ ) 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="rl_games_cfg_entry_point", help="Name of the RL agent configuration entry point." +) parser.add_argument("--checkpoint", type=str, default=None, help="Path to model checkpoint.") +parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment") parser.add_argument( "--use_pretrained_checkpoint", action="store_true", @@ -35,11 +40,13 @@ # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments -args_cli = parser.parse_args() +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 @@ -47,36 +54,56 @@ """Rest everything follows.""" -import gymnasium as gym import math import os +import random import time -import torch +import gymnasium as gym +import torch from rl_games.common import env_configurations, vecenv from rl_games.common.player import BasePlayer from rl_games.torch_runner import Runner -from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent +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.utils.pretrained_checkpoint import get_published_pretrained_checkpoint from isaaclab_rl.rl_games import RlGamesGpuEnv, RlGamesVecEnvWrapper +from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils import get_checkpoint_path, load_cfg_from_registry, parse_env_cfg +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 main(): +@hydra_task_config(args_cli.task, args_cli.agent) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: dict): """Play with RL-Games agent.""" - # parse env configuration - env_cfg = parse_env_cfg( - args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric - ) - agent_cfg = load_cfg_from_registry(args_cli.task, "rl_games_cfg_entry_point") + # grab task name for checkpoint path + task_name = args_cli.task.split(":")[-1] + train_task_name = task_name.replace("-Play", "") + + # 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 + + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + + agent_cfg["params"]["seed"] = args_cli.seed if args_cli.seed is not None else agent_cfg["params"]["seed"] + # set the environment seed (after multi-gpu config for updated rank from agent seed) + # note: certain randomizations occur in the environment initialization so we set the seed here + env_cfg.seed = agent_cfg["params"]["seed"] # specify directory for logging experiments log_root_path = os.path.join("logs", "rl_games", agent_cfg["params"]["config"]["name"]) @@ -84,7 +111,7 @@ def main(): print(f"[INFO] Loading experiment from directory: {log_root_path}") # find checkpoint if args_cli.use_pretrained_checkpoint: - resume_path = get_published_pretrained_checkpoint("rl_games", args_cli.task) + resume_path = get_published_pretrained_checkpoint("rl_games", train_task_name) if not resume_path: print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.") return @@ -103,10 +130,15 @@ def main(): 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) clip_actions = agent_cfg["params"]["env"].get("clip_actions", math.inf) + obs_groups = agent_cfg["params"]["env"].get("obs_groups") + concate_obs_groups = agent_cfg["params"]["env"].get("concate_obs_groups", True) # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) @@ -128,7 +160,7 @@ def main(): env = gym.wrappers.RecordVideo(env, **video_kwargs) # wrap around environment for rl-games - env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions) + env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions, obs_groups, concate_obs_groups) # register the environment to rl-games registry # note: in agents configuration: environment name must be "rlgpu" @@ -152,7 +184,7 @@ def main(): agent.restore(resume_path) agent.reset() - dt = env.unwrapped.physics_dt + dt = env.unwrapped.step_dt # reset environment obs = env.reset() @@ -187,7 +219,7 @@ def main(): s[:, dones, :] = 0.0 if args_cli.video: timestep += 1 - # Exit the play loop after recording one video + # exit the play loop after recording one video if timestep == args_cli.video_length: break diff --git a/scripts/reinforcement_learning/rl_games/train.py b/scripts/reinforcement_learning/rl_games/train.py index 17247ffcf845..5b85ba5b429d 100644 --- a/scripts/reinforcement_learning/rl_games/train.py +++ b/scripts/reinforcement_learning/rl_games/train.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,6 +9,7 @@ import argparse import sys +from distutils.util import strtobool from isaaclab.app import AppLauncher @@ -19,6 +20,9 @@ parser.add_argument("--video_interval", type=int, default=2000, help="Interval between video recordings (in steps).") 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="rl_games_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( "--distributed", action="store_true", default=False, help="Run training with multiple GPUs or nodes." @@ -26,7 +30,21 @@ parser.add_argument("--checkpoint", type=str, default=None, help="Path to model checkpoint.") parser.add_argument("--sigma", type=str, default=None, help="The policy's initial standard deviation.") parser.add_argument("--max_iterations", type=int, default=None, help="RL Policy training iterations.") - +parser.add_argument("--wandb-project-name", type=str, default=None, help="the wandb's project name") +parser.add_argument("--wandb-entity", type=str, default=None, help="the entity (team) of wandb's project") +parser.add_argument("--wandb-name", type=str, default=None, help="the name of wandb's run") +parser.add_argument( + "--track", + type=lambda x: bool(strtobool(x)), + default=False, + nargs="?", + const=True, + help="if toggled, this experiment will be tracked with Weights and Biases", +) +parser.add_argument("--export_io_descriptors", action="store_true", default=False, help="Export IO descriptors.") +parser.add_argument( + "--ray-proc-id", "-rid", type=int, default=None, help="Automatically configured by Ray integration, otherwise None." +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments @@ -44,12 +62,14 @@ """Rest everything follows.""" -import gymnasium as gym +import logging import math import os import random +import time from datetime import datetime +import gymnasium as gym from rl_games.common import env_configurations, vecenv from rl_games.common.algo_observer import IsaacAlgoObserver from rl_games.torch_runner import Runner @@ -63,22 +83,31 @@ ) 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 +# import logger +logger = logging.getLogger(__name__) + # PLACEHOLDER: Extension template (do not remove this comment) -@hydra_task_config(args_cli.task, "rl_games_cfg_entry_point") +@hydra_task_config(args_cli.task, args_cli.agent) def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: dict): """Train with RL-Games agent.""" # 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." + ) # randomly sample a seed if seed = -1 if args_cli.seed == -1: @@ -109,8 +138,13 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen env_cfg.seed = agent_cfg["params"]["seed"] # specify directory for logging experiments - log_root_path = os.path.join("logs", "rl_games", agent_cfg["params"]["config"]["name"]) - log_root_path = os.path.abspath(log_root_path) + config_name = agent_cfg["params"]["config"]["name"] + log_root_path = os.path.join("logs", "rl_games", config_name) + if "pbt" in agent_cfg and agent_cfg["pbt"]["directory"] != ".": + log_root_path = os.path.join(agent_cfg["pbt"]["directory"], log_root_path) + else: + log_root_path = os.path.abspath(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")) @@ -118,17 +152,31 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # logging directory path: / agent_cfg["params"]["config"]["train_dir"] = log_root_path agent_cfg["params"]["config"]["full_experiment_name"] = log_dir + wandb_project = config_name if args_cli.wandb_project_name is None else args_cli.wandb_project_name + experiment_name = log_dir if args_cli.wandb_name is None else args_cli.wandb_name # 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) + print(f"Exact experiment name requested from command line: {os.path.join(log_root_path, log_dir)}") # read configurations about the agent-training rl_device = agent_cfg["params"]["config"]["device"] clip_obs = agent_cfg["params"]["env"].get("clip_observations", math.inf) clip_actions = agent_cfg["params"]["env"].get("clip_actions", math.inf) + obs_groups = agent_cfg["params"]["env"].get("obs_groups") + concate_obs_groups = agent_cfg["params"]["env"].get("concate_obs_groups", True) + + # set the IO descriptors export flag if requested + if isinstance(env_cfg, ManagerBasedRLEnvCfg): + env_cfg.export_io_descriptors = args_cli.export_io_descriptors + else: + logger.warning( + "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) @@ -149,8 +197,10 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print_dict(video_kwargs, nesting=4) env = gym.wrappers.RecordVideo(env, **video_kwargs) + start_time = time.time() + # wrap around environment for rl-games - env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions) + env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions, obs_groups, concate_obs_groups) # register the environment to rl-games registry # note: in agents configuration: environment name must be "rlgpu" @@ -162,17 +212,44 @@ 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 runner.reset() # train the agent + + global_rank = int(os.getenv("RANK", "0")) + if args_cli.track and global_rank == 0: + if args_cli.wandb_entity is None: + raise ValueError("Weights and Biases entity must be specified for tracking.") + import wandb + + wandb.init( + project=wandb_project, + entity=args_cli.wandb_entity, + name=experiment_name, + sync_tensorboard=True, + monitor_gym=True, + save_code=True, + ) + 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}) else: runner.run({"train": True, "play": False, "sigma": train_sigma}) + print(f"Training time: {round(time.time() - start_time, 2)} seconds") + # close the simulator env.close() diff --git a/scripts/reinforcement_learning/rsl_rl/cli_args.py b/scripts/reinforcement_learning/rsl_rl/cli_args.py index 96747aeb9171..51cf868b5cd5 100644 --- a/scripts/reinforcement_learning/rsl_rl/cli_args.py +++ b/scripts/reinforcement_learning/rsl_rl/cli_args.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,7 +10,7 @@ from typing import TYPE_CHECKING if TYPE_CHECKING: - from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg + from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg def add_rsl_rl_args(parser: argparse.ArgumentParser): @@ -27,7 +27,7 @@ def add_rsl_rl_args(parser: argparse.ArgumentParser): ) arg_group.add_argument("--run_name", type=str, default=None, help="Run name suffix to the log directory.") # -- load arguments - arg_group.add_argument("--resume", type=bool, default=None, help="Whether to resume from a checkpoint.") + arg_group.add_argument("--resume", action="store_true", default=False, help="Whether to resume from a checkpoint.") arg_group.add_argument("--load_run", type=str, default=None, help="Name of the run folder to resume from.") arg_group.add_argument("--checkpoint", type=str, default=None, help="Checkpoint file to resume from.") # -- logger arguments @@ -39,7 +39,7 @@ def add_rsl_rl_args(parser: argparse.ArgumentParser): ) -def parse_rsl_rl_cfg(task_name: str, args_cli: argparse.Namespace) -> RslRlOnPolicyRunnerCfg: +def parse_rsl_rl_cfg(task_name: str, args_cli: argparse.Namespace) -> RslRlBaseRunnerCfg: """Parse configuration for RSL-RL agent based on inputs. Args: @@ -52,12 +52,12 @@ def parse_rsl_rl_cfg(task_name: str, args_cli: argparse.Namespace) -> RslRlOnPol from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry # load the default configuration - rslrl_cfg: RslRlOnPolicyRunnerCfg = load_cfg_from_registry(task_name, "rsl_rl_cfg_entry_point") + rslrl_cfg: RslRlBaseRunnerCfg = load_cfg_from_registry(task_name, "rsl_rl_cfg_entry_point") rslrl_cfg = update_rsl_rl_cfg(rslrl_cfg, args_cli) return rslrl_cfg -def update_rsl_rl_cfg(agent_cfg: RslRlOnPolicyRunnerCfg, args_cli: argparse.Namespace): +def update_rsl_rl_cfg(agent_cfg: RslRlBaseRunnerCfg, args_cli: argparse.Namespace): """Update configuration for RSL-RL agent based on inputs. Args: diff --git a/scripts/reinforcement_learning/rsl_rl/play.py b/scripts/reinforcement_learning/rsl_rl/play.py index a60cac141272..beb920721738 100644 --- a/scripts/reinforcement_learning/rsl_rl/play.py +++ b/scripts/reinforcement_learning/rsl_rl/play.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,6 +8,7 @@ """Launch Isaac Sim Simulator first.""" import argparse +import sys from isaaclab.app import AppLauncher @@ -23,6 +24,10 @@ ) 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( "--use_pretrained_checkpoint", action="store_true", @@ -33,51 +38,70 @@ cli_args.add_rsl_rl_args(parser) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() +# 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 - -from rsl_rl.runners import OnPolicyRunner -from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent +import gymnasium as gym +import torch +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.utils.pretrained_checkpoint import get_published_pretrained_checkpoint -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper, export_policy_as_jit, export_policy_as_onnx +from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg, RslRlVecEnvWrapper, export_policy_as_jit, export_policy_as_onnx +from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils import get_checkpoint_path, parse_env_cfg +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 main(): +@hydra_task_config(args_cli.task, args_cli.agent) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlBaseRunnerCfg): """Play with RSL-RL agent.""" - # parse configuration - env_cfg = parse_env_cfg( - args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric - ) - agent_cfg: RslRlOnPolicyRunnerCfg = cli_args.parse_rsl_rl_cfg(args_cli.task, args_cli) + # grab task name for checkpoint path + task_name = args_cli.task.split(":")[-1] + train_task_name = task_name.replace("-Play", "") + + # override configurations with non-hydra CLI arguments + agent_cfg: RslRlBaseRunnerCfg = 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.use_pretrained_checkpoint: - resume_path = get_published_pretrained_checkpoint("rsl_rl", args_cli.task) + resume_path = get_published_pretrained_checkpoint("rsl_rl", train_task_name) if not resume_path: print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.") return @@ -88,6 +112,9 @@ def main(): 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) @@ -112,25 +139,43 @@ def main(): print(f"[INFO]: Loading model checkpoint from: {resume_path}") # load previously trained model - ppo_runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) - ppo_runner.load(resume_path) + 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 = ppo_runner.get_inference_policy(device=env.unwrapped.device) + 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( - ppo_runner.alg.actor_critic, ppo_runner.obs_normalizer, path=export_model_dir, filename="policy.pt" - ) - export_policy_as_onnx( - ppo_runner.alg.actor_critic, normalizer=ppo_runner.obs_normalizer, path=export_model_dir, filename="policy.onnx" - ) + 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.physics_dt + dt = env.unwrapped.step_dt # reset environment - obs, _ = env.get_observations() + obs = env.get_observations() timestep = 0 # simulate environment while simulation_app.is_running(): @@ -140,7 +185,9 @@ def main(): # agent stepping actions = policy(obs) # env stepping - obs, _, _, _ = env.step(actions) + obs, _, dones, _ = env.step(actions) + # reset recurrent states for episodes that have terminated + policy_nn.reset(dones) if args_cli.video: timestep += 1 # Exit the play loop after recording one video diff --git a/scripts/reinforcement_learning/rsl_rl/train.py b/scripts/reinforcement_learning/rsl_rl/train.py index ab61b32f6b80..0cce12d7eba0 100644 --- a/scripts/reinforcement_learning/rsl_rl/train.py +++ b/scripts/reinforcement_learning/rsl_rl/train.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -15,7 +15,6 @@ # local imports import cli_args # isort: skip - # add argparse arguments parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") parser.add_argument("--video", action="store_true", default=False, help="Record videos during training.") @@ -23,8 +22,18 @@ parser.add_argument("--video_interval", type=int, default=2000, help="Interval between video recordings (in steps).") 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("--max_iterations", type=int, default=None, help="RL Policy training iterations.") +parser.add_argument( + "--distributed", action="store_true", default=False, help="Run training with multiple GPUs or nodes." +) +parser.add_argument("--export_io_descriptors", action="store_true", default=False, help="Export IO descriptors.") +parser.add_argument( + "--ray-proc-id", "-rid", type=int, default=None, help="Automatically configured by Ray integration, otherwise None." +) # append RSL-RL cli arguments cli_args.add_rsl_rl_args(parser) # append AppLauncher cli args @@ -42,14 +51,38 @@ app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app +"""Check for minimum supported RSL-RL version.""" + +import importlib.metadata as metadata +import platform + +from packaging import version + +# check minimum supported rsl-rl version +RSL_RL_VERSION = "3.0.1" +installed_version = metadata.version("rsl-rl-lib") +if version.parse(installed_version) < version.parse(RSL_RL_VERSION): + if platform.system() == "Windows": + cmd = [r".\isaaclab.bat", "-p", "-m", "pip", "install", f"rsl-rl-lib=={RSL_RL_VERSION}"] + else: + cmd = ["./isaaclab.sh", "-p", "-m", "pip", "install", f"rsl-rl-lib=={RSL_RL_VERSION}"] + print( + f"Please install the correct version of RSL-RL.\nExisting version is: '{installed_version}'" + f" and required version is: '{RSL_RL_VERSION}'.\nTo install the correct version, run:" + f"\n\n\t{' '.join(cmd)}\n" + ) + exit(1) + """Rest everything follows.""" -import gymnasium as gym +import logging import os -import torch +import time from datetime import datetime -from rsl_rl.runners import OnPolicyRunner +import gymnasium as gym +import torch +from rsl_rl.runners import DistillationRunner, OnPolicyRunner from isaaclab.envs import ( DirectMARLEnv, @@ -59,14 +92,17 @@ 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 RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper +from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg, RslRlVecEnvWrapper import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils import get_checkpoint_path from isaaclab_tasks.utils.hydra import hydra_task_config +# import logger +logger = logging.getLogger(__name__) + # PLACEHOLDER: Extension template (do not remove this comment) torch.backends.cuda.matmul.allow_tf32 = True @@ -75,8 +111,8 @@ torch.backends.cudnn.benchmark = False -@hydra_task_config(args_cli.task, "rsl_rl_cfg_entry_point") -def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlOnPolicyRunnerCfg): +@hydra_task_config(args_cli.task, args_cli.agent) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlBaseRunnerCfg): """Train with RSL-RL agent.""" # override configurations with non-hydra CLI arguments agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) @@ -89,6 +125,22 @@ 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: + env_cfg.sim.device = f"cuda:{app_launcher.local_rank}" + agent_cfg.device = f"cuda:{app_launcher.local_rank}" + + # set seed to have diversity in different threads + seed = agent_cfg.seed + app_launcher.local_rank + env_cfg.seed = seed + agent_cfg.seed = seed # specify directory for logging experiments log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) @@ -96,12 +148,24 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print(f"[INFO] Logging experiment in directory: {log_root_path}") # specify directory for logging runs: {time-stamp}_{run_name} log_dir = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - # This way, the Ray Tune workflow can extract experiment name. + # The Ray Tune workflow extracts experiment name using the logging line below, hence, do not + # change it (see PR #2346, comment-2819298849) print(f"Exact experiment name requested from command line: {log_dir}") if agent_cfg.run_name: log_dir += f"_{agent_cfg.run_name}" log_dir = os.path.join(log_root_path, log_dir) + # set the IO descriptors export flag if requested + if isinstance(env_cfg, ManagerBasedRLEnvCfg): + env_cfg.export_io_descriptors = args_cli.export_io_descriptors + else: + logger.warning( + "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) @@ -110,7 +174,7 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen env = multi_agent_to_single_agent(env) # save resume path before creating a new log_dir - if agent_cfg.resume: + if agent_cfg.resume or agent_cfg.algorithm.class_name == "Distillation": resume_path = get_checkpoint_path(log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) # wrap for video recording @@ -125,15 +189,22 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print_dict(video_kwargs, nesting=4) env = gym.wrappers.RecordVideo(env, **video_kwargs) + start_time = time.time() + # wrap around environment for rsl-rl env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) # create runner from rsl-rl - runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device) + if agent_cfg.class_name == "OnPolicyRunner": + runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device) + elif agent_cfg.class_name == "DistillationRunner": + runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device) + else: + raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") # write git state to logs runner.add_git_repo_to_log(__file__) # load the checkpoint - if agent_cfg.resume: + if agent_cfg.resume or agent_cfg.algorithm.class_name == "Distillation": print(f"[INFO]: Loading model checkpoint from: {resume_path}") # load previously trained model runner.load(resume_path) @@ -141,12 +212,12 @@ 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) + print(f"Training time: {round(time.time() - start_time, 2)} seconds") + # close the simulator env.close() diff --git a/scripts/reinforcement_learning/sb3/play.py b/scripts/reinforcement_learning/sb3/play.py index 5407804b304a..4afe943f62fd 100644 --- a/scripts/reinforcement_learning/sb3/play.py +++ b/scripts/reinforcement_learning/sb3/play.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,6 +8,8 @@ """Launch Isaac Sim Simulator first.""" import argparse +import sys +from pathlib import Path from isaaclab.app import AppLauncher @@ -20,7 +22,11 @@ ) 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="sb3_cfg_entry_point", help="Name of the RL agent configuration entry point." +) parser.add_argument("--checkpoint", type=str, default=None, help="Path to model checkpoint.") +parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment") parser.add_argument( "--use_pretrained_checkpoint", action="store_true", @@ -32,74 +38,104 @@ help="When no checkpoint provided, use the last saved model. Otherwise use the best saved model.", ) parser.add_argument("--real-time", action="store_true", default=False, help="Run in real-time, if possible.") +parser.add_argument( + "--keep_all_info", + action="store_true", + default=False, + help="Use a slower SB3 wrapper but keep all the extra training info.", +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments -args_cli = parser.parse_args() +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 numpy as np import os +import random import time -import torch +import gymnasium as gym +import torch from stable_baselines3 import PPO from stable_baselines3.common.vec_env import VecNormalize -from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent +from isaaclab.envs import ( + DirectMARLEnv, + DirectMARLEnvCfg, + DirectRLEnvCfg, + ManagerBasedRLEnvCfg, + multi_agent_to_single_agent, +) from isaaclab.utils.dict import print_dict -from isaaclab.utils.pretrained_checkpoint import get_published_pretrained_checkpoint from isaaclab_rl.sb3 import Sb3VecEnvWrapper, process_sb3_cfg +from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils.parse_cfg import get_checkpoint_path, load_cfg_from_registry, parse_env_cfg +from isaaclab_tasks.utils.hydra import hydra_task_config +from isaaclab_tasks.utils.parse_cfg import get_checkpoint_path # PLACEHOLDER: Extension template (do not remove this comment) -def main(): +@hydra_task_config(args_cli.task, args_cli.agent) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: dict): """Play with stable-baselines agent.""" - # parse configuration - env_cfg = parse_env_cfg( - args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric - ) - agent_cfg = load_cfg_from_registry(args_cli.task, "sb3_cfg_entry_point") + # grab task name for checkpoint path + task_name = args_cli.task.split(":")[-1] + train_task_name = task_name.replace("-Play", "") + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + + # 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 + agent_cfg["seed"] = args_cli.seed if args_cli.seed is not None else agent_cfg["seed"] + # 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 # directory for logging into - log_root_path = os.path.join("logs", "sb3", args_cli.task) + log_root_path = os.path.join("logs", "sb3", train_task_name) log_root_path = os.path.abspath(log_root_path) # checkpoint and log_dir stuff if args_cli.use_pretrained_checkpoint: - checkpoint_path = get_published_pretrained_checkpoint("sb3", args_cli.task) + checkpoint_path = get_published_pretrained_checkpoint("sb3", train_task_name) if not checkpoint_path: print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.") return elif args_cli.checkpoint is None: + # FIXME: last checkpoint doesn't seem to really use the last one' if args_cli.use_last_checkpoint: checkpoint = "model_.*.zip" else: checkpoint = "model.zip" - checkpoint_path = get_checkpoint_path(log_root_path, ".*", checkpoint) + checkpoint_path = get_checkpoint_path(log_root_path, ".*", checkpoint, sort_alpha=False) else: checkpoint_path = args_cli.checkpoint log_dir = os.path.dirname(checkpoint_path) - # post-process agent configuration - agent_cfg = process_sb3_cfg(agent_cfg) + # 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) + # post-process agent configuration + agent_cfg = process_sb3_cfg(agent_cfg, env.unwrapped.num_envs) + # convert to single-agent instance if required by the RL algorithm if isinstance(env.unwrapped, DirectMARLEnv): env = multi_agent_to_single_agent(env) @@ -116,25 +152,32 @@ def main(): print_dict(video_kwargs, nesting=4) env = gym.wrappers.RecordVideo(env, **video_kwargs) # wrap around environment for stable baselines - env = Sb3VecEnvWrapper(env) + env = Sb3VecEnvWrapper(env, fast_variant=not args_cli.keep_all_info) + + vec_norm_path = checkpoint_path.replace("/model", "/model_vecnormalize").replace(".zip", ".pkl") + vec_norm_path = Path(vec_norm_path) # normalize environment (if needed) - if "normalize_input" in agent_cfg: + if vec_norm_path.exists(): + print(f"Loading saved normalization: {vec_norm_path}") + env = VecNormalize.load(vec_norm_path, env) + # do not update them at test time + env.training = False + # reward normalization is not needed at test time + env.norm_reward = False + elif "normalize_input" in agent_cfg: env = VecNormalize( env, training=True, norm_obs="normalize_input" in agent_cfg and agent_cfg.pop("normalize_input"), - norm_reward="normalize_value" in agent_cfg and agent_cfg.pop("normalize_value"), clip_obs="clip_obs" in agent_cfg and agent_cfg.pop("clip_obs"), - gamma=agent_cfg["gamma"], - clip_reward=np.inf, ) # create agent from stable baselines print(f"Loading checkpoint from: {checkpoint_path}") agent = PPO.load(checkpoint_path, env, print_system_info=True) - dt = env.unwrapped.physics_dt + dt = env.unwrapped.step_dt # reset environment obs = env.reset() diff --git a/scripts/reinforcement_learning/sb3/train.py b/scripts/reinforcement_learning/sb3/train.py index ba956b894a8b..32549dcd4ea3 100644 --- a/scripts/reinforcement_learning/sb3/train.py +++ b/scripts/reinforcement_learning/sb3/train.py @@ -1,19 +1,18 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""Script to train RL agent with Stable Baselines3. -Since Stable-Baselines3 does not support buffers living on GPU directly, -we recommend using smaller number of environments. Otherwise, -there will be significant overhead in GPU->CPU transfer. -""" +"""Script to train RL agent with Stable Baselines3.""" """Launch Isaac Sim Simulator first.""" import argparse +import contextlib +import signal import sys +from pathlib import Path from isaaclab.app import AppLauncher @@ -24,8 +23,23 @@ parser.add_argument("--video_interval", type=int, default=2000, help="Interval between video recordings (in steps).") 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="sb3_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("--log_interval", type=int, default=100_000, help="Log data every n timesteps.") +parser.add_argument("--checkpoint", type=str, default=None, help="Continue the training from checkpoint.") parser.add_argument("--max_iterations", type=int, default=None, help="RL Policy training iterations.") +parser.add_argument("--export_io_descriptors", action="store_true", default=False, help="Export IO descriptors.") +parser.add_argument( + "--keep_all_info", + action="store_true", + default=False, + help="Use a slower SB3 wrapper but keep all the extra training info.", +) +parser.add_argument( + "--ray-proc-id", "-rid", type=int, default=None, help="Automatically configured by Ray integration, otherwise None." +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments @@ -41,17 +55,36 @@ app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app + +def cleanup_pbar(*args): + """ + A small helper to stop training and + cleanup progress bar properly on ctrl+c + """ + import gc + + tqdm_objects = [obj for obj in gc.get_objects() if "tqdm" in type(obj).__name__] + for tqdm_object in tqdm_objects: + if "tqdm_rich" in type(tqdm_object).__name__: + tqdm_object.close() + raise KeyboardInterrupt + + +# disable KeyboardInterrupt override +signal.signal(signal.SIGINT, cleanup_pbar) + """Rest everything follows.""" -import gymnasium as gym -import numpy as np +import logging import os import random +import time from datetime import datetime +import gymnasium as gym +import numpy as np from stable_baselines3 import PPO -from stable_baselines3.common.callbacks import CheckpointCallback -from stable_baselines3.common.logger import configure +from stable_baselines3.common.callbacks import CheckpointCallback, LogEveryNTimesteps from stable_baselines3.common.vec_env import VecNormalize from isaaclab.envs import ( @@ -62,17 +95,19 @@ 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 import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.hydra import hydra_task_config +# import logger +logger = logging.getLogger(__name__) # PLACEHOLDER: Extension template (do not remove this comment) -@hydra_task_config(args_cli.task, "sb3_cfg_entry_point") +@hydra_task_config(args_cli.task, args_cli.agent) def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: dict): """Train with stable-baselines agent.""" # randomly sample a seed if seed = -1 @@ -95,20 +130,35 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen run_info = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") log_root_path = os.path.abspath(os.path.join("logs", "sb3", args_cli.task)) print(f"[INFO] Logging experiment in directory: {log_root_path}") + # The Ray Tune workflow extracts experiment name using the logging line below, hence, + # do not change it (see PR #2346, comment-2819298849) print(f"Exact experiment name requested from command line: {run_info}") log_dir = os.path.join(log_root_path, run_info) # 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) + (Path(log_dir) / "command.txt").write_text(command) # post-process agent configuration - agent_cfg = process_sb3_cfg(agent_cfg) + agent_cfg = process_sb3_cfg(agent_cfg, env_cfg.scene.num_envs) # read configurations about the agent-training policy_arch = agent_cfg.pop("policy") n_timesteps = agent_cfg.pop("n_timesteps") + # set the IO descriptors export flag if requested + if isinstance(env_cfg, ManagerBasedRLEnvCfg): + env_cfg.export_io_descriptors = args_cli.export_io_descriptors + else: + logger.warning( + "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) @@ -128,32 +178,56 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print_dict(video_kwargs, nesting=4) env = gym.wrappers.RecordVideo(env, **video_kwargs) + start_time = time.time() + # wrap around environment for stable baselines - env = Sb3VecEnvWrapper(env) + env = Sb3VecEnvWrapper(env, fast_variant=not args_cli.keep_all_info) - if "normalize_input" in agent_cfg: + norm_keys = {"normalize_input", "normalize_value", "clip_obs"} + norm_args = {} + for key in norm_keys: + if key in agent_cfg: + norm_args[key] = agent_cfg.pop(key) + + if norm_args and norm_args.get("normalize_input"): + print(f"Normalizing input, {norm_args=}") env = VecNormalize( env, training=True, - norm_obs="normalize_input" in agent_cfg and agent_cfg.pop("normalize_input"), - norm_reward="normalize_value" in agent_cfg and agent_cfg.pop("normalize_value"), - clip_obs="clip_obs" in agent_cfg and agent_cfg.pop("clip_obs"), + norm_obs=norm_args["normalize_input"], + norm_reward=norm_args.get("normalize_value", False), + clip_obs=norm_args.get("clip_obs", 100.0), gamma=agent_cfg["gamma"], clip_reward=np.inf, ) # create agent from stable baselines - agent = PPO(policy_arch, env, verbose=1, **agent_cfg) - # configure the logger - new_logger = configure(log_dir, ["stdout", "tensorboard"]) - agent.set_logger(new_logger) + agent = PPO(policy_arch, env, verbose=1, tensorboard_log=log_dir, **agent_cfg) + if args_cli.checkpoint is not None: + agent = agent.load(args_cli.checkpoint, env, print_system_info=True) # callbacks for agent checkpoint_callback = CheckpointCallback(save_freq=1000, save_path=log_dir, name_prefix="model", verbose=2) + callbacks = [checkpoint_callback, LogEveryNTimesteps(n_steps=args_cli.log_interval)] + # train the agent - agent.learn(total_timesteps=n_timesteps, callback=checkpoint_callback) + with contextlib.suppress(KeyboardInterrupt): + agent.learn( + total_timesteps=n_timesteps, + callback=callbacks, + progress_bar=True, + log_interval=None, + ) # save the final model agent.save(os.path.join(log_dir, "model")) + print("Saving to:") + print(os.path.join(log_dir, "model.zip")) + + if isinstance(env, VecNormalize): + print("Saving normalization") + env.save(os.path.join(log_dir, "model_vecnormalize.pkl")) + + print(f"Training time: {round(time.time() - start_time, 2)} seconds") # close the simulator env.close() diff --git a/scripts/reinforcement_learning/skrl/play.py b/scripts/reinforcement_learning/skrl/play.py index 1339d22b5365..089ec7561979 100644 --- a/scripts/reinforcement_learning/skrl/play.py +++ b/scripts/reinforcement_learning/skrl/play.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -13,6 +13,7 @@ """Launch Isaac Sim Simulator first.""" import argparse +import sys from isaaclab.app import AppLauncher @@ -25,7 +26,17 @@ ) 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=None, + help=( + "Name of the RL agent configuration entry point. Defaults to None, in which case the argument " + "--algorithm is used to determine the default agent configuration entry point." + ), +) parser.add_argument("--checkpoint", type=str, default=None, help="Path to model checkpoint.") +parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment") parser.add_argument( "--use_pretrained_checkpoint", action="store_true", @@ -49,27 +60,31 @@ # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() +# 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 random import time -import torch +import gymnasium as gym import skrl +import torch from packaging import version # check for minimum supported skrl version -SKRL_VERSION = "1.4.2" +SKRL_VERSION = "1.4.3" if version.parse(skrl.__version__) < version.parse(SKRL_VERSION): skrl.logger.error( f"Unsupported skrl version: {skrl.__version__}. " @@ -82,35 +97,56 @@ elif args_cli.ml_framework.startswith("jax"): from skrl.utils.runner.jax import Runner -from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent +from isaaclab.envs import ( + DirectMARLEnv, + DirectMARLEnvCfg, + DirectRLEnvCfg, + ManagerBasedRLEnvCfg, + multi_agent_to_single_agent, +) from isaaclab.utils.dict import print_dict -from isaaclab.utils.pretrained_checkpoint import get_published_pretrained_checkpoint from isaaclab_rl.skrl import SkrlVecEnvWrapper +from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils import get_checkpoint_path, load_cfg_from_registry, parse_env_cfg +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) # config shortcuts -algorithm = args_cli.algorithm.lower() +if args_cli.agent is None: + algorithm = args_cli.algorithm.lower() + 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() -def main(): +@hydra_task_config(args_cli.task, agent_cfg_entry_point) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, experiment_cfg: dict): """Play with skrl agent.""" + # grab task name for checkpoint path + task_name = args_cli.task.split(":")[-1] + train_task_name = task_name.replace("-Play", "") + + # 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 + # configure the ML framework into the global skrl variable if args_cli.ml_framework.startswith("jax"): skrl.config.jax.backend = "jax" if args_cli.ml_framework == "jax" else "numpy" - # parse configuration - env_cfg = parse_env_cfg( - args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric - ) - try: - experiment_cfg = load_cfg_from_registry(args_cli.task, f"skrl_{algorithm}_cfg_entry_point") - except ValueError: - experiment_cfg = load_cfg_from_registry(args_cli.task, "skrl_cfg_entry_point") + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + + # set the agent and environment seed from command line + # note: certain randomization occur in the environment initialization so we set the seed here + experiment_cfg["seed"] = args_cli.seed if args_cli.seed is not None else experiment_cfg["seed"] + env_cfg.seed = experiment_cfg["seed"] # specify directory for logging experiments (load checkpoint) log_root_path = os.path.join("logs", "skrl", experiment_cfg["agent"]["experiment"]["directory"]) @@ -118,7 +154,7 @@ def main(): print(f"[INFO] Loading experiment from directory: {log_root_path}") # get checkpoint path if args_cli.use_pretrained_checkpoint: - resume_path = get_published_pretrained_checkpoint("skrl", args_cli.task) + resume_path = get_published_pretrained_checkpoint("skrl", train_task_name) if not resume_path: print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.") return @@ -130,6 +166,9 @@ def main(): ) 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) @@ -137,11 +176,11 @@ def main(): if isinstance(env.unwrapped, DirectMARLEnv) and algorithm in ["ppo"]: env = multi_agent_to_single_agent(env) - # get environment (physics) dt for real-time evaluation + # get environment (step) dt for real-time evaluation try: - dt = env.physics_dt + dt = env.step_dt except AttributeError: - dt = env.unwrapped.physics_dt + dt = env.unwrapped.step_dt # wrap for video recording if args_cli.video: diff --git a/scripts/reinforcement_learning/skrl/train.py b/scripts/reinforcement_learning/skrl/train.py index ac6507ca29c5..cf2edce47435 100644 --- a/scripts/reinforcement_learning/skrl/train.py +++ b/scripts/reinforcement_learning/skrl/train.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -24,12 +24,22 @@ parser.add_argument("--video_interval", type=int, default=2000, help="Interval between video recordings (in steps).") 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=None, + help=( + "Name of the RL agent configuration entry point. Defaults to None, in which case the argument " + "--algorithm is used to determine the default agent configuration entry point." + ), +) parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment") parser.add_argument( "--distributed", action="store_true", default=False, help="Run training with multiple GPUs or nodes." ) parser.add_argument("--checkpoint", type=str, default=None, help="Path to model checkpoint to resume training.") parser.add_argument("--max_iterations", type=int, default=None, help="RL Policy training iterations.") +parser.add_argument("--export_io_descriptors", action="store_true", default=False, help="Export IO descriptors.") parser.add_argument( "--ml_framework", type=str, @@ -44,7 +54,9 @@ choices=["AMP", "PPO", "IPPO", "MAPPO"], help="The RL algorithm used for training the skrl agent.", ) - +parser.add_argument( + "--ray-proc-id", "-rid", type=int, default=None, help="Automatically configured by Ray integration, otherwise None." +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments @@ -62,16 +74,18 @@ """Rest everything follows.""" -import gymnasium as gym +import logging import os import random +import time from datetime import datetime +import gymnasium as gym import skrl from packaging import version # check for minimum supported skrl version -SKRL_VERSION = "1.4.2" +SKRL_VERSION = "1.4.3" if version.parse(skrl.__version__) < version.parse(SKRL_VERSION): skrl.logger.error( f"Unsupported skrl version: {skrl.__version__}. " @@ -93,18 +107,25 @@ ) 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 import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.hydra import hydra_task_config +# import logger +logger = logging.getLogger(__name__) + # PLACEHOLDER: Extension template (do not remove this comment) # config shortcuts -algorithm = args_cli.algorithm.lower() -agent_cfg_entry_point = "skrl_cfg_entry_point" if algorithm in ["ppo"] else f"skrl_{algorithm}_cfg_entry_point" +if args_cli.agent is None: + algorithm = args_cli.algorithm.lower() + 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) @@ -114,6 +135,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}" @@ -140,9 +168,11 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print(f"[INFO] Logging experiment in directory: {log_root_path}") # specify directory for logging runs: {time-stamp}_{run_name} log_dir = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + f"_{algorithm}_{args_cli.ml_framework}" - print(f"Exact experiment name requested from command line {log_dir}") + # The Ray Tune workflow extracts experiment name using the logging line below, hence, + # do not change it (see PR #2346, comment-2819298849) + print(f"Exact experiment name requested from command line: {log_dir}") if agent_cfg["agent"]["experiment"]["experiment_name"]: - log_dir += f'_{agent_cfg["agent"]["experiment"]["experiment_name"]}' + log_dir += f"_{agent_cfg['agent']['experiment']['experiment_name']}" # set directory into agent config agent_cfg["agent"]["experiment"]["directory"] = log_root_path agent_cfg["agent"]["experiment"]["experiment_name"] = log_dir @@ -152,12 +182,21 @@ 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 + # set the IO descriptors export flag if requested + if isinstance(env_cfg, ManagerBasedRLEnvCfg): + env_cfg.export_io_descriptors = args_cli.export_io_descriptors + else: + logger.warning( + "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) @@ -177,6 +216,8 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print_dict(video_kwargs, nesting=4) env = gym.wrappers.RecordVideo(env, **video_kwargs) + start_time = time.time() + # wrap around environment for skrl env = SkrlVecEnvWrapper(env, ml_framework=args_cli.ml_framework) # same as: `wrap_env(env, wrapper="auto")` @@ -192,6 +233,8 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # run training runner.run() + print(f"Training time: {round(time.time() - start_time, 2)} seconds") + # close the simulator env.close() 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 000000000000..00d2925345b1 --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_anymal_d.yaml @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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 000000000000..839980c4d10e --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_g1.yaml @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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 000000000000..d9f976ee1bb6 --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_go2.yaml @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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 000000000000..cb0e0996f054 --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_h1.yaml @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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 000000000000..0ec1b389879f --- /dev/null +++ b/scripts/sim2sim_transfer/rsl_rl_transfer.py @@ -0,0 +1,286 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 os +import time + +import gymnasium as gym +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/blender_obj.py b/scripts/tools/blender_obj.py index a9ac5e62042c..c03a525fae49 100644 --- a/scripts/tools/blender_obj.py +++ b/scripts/tools/blender_obj.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -17,10 +17,11 @@ The script was tested on Blender 3.2 on Ubuntu 20.04LTS. """ -import bpy import os import sys +import bpy + def parse_cli_args(): """Parse the input command line arguments.""" diff --git a/scripts/tools/check_instanceable.py b/scripts/tools/check_instanceable.py index 9ca3a67c0245..d9ce51497d1a 100644 --- a/scripts/tools/check_instanceable.py +++ b/scripts/tools/check_instanceable.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -64,11 +64,10 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner -from isaacsim.core.utils.carb import set_carb_setting +import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils import Timer from isaaclab.utils.assets import check_file_path @@ -79,9 +78,11 @@ def main(): if not check_file_path(args_cli.input): raise ValueError(f"Invalid file path: {args_cli.input}") # Load kit helper - sim = SimulationContext( - stage_units_in_meters=1.0, physics_dt=0.01, rendering_dt=0.01, backend="torch", device="cuda:0" - ) + sim = SimulationContext(SimulationCfg(dt=0.01)) + + # get stage handle + stage = sim_utils.get_current_stage() + # enable fabric which avoids passing data over to USD structure # this speeds up the read-write operation of GPU buffers if sim.get_physics_context().use_gpu_pipeline: @@ -91,17 +92,17 @@ def main(): sim.get_physics_context().set_gpu_total_aggregate_pairs_capacity(2**21) # enable hydra scene-graph instancing # this is needed to visualize the scene when fabric is enabled - set_carb_setting(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + sim._settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) # Create interface to clone the scene - cloner = GridCloner(spacing=args_cli.spacing) + cloner = GridCloner(spacing=args_cli.spacing, stage=stage) cloner.define_base_env("/World/envs") - prim_utils.define_prim("/World/envs/env_0") + stage.DefinePrim("/World/envs/env_0", "Xform") # Spawn things into stage - prim_utils.create_prim("/World/Light", "DistantLight") + sim_utils.create_prim("/World/Light", "DistantLight") # Everything under the namespace "/World/envs/env_0" will be cloned - prim_utils.create_prim("/World/envs/env_0/Asset", "Xform", usd_path=os.path.abspath(args_cli.input)) + sim_utils.create_prim("/World/envs/env_0/Asset", "Xform", usd_path=os.path.abspath(args_cli.input)) # Clone the scene num_clones = args_cli.num_clones diff --git a/scripts/tools/convert_instanceable.py b/scripts/tools/convert_instanceable.py new file mode 100644 index 000000000000..7713bdc728f3 --- /dev/null +++ b/scripts/tools/convert_instanceable.py @@ -0,0 +1,160 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Utility to bulk convert URDFs or mesh files into instanceable USD format. + +Unified Robot Description Format (URDF) is an XML file format used in ROS to describe all elements of +a robot. For more information, see: http://wiki.ros.org/urdf + +This script uses the URDF importer extension from Isaac Sim (``omni.isaac.urdf_importer``) to convert a +URDF asset into USD format. It is designed as a convenience script for command-line use. For more +information on the URDF importer, see the documentation for the extension: +https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/ext_omni_isaac_urdf.html + + +positional arguments: + input The path to the input directory containing URDFs and Meshes. + output The path to directory to store the instanceable files. + +optional arguments: + -h, --help Show this help message and exit + --conversion-type Select file type to convert, urdf or mesh. (default: urdf) + --merge-joints Consolidate links that are connected by fixed joints. (default: False) + --fix-base Fix the base to where it is imported. (default: False) + --make-instanceable Make the asset instanceable for efficient cloning. (default: False) + +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Utility to convert a URDF or mesh into an Instanceable asset.") +parser.add_argument("input", type=str, help="The path to the input directory.") +parser.add_argument("output", type=str, help="The path to directory to store converted instanceable files.") +parser.add_argument( + "--conversion-type", type=str, default="both", help="Select file type to convert, urdf, mesh, or both." +) +parser.add_argument( + "--merge-joints", + action="store_true", + default=False, + help="Consolidate links that are connected by fixed joints.", +) +parser.add_argument("--fix-base", action="store_true", default=False, help="Fix the base to where it is imported.") +parser.add_argument( + "--make-instanceable", + action="store_true", + default=True, + help="Make the asset instanceable for efficient cloning.", +) +parser.add_argument( + "--collision-approximation", + type=str, + default="convexDecomposition", + choices=["convexDecomposition", "convexHull", "none"], + help=( + 'The method used for approximating collision mesh. Set to "none" ' + "to not add a collision mesh to the converted mesh." + ), +) +parser.add_argument( + "--mass", + type=float, + default=None, + help="The mass (in kg) to assign to the converted asset. If not provided, then no mass is added.", +) + +# 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 os + +from isaaclab.sim.converters import MeshConverter, MeshConverterCfg, UrdfConverter, UrdfConverterCfg +from isaaclab.sim.schemas import schemas_cfg + + +def main(): + # Define conversion time given + conversion_type = args_cli.conversion_type.lower() + # Warning if conversion type input is not valid + if conversion_type != "urdf" and conversion_type != "mesh" and conversion_type != "both": + raise Warning("Conversion type is not valid, please select either 'urdf', 'mesh', or 'both'.") + + if not os.path.exists(args_cli.input): + print(f"Error: The directory {args_cli.input} does not exist.") + + # For each file and subsequent sub-directory + for root, dirs, files in os.walk(args_cli.input): + # For each file + for filename in files: + # Check for URDF extensions + if (conversion_type == "urdf" or conversion_type == "both") and filename.lower().endswith(".urdf"): + # URDF converter call + urdf_converter_cfg = UrdfConverterCfg( + asset_path=f"{root}/{filename}", + usd_dir=f"{args_cli.output}/{filename[:-5]}", + usd_file_name=f"{filename[:-5]}.usd", + fix_base=args_cli.fix_base, + merge_fixed_joints=args_cli.merge_joints, + force_usd_conversion=True, + make_instanceable=args_cli.make_instanceable, + ) + # Create Urdf converter and import the file + urdf_converter = UrdfConverter(urdf_converter_cfg) + print(f"Generated USD file: {urdf_converter.usd_path}") + + elif (conversion_type == "mesh" or conversion_type == "both") and ( + filename.lower().endswith(".fbx") + or filename.lower().endswith(".obj") + or filename.lower().endswith(".dae") + or filename.lower().endswith(".stl") + ): + # Mass properties + if args_cli.mass is not None: + mass_props = schemas_cfg.MassPropertiesCfg(mass=args_cli.mass) + rigid_props = schemas_cfg.RigidBodyPropertiesCfg() + else: + mass_props = None + rigid_props = None + + # Collision properties + collision_props = schemas_cfg.CollisionPropertiesCfg( + collision_enabled=args_cli.collision_approximation != "none" + ) + # Mesh converter call + mesh_converter_cfg = MeshConverterCfg( + mass_props=mass_props, + rigid_props=rigid_props, + collision_props=collision_props, + asset_path=f"{root}/{filename}", + force_usd_conversion=True, + usd_dir=f"{args_cli.output}/{filename[:-4]}", + usd_file_name=f"{filename[:-4]}.usd", + make_instanceable=args_cli.make_instanceable, + collision_approximation=args_cli.collision_approximation, + ) + # Create mesh converter and import the file + mesh_converter = MeshConverter(mesh_converter_cfg) + print(f"Generated USD file: {mesh_converter.usd_path}") + + +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 9e8f37645ebc..6e9fd46befd3 100644 --- a/scripts/tools/convert_mesh.py +++ b/scripts/tools/convert_mesh.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -31,7 +31,8 @@ -h, --help Show this help message and exit --make-instanceable, Make the asset instanceable for efficient cloning. (default: False) --collision-approximation The method used for approximating collision mesh. Defaults to convexDecomposition. - Set to \"none\" to not add a collision mesh to the converted mesh. (default: convexDecomposition) + Set to \"none\" to not add a collision mesh to the converted mesh. + (default: convexDecomposition) --mass The mass (in kg) to assign to the converted asset. (default: None) """ @@ -43,6 +44,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 +70,8 @@ "--collision-approximation", type=str, default="convexDecomposition", - choices=["convexDecomposition", "convexHull", "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", @@ -84,14 +94,25 @@ import os import carb -import isaacsim.core.utils.stage as stage_utils import omni.kit.app +import isaaclab.sim as sim_utils from isaaclab.sim.converters import MeshConverter, MeshConverterCfg from isaaclab.sim.schemas import schemas_cfg 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 +139,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 +157,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 @@ -158,7 +188,7 @@ def main(): # Simulate scene (if not headless) if local_gui or livestream_gui: # Open the stage with USD - stage_utils.open_stage(mesh_converter.usd_path) + sim_utils.open_stage(mesh_converter.usd_path) # Reinitialize the simulation app = omni.kit.app.get_app_interface() # Run simulation diff --git a/scripts/tools/convert_mjcf.py b/scripts/tools/convert_mjcf.py index 6499d5b960a0..40e46b82d59d 100644 --- a/scripts/tools/convert_mjcf.py +++ b/scripts/tools/convert_mjcf.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -63,9 +63,9 @@ import os import carb -import isaacsim.core.utils.stage as stage_utils import omni.kit.app +import isaaclab.sim as sim_utils from isaaclab.sim.converters import MjcfConverter, MjcfConverterCfg from isaaclab.utils.assets import check_file_path from isaaclab.utils.dict import print_dict @@ -122,7 +122,7 @@ def main(): # Simulate scene (if not headless) if local_gui or livestream_gui: # Open the stage with USD - stage_utils.open_stage(mjcf_converter.usd_path) + sim_utils.open_stage(mjcf_converter.usd_path) # Reinitialize the simulation app = omni.kit.app.get_app_interface() # Run simulation diff --git a/scripts/tools/convert_urdf.py b/scripts/tools/convert_urdf.py index b94690c6624d..7d7a74708c59 100644 --- a/scripts/tools/convert_urdf.py +++ b/scripts/tools/convert_urdf.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -81,9 +81,9 @@ import os import carb -import isaacsim.core.utils.stage as stage_utils import omni.kit.app +import isaaclab.sim as sim_utils from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg from isaaclab.utils.assets import check_file_path from isaaclab.utils.dict import print_dict @@ -146,7 +146,7 @@ def main(): # Simulate scene (if not headless) if local_gui or livestream_gui: # Open the stage with USD - stage_utils.open_stage(urdf_converter.usd_path) + sim_utils.open_stage(urdf_converter.usd_path) # Reinitialize the simulation app = omni.kit.app.get_app_interface() # Run simulation diff --git a/scripts/tools/cosmos/cosmos_prompt_gen.py b/scripts/tools/cosmos/cosmos_prompt_gen.py new file mode 100644 index 000000000000..32db884adc56 --- /dev/null +++ b/scripts/tools/cosmos/cosmos_prompt_gen.py @@ -0,0 +1,85 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to construct prompts to control the Cosmos model's generation. + +Required arguments: + --templates_path Path to the file containing templates for the prompts. + +Optional arguments: + --num_prompts Number of prompts to generate (default: 1). + --output_path Path to the output file to write generated prompts (default: prompts.txt). +""" + +import argparse +import json +import random + + +def parse_args(): + """Parse command line arguments.""" + parser = argparse.ArgumentParser(description="Generate prompts for controlling Cosmos model's generation.") + parser.add_argument( + "--templates_path", type=str, required=True, help="Path to the JSON file containing prompt templates" + ) + parser.add_argument("--num_prompts", type=int, default=1, help="Number of prompts to generate (default: 1)") + parser.add_argument( + "--output_path", type=str, default="prompts.txt", help="Path to the output file to write generated prompts" + ) + args = parser.parse_args() + + return args + + +def generate_prompt(templates_path: str): + """Generate a random prompt for controlling the Cosmos model's visual augmentation. + + The prompt describes the scene and desired visual variations, which the model + uses to guide the augmentation process while preserving the core robotic actions. + + Args: + templates_path (str): Path to the JSON file containing prompt templates. + + Returns: + str: Generated prompt string that specifies visual aspects to modify in the video. + """ + try: + with open(templates_path) as f: + templates = json.load(f) + except FileNotFoundError: + raise FileNotFoundError(f"Prompt templates file not found: {templates_path}") + except json.JSONDecodeError: + raise ValueError(f"Invalid JSON in prompt templates file: {templates_path}") + + prompt_parts = [] + + for section_name, section_options in templates.items(): + if not isinstance(section_options, list): + continue + if len(section_options) == 0: + continue + selected_option = random.choice(section_options) + prompt_parts.append(selected_option) + + return " ".join(prompt_parts) + + +def main(): + # Parse command line arguments + args = parse_args() + + prompts = [generate_prompt(args.templates_path) for _ in range(args.num_prompts)] + + try: + with open(args.output_path, "w") as f: + for prompt in prompts: + f.write(prompt + "\n") + except Exception as e: + print(f"Failed to write to {args.output_path}: {e}") + + +if __name__ == "__main__": + main() diff --git a/scripts/tools/cosmos/transfer1_templates.json b/scripts/tools/cosmos/transfer1_templates.json new file mode 100644 index 000000000000..d2d4b063a268 --- /dev/null +++ b/scripts/tools/cosmos/transfer1_templates.json @@ -0,0 +1,96 @@ +{ + "env": [ + "A robotic arm is picking up and stacking cubes inside a foggy industrial scrapyard at dawn, surrounded by piles of old robotic parts and twisted metal. The background includes large magnetic cranes, rusted conveyor belts, and flickering yellow floodlights struggling to penetrate the fog.", + "A robotic arm is picking up and stacking cubes inside a luxury penthouse showroom during sunset. The background includes minimalist designer furniture, a panoramic view of a glowing city skyline, and hovering autonomous drones offering refreshments.", + "A robotic arm is picking up and stacking cubes within an ancient temple-themed robotics exhibit in a museum. The background includes stone columns with hieroglyphic-style etchings, interactive display panels, and a few museum visitors observing silently from behind glass barriers.", + "A robotic arm is picking up and stacking cubes inside a futuristic daycare facility for children. The background includes robotic toys, soft padded walls, holographic storybooks floating in mid-air, and tiny humanoid robots assisting toddlers.", + "A robotic arm is picking up and stacking cubes inside a deep underwater laboratory where pressure-resistant glass panels reveal a shimmering ocean outside. The background includes jellyfish drifting outside the windows, robotic submarines gliding by, and walls lined with wet-surface equipment panels.", + "A robotic arm is picking up and stacking cubes inside a post-apocalyptic lab, partially collapsed and exposed to the open sky. The background includes ruined machinery, exposed rebar, and a distant city skyline covered in ash and fog.", + "A robotic arm is picking up and stacking cubes in a biotech greenhouse surrounded by lush plant life. The background includes rows of bio-engineered plants, misting systems, and hovering inspection drones checking crop health.", + "A robotic arm is picking up and stacking cubes inside a dark, volcanic research outpost. The background includes robotic arms encased in heat-resistant suits, seismic monitors, and distant lava fountains occasionally illuminating the space.", + "A robotic arm is picking up and stacking cubes inside an icy arctic base, with frost-covered walls and equipment glinting under bright artificial white lights. The background includes heavy-duty heaters, control consoles wrapped in thermal insulation, and a large window looking out onto a frozen tundra with polar winds swirling snow outside.", + "A robotic arm is picking up and stacking cubes inside a zero-gravity chamber on a rotating space habitat. The background includes floating lab instruments, panoramic windows showing stars and Earth in rotation, and astronauts monitoring data.", + "A robotic arm is picking up and stacking cubes inside a mystical tech-art installation blending robotics with generative art. The background includes sculptural robotics, shifting light patterns on the walls, and visitors interacting with the exhibit using gestures.", + "A robotic arm is picking up and stacking cubes in a Martian colony dome, under a terraformed red sky filtering through thick glass. The background includes pressure-locked entry hatches, Martian rovers parked outside, and domed hydroponic farms stretching into the distance.", + "A robotic arm is picking up and stacking cubes inside a high-security military robotics testing bunker, with matte green steel walls and strict order. The background includes surveillance cameras, camouflage netting over equipment racks, and military personnel observing from a secure glass-walled control room.", + "A robotic arm is picking up and stacking cubes inside a retro-futuristic robotics lab from the 1980s with checkered floors and analog computer panels. The background includes CRT monitors with green code, rotary dials, printed schematics on the walls, and operators in lab coats typing on clunky terminals.", + "A robotic arm is picking up and stacking cubes inside a sunken ancient ruin repurposed for modern robotics experiments. The background includes carved pillars, vines creeping through gaps in stone, and scattered crates of modern equipment sitting on ancient floors.", + "A robotic arm is picking up and stacking cubes on a luxury interstellar yacht cruising through deep space. The background includes elegant furnishings, ambient synth music systems, and holographic butlers attending to other passengers.", + "A robotic arm is picking up and stacking cubes in a rebellious underground cybernetic hacker hideout. The background includes graffiti-covered walls, tangled wires, makeshift workbenches, and anonymous figures hunched over terminals with scrolling code.", + "A robotic arm is picking up and stacking cubes inside a dense jungle outpost where technology is being tested in extreme organic environments. The background includes humid control panels, vines creeping onto the robotics table, and occasional wildlife observed from a distance by researchers in camo gear.", + "A robotic arm is picking up and stacking cubes in a minimalist Zen tech temple. The background includes bonsai trees on floating platforms, robotic monks sweeping floors silently, and smooth stone pathways winding through digital meditation alcoves." + ], + + "robot": [ + "The robot arm is matte dark green with yellow diagonal hazard stripes along the upper arm; the joints are rugged and chipped, and the hydraulics are exposed with faded red tubing.", + "The robot arm is worn orange with black caution tape markings near the wrist; the elbow joint is dented and the pistons have visible scarring from long use.", + "The robot arm is steel gray with smooth curved panels and subtle blue stripes running down the length; the joints are sealed tight and the hydraulics have a glossy black casing.", + "The robot arm is bright yellow with alternating black bands around each segment; the joints show minor wear, and the hydraulics gleam with fresh lubrication.", + "The robot arm is navy blue with white serial numbers stenciled along the arm; the joints are well-maintained and the hydraulic shafts are matte silver with no visible dirt.", + "The robot arm is deep red with a matte finish and faint white grid lines across the panels; the joints are squared off and the hydraulic units look compact and embedded.", + "The robot arm is dirty white with dark gray speckled patches from wear; the joints are squeaky with exposed rivets, and the hydraulics are rusted at the base.", + "The robot arm is olive green with chipped paint and a black triangle warning icon near the shoulder; the joints are bulky and the hydraulics leak slightly around the seals.", + "The robot arm is bright teal with a glossy surface and silver stripes on the outer edges; the joints rotate smoothly and the pistons reflect a pale cyan hue.", + "The robot arm is orange-red with carbon fiber textures and white racing-style stripes down the forearm; the joints have minimal play and the hydraulics are tightly sealed in synthetic tubing.", + "The robot arm is flat black with uneven camouflage blotches in dark gray; the joints are reinforced and the hydraulic tubes are dusty and loose-fitting.", + "The robot arm is dull maroon with vertical black grooves etched into the panels; the joints show corrosion on the bolts and the pistons are thick and slow-moving.", + "The robot arm is powder blue with repeating geometric patterns printed in light gray; the joints are square and the hydraulic systems are internal and silent.", + "The robot arm is brushed silver with high-gloss finish and blue LED strips along the seams; the joints are shiny and tight, and the hydraulics hiss softly with every movement.", + "The robot arm is lime green with paint faded from sun exposure and white warning labels near each joint; the hydraulics are scraped and the fittings show heat marks.", + "The robot arm is dusty gray with chevron-style black stripes pointing toward the claw; the joints have uneven wear, and the pistons are dented and slightly bent.", + "The robot arm is cobalt blue with glossy texture and stylized angular black patterns across each segment; the joints are clean and the hydraulics show new flexible tubing.", + "The robot arm is industrial brown with visible welded seams and red caution tape wrapped loosely around the middle section; the joints are clunky and the hydraulics are slow and loud.", + "The robot arm is flat tan with dark green splotches and faint stencil text across the forearm; the joints have dried mud stains and the pistons are partially covered in grime.", + "The robot arm is light orange with chrome hexagon detailing and black number codes on the side; the joints are smooth and the hydraulic actuators shine under the lab lights." + ], + + "table": [ + "The robot arm is mounted on a table that is dull gray metal with scratches and scuff marks across the surface; faint rust rings are visible where older machinery used to be mounted.", + "The robot arm is mounted on a table that is smooth black plastic with a matte finish and faint fingerprint smudges near the edges; corners are slightly worn from regular use.", + "The robot arm is mounted on a table that is light oak wood with a natural grain pattern and a glossy varnish that reflects overhead lights softly; small burn marks dot one corner.", + "The robot arm is mounted on a table that is rough concrete with uneven texture and visible air bubbles; some grease stains and faded yellow paint markings suggest heavy usage.", + "The robot arm is mounted on a table that is brushed aluminum with a clean silver tone and very fine linear grooves; surface reflects light evenly, giving a soft glow.", + "The robot arm is mounted on a table that is pale green composite with chipped corners and scratches revealing darker material beneath; tape residue is stuck along the edges.", + "The robot arm is mounted on a table that is dark brown with a slightly cracked synthetic coating; patches of discoloration suggest exposure to heat or chemicals over time.", + "The robot arm is mounted on a table that is polished steel with mirror-like reflections; every small movement of the robot is mirrored faintly across the surface.", + "The robot arm is mounted on a table that is white with a slightly textured ceramic top, speckled with tiny black dots; the surface is clean but the edges are chipped.", + "The robot arm is mounted on a table that is glossy black glass with a deep shine and minimal dust; any lights above are clearly reflected, and fingerprints are visible under certain angles.", + "The robot arm is mounted on a table that is matte red plastic with wide surface scuffs and paint transfer from other objects; faint gridlines are etched into one side.", + "The robot arm is mounted on a table that is dark navy laminate with a low-sheen surface and subtle wood grain texture; the edge banding is slightly peeling off.", + "The robot arm is mounted on a table that is yellow-painted steel with diagonal black warning stripes running along one side; the paint is scratched and faded in high-contact areas.", + "The robot arm is mounted on a table that is translucent pale blue polymer with internal striations and slight glow under overhead lights; small bubbles are frozen inside the material.", + "The robot arm is mounted on a table that is cold concrete with embedded metal panels bolted into place; the surface has oil stains, welding marks, and tiny debris scattered around.", + "The robot arm is mounted on a table that is shiny chrome with heavy smudging and streaks; the table reflects distorted shapes of everything around it, including the arm itself.", + "The robot arm is mounted on a table that is matte forest green with shallow dents and drag marks from prior mechanical operations; a small sticker label is half-torn in one corner.", + "The robot arm is mounted on a table that is textured black rubber with slight give under pressure; scratches from the robot's base and clamp marks are clearly visible.", + "The robot arm is mounted on a table that is medium gray ceramic tile with visible grout lines and chips along the edges; some tiles have tiny cracks or stains.", + "The robot arm is mounted on a table that is old dark wood with faded polish and visible circular stains from spilled liquids; a few deep grooves are carved into the surface near the center." + ], + + "cubes": [ + "The arm is connected to the base mounted on the table. The bottom cube is deep blue, the second cube is bright red, and the top cube is vivid green, maintaining their correct order after stacking." + ], + + "light": [ + "The lighting is soft and diffused from large windows, allowing daylight to fill the room, creating gentle shadows that elongate throughout the space, with a natural warmth due to the sunlight streaming in.", + "Bright fluorescent tubes overhead cast a harsh, even light across the scene, creating sharp, well-defined shadows under the arm and cubes, with a sterile, clinical feel due to the cold white light.", + "Warm tungsten lights in the ceiling cast a golden glow over the table, creating long, soft shadows and a cozy, welcoming atmosphere. The light contrasts with cool blue tones from the robot arm.", + "The lighting comes from several intense spotlights mounted above, each casting focused beams of light that create stark, dramatic shadows around the cubes and the robotic arm, producing a high-contrast look.", + "A single adjustable desk lamp with a soft white bulb casts a directional pool of light over the cubes, causing deep, hard shadows and a quiet, intimate feel in the dimly lit room.", + "The space is illuminated with bright daylight filtering in through a skylight above, casting diffused, soft shadows and giving the scene a clean and natural look, with a cool tint from the daylight.", + "Soft, ambient lighting from hidden LEDs embedded in the ceiling creates a halo effect around the robotic arm, while subtle, elongated shadows stretch across the table surface, giving a sleek modern vibe.", + "Neon strip lights line the walls, casting a cool blue and purple glow across the scene. The robot and table are bathed in this colored light, producing sharp-edged shadows with a futuristic feel.", + "Bright artificial lights overhead illuminate the scene in a harsh white, with scattered, uneven shadows across the table and robot arm. There's a slight yellow hue to the light, giving it an industrial ambiance.", + "Soft morning sunlight spills through a large open window, casting long shadows across the floor and the robot arm. The warm, golden light creates a peaceful, natural atmosphere with a slight coolness in the shadows.", + "Dim ambient lighting with occasional flashes of bright blue light from overhead digital screens creates a high-tech, slightly eerie atmosphere. The shadows are soft, stretching in an almost surreal manner.", + "Lighting from tall lamps outside the room filters in through large glass doors, casting angled shadows across the table and robot arm. The ambient light creates a relaxing, slightly diffused atmosphere.", + "Artificial overhead lighting casts a harsh, stark white light with little warmth, producing sharply defined, almost clinical shadows on the robot arm and cubes. The space feels cold and industrial.", + "Soft moonlight from a large window at night creates a cool, ethereal glow on the table and arm. The shadows are long and faint, and the lighting provides a calm and serene atmosphere.", + "Bright overhead LED panels illuminate the scene with clean, white light, casting neutral shadows that give the environment a modern, sleek feel with minimal distortion or softness in the shadows.", + "A floodlight positioned outside casts bright, almost blinding natural light through an open door, creating high-contrast, sharp-edged shadows across the table and robot arm, adding dramatic tension to the scene.", + "Dim lighting from vintage tungsten bulbs hanging from the ceiling gives the room a warm, nostalgic glow, casting elongated, soft shadows that provide a cozy atmosphere around the robotic arm.", + "Bright fluorescent lights directly above produce a harsh, clinical light that creates sharp, defined shadows on the table and robotic arm, enhancing the industrial feel of the scene.", + "Neon pink and purple lights flicker softly from the walls, illuminating the robot arm with an intense glow that produces sharp, angular shadows across the cubes. The atmosphere feels futuristic and edgy.", + "Sunlight pouring in from a large, open window bathes the table and robotic arm in a warm golden light. The shadows are soft, and the scene feels natural and inviting with a slight contrast between light and shadow." + ] +} diff --git a/scripts/tools/hdf5_to_mp4.py b/scripts/tools/hdf5_to_mp4.py new file mode 100644 index 000000000000..0cd8a40c78f2 --- /dev/null +++ b/scripts/tools/hdf5_to_mp4.py @@ -0,0 +1,208 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to convert HDF5 demonstration files to MP4 videos. + +This script converts camera frames stored in HDF5 demonstration files to MP4 videos. +It supports multiple camera modalities including RGB, segmentation, and normal maps. +The output videos are saved in the specified directory with appropriate naming. + +required arguments: + --input_file Path to the input HDF5 file. + --output_dir Directory to save the output MP4 files. + +optional arguments: + --input_keys List of input keys to process from the HDF5 file. + (default: ["table_cam", "wrist_cam", "table_cam_segmentation", + "table_cam_normals", "table_cam_shaded_segmentation"]) + --video_height Height of the output video in pixels. (default: 704) + --video_width Width of the output video in pixels. (default: 1280) + --framerate Frames per second for the output video. (default: 30) +""" + +import argparse +import os + +import cv2 +import h5py +import numpy as np + +# Constants +DEFAULT_VIDEO_HEIGHT = 704 +DEFAULT_VIDEO_WIDTH = 1280 +DEFAULT_INPUT_KEYS = [ + "table_cam", + "wrist_cam", + "table_cam_segmentation", + "table_cam_normals", + "table_cam_shaded_segmentation", + "table_cam_depth", +] +DEFAULT_FRAMERATE = 30 +LIGHT_SOURCE = np.array([0.0, 0.0, 1.0]) +MIN_DEPTH = 0.0 +MAX_DEPTH = 1.5 + + +def parse_args(): + """Parse command line arguments.""" + parser = argparse.ArgumentParser(description="Convert HDF5 demonstration files to MP4 videos.") + parser.add_argument( + "--input_file", + type=str, + required=True, + help="Path to the input HDF5 file containing demonstration data.", + ) + parser.add_argument( + "--output_dir", + type=str, + required=True, + help="Directory path where the output MP4 files will be saved.", + ) + + parser.add_argument( + "--input_keys", + type=str, + nargs="+", + default=DEFAULT_INPUT_KEYS, + help="List of input keys to process.", + ) + parser.add_argument( + "--video_height", + type=int, + default=DEFAULT_VIDEO_HEIGHT, + help="Height of the output video in pixels.", + ) + parser.add_argument( + "--video_width", + type=int, + default=DEFAULT_VIDEO_WIDTH, + help="Width of the output video in pixels.", + ) + parser.add_argument( + "--framerate", + type=int, + default=DEFAULT_FRAMERATE, + help="Frames per second for the output video.", + ) + + args = parser.parse_args() + + return args + + +def write_demo_to_mp4( + hdf5_file, + demo_id, + frames_path, + input_key, + output_dir, + video_height, + video_width, + framerate=DEFAULT_FRAMERATE, +): + """Convert frames from an HDF5 file to an MP4 video. + + Args: + hdf5_file (str): Path to the HDF5 file containing the frames. + demo_id (int): ID of the demonstration to convert. + frames_path (str): Path to the frames data in the HDF5 file. + input_key (str): Name of the input key to convert. + output_dir (str): Directory to save the output MP4 file. + video_height (int): Height of the output video in pixels. + video_width (int): Width of the output video in pixels. + framerate (int, optional): Frames per second for the output video. Defaults to 30. + """ + with h5py.File(hdf5_file, "r") as f: + # Get frames based on input key type + if "shaded_segmentation" in input_key: + temp_key = input_key.replace("shaded_segmentation", "segmentation") + frames = f[f"data/demo_{demo_id}/obs/{temp_key}"] + else: + frames = f[frames_path + "/" + input_key] + + # Setup video writer + output_path = os.path.join(output_dir, f"demo_{demo_id}_{input_key}.mp4") + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + if "depth" in input_key: + video = cv2.VideoWriter(output_path, fourcc, framerate, (video_width, video_height), isColor=False) + else: + video = cv2.VideoWriter(output_path, fourcc, framerate, (video_width, video_height)) + + # Process and write frames + for ix, frame in enumerate(frames): + # Convert normal maps to uint8 if needed + if "normals" in input_key: + frame = (frame * 255.0).astype(np.uint8) + + # Process shaded segmentation frames + elif "shaded_segmentation" in input_key: + seg = frame[..., :-1] + normals_key = input_key.replace("shaded_segmentation", "normals") + normals = f[f"data/demo_{demo_id}/obs/{normals_key}"][ix] + shade = 0.5 + (normals * LIGHT_SOURCE[None, None, :]).sum(axis=-1) * 0.5 + shaded_seg = (shade[..., None] * seg).astype(np.uint8) + frame = np.concatenate((shaded_seg, frame[..., -1:]), axis=-1) + + # Convert RGB to BGR + if "depth" not in input_key: + frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) + else: + frame = (frame[..., 0] - MIN_DEPTH) / (MAX_DEPTH - MIN_DEPTH) + frame = np.where(frame < 0.01, 1.0, frame) + frame = 1.0 - frame + frame = (frame * 255.0).astype(np.uint8) + + # Resize to video resolution + frame = cv2.resize(frame, (video_width, video_height), interpolation=cv2.INTER_CUBIC) + video.write(frame) + + video.release() + + +def get_num_demos(hdf5_file): + """Get the number of demonstrations in the HDF5 file. + + Args: + hdf5_file (str): Path to the HDF5 file. + + Returns: + int: Number of demonstrations found in the file. + """ + with h5py.File(hdf5_file, "r") as f: + return len(f["data"].keys()) + + +def main(): + """Main function to convert all demonstrations to MP4 videos.""" + # Parse command line arguments + args = parse_args() + + # Create output directory if it doesn't exist + os.makedirs(args.output_dir, exist_ok=True) + + # Get number of demonstrations from the file + num_demos = get_num_demos(args.input_file) + print(f"Found {num_demos} demonstrations in {args.input_file}") + + # Convert each demonstration + for i in range(num_demos): + frames_path = f"data/demo_{str(i)}/obs" + for input_key in args.input_keys: + write_demo_to_mp4( + args.input_file, + i, + frames_path, + input_key, + args.output_dir, + args.video_height, + args.video_width, + args.framerate, + ) + + +if __name__ == "__main__": + main() diff --git a/scripts/tools/merge_hdf5_datasets.py b/scripts/tools/merge_hdf5_datasets.py index a5e19363dfab..a9fe1c63561d 100644 --- a/scripts/tools/merge_hdf5_datasets.py +++ b/scripts/tools/merge_hdf5_datasets.py @@ -1,12 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import argparse -import h5py import os +import h5py + parser = argparse.ArgumentParser(description="Merge a set of HDF5 datasets.") parser.add_argument( "--input_files", @@ -30,7 +31,6 @@ def merge_datasets(): copy_attributes = True for filepath in args_cli.input_files: - with h5py.File(filepath, "r") as input: for episode, data in input["data"].items(): input.copy(f"data/{episode}", output, f"data/demo_{episode_idx}") diff --git a/scripts/tools/mp4_to_hdf5.py b/scripts/tools/mp4_to_hdf5.py new file mode 100644 index 000000000000..61f7b5b0b40b --- /dev/null +++ b/scripts/tools/mp4_to_hdf5.py @@ -0,0 +1,169 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to create a new dataset by combining existing HDF5 demonstrations with visually augmented MP4 videos. + +This script takes an existing HDF5 dataset containing demonstrations and a directory of MP4 videos +that are visually augmented versions of the original demonstration videos (e.g., with different lighting, +color schemes, or visual effects). It creates a new HDF5 dataset that preserves all the original +demonstration data (actions, robot state, etc.) but replaces the video frames with the augmented versions. + +required arguments: + --input_file Path to the input HDF5 file containing original demonstrations. + --output_file Path to save the new HDF5 file with augmented videos. + --videos_dir Directory containing the visually augmented MP4 videos. +""" + +import argparse +import glob +import os + +import cv2 +import h5py +import numpy as np + + +def parse_args(): + """Parse command line arguments.""" + parser = argparse.ArgumentParser(description="Create a new dataset with visually augmented videos.") + parser.add_argument( + "--input_file", + type=str, + required=True, + help="Path to the input HDF5 file containing original demonstrations.", + ) + parser.add_argument( + "--videos_dir", + type=str, + required=True, + help="Directory containing the visually augmented MP4 videos.", + ) + parser.add_argument( + "--output_file", + type=str, + required=True, + help="Path to save the new HDF5 file with augmented videos.", + ) + + args = parser.parse_args() + + return args + + +def get_frames_from_mp4(video_path, target_height=None, target_width=None): + """Extract frames from an MP4 video file. + + Args: + video_path (str): Path to the MP4 video file. + target_height (int, optional): Target height for resizing frames. If None, no resizing is done. + target_width (int, optional): Target width for resizing frames. If None, no resizing is done. + + Returns: + np.ndarray: Array of frames from the video in RGB format. + """ + # Open the video file + video = cv2.VideoCapture(video_path) + + # Get video properties + frame_count = int(video.get(cv2.CAP_PROP_FRAME_COUNT)) + + # Read all frames into a numpy array + frames = [] + for _ in range(frame_count): + ret, frame = video.read() + if not ret: + break + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + if target_height is not None and target_width is not None: + frame = cv2.resize(frame, (target_width, target_height), interpolation=cv2.INTER_LINEAR) + frames.append(frame) + + # Convert to numpy array + frames = np.array(frames).astype(np.uint8) + + # Release the video object + video.release() + + return frames + + +def process_video_and_demo(f_in, f_out, video_path, orig_demo_id, new_demo_id): + """Process a single video and create a new demo with augmented video frames. + + Args: + f_in (h5py.File): Input HDF5 file. + f_out (h5py.File): Output HDF5 file. + video_path (str): Path to the augmented video file. + orig_demo_id (int): ID of the original demo to copy. + new_demo_id (int): ID for the new demo. + """ + # Get original demo data + actions = f_in[f"data/demo_{str(orig_demo_id)}/actions"] + eef_pos = f_in[f"data/demo_{str(orig_demo_id)}/obs/eef_pos"] + eef_quat = f_in[f"data/demo_{str(orig_demo_id)}/obs/eef_quat"] + gripper_pos = f_in[f"data/demo_{str(orig_demo_id)}/obs/gripper_pos"] + wrist_cam = f_in[f"data/demo_{str(orig_demo_id)}/obs/wrist_cam"] + + # Get original video resolution + orig_video = f_in[f"data/demo_{str(orig_demo_id)}/obs/table_cam"] + target_height, target_width = orig_video.shape[1:3] + + # Extract frames from video with original resolution + frames = get_frames_from_mp4(video_path, target_height, target_width) + + # Create new datasets + f_out.create_dataset(f"data/demo_{str(new_demo_id)}/actions", data=actions, compression="gzip") + f_out.create_dataset(f"data/demo_{str(new_demo_id)}/obs/eef_pos", data=eef_pos, compression="gzip") + f_out.create_dataset(f"data/demo_{str(new_demo_id)}/obs/eef_quat", data=eef_quat, compression="gzip") + f_out.create_dataset(f"data/demo_{str(new_demo_id)}/obs/gripper_pos", data=gripper_pos, compression="gzip") + f_out.create_dataset( + f"data/demo_{str(new_demo_id)}/obs/table_cam", data=frames.astype(np.uint8), compression="gzip" + ) + f_out.create_dataset(f"data/demo_{str(new_demo_id)}/obs/wrist_cam", data=wrist_cam, compression="gzip") + + # Copy attributes + f_out[f"data/demo_{str(new_demo_id)}"].attrs["num_samples"] = f_in[f"data/demo_{str(orig_demo_id)}"].attrs[ + "num_samples" + ] + + +def main(): + """Main function to create a new dataset with augmented videos.""" + # Parse command line arguments + args = parse_args() + + # Get list of MP4 videos + search_path = os.path.join(args.videos_dir, "*.mp4") + video_paths = glob.glob(search_path) + video_paths.sort() + print(f"Found {len(video_paths)} MP4 videos in {args.videos_dir}") + + # Create output directory if it doesn't exist + os.makedirs(os.path.dirname(args.output_file), exist_ok=True) + + with h5py.File(args.input_file, "r") as f_in, h5py.File(args.output_file, "w") as f_out: + # Copy all data from input to output + f_in.copy("data", f_out) + + # Get the largest demo ID to start new demos from + demo_ids = [int(key.split("_")[1]) for key in f_in["data"].keys()] + next_demo_id = max(demo_ids) + 1 # noqa: SIM113 + print(f"Starting new demos from ID: {next_demo_id}") + + # Process each video and create new demo + for video_path in video_paths: + # Extract original demo ID from video filename + video_filename = os.path.basename(video_path) + orig_demo_id = int(video_filename.split("_")[1]) + + process_video_and_demo(f_in, f_out, video_path, orig_demo_id, next_demo_id) + next_demo_id += 1 + + print(f"Augmented data saved to {args.output_file}") + + +if __name__ == "__main__": + main() diff --git a/scripts/tools/process_meshes_to_obj.py b/scripts/tools/process_meshes_to_obj.py index 4e50ac4eb228..2c5be04c0e5c 100644 --- a/scripts/tools/process_meshes_to_obj.py +++ b/scripts/tools/process_meshes_to_obj.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/scripts/tools/record_demos.py b/scripts/tools/record_demos.py index 3e7587da7f86..6bb8dea57073 100644 --- a/scripts/tools/record_demos.py +++ b/scripts/tools/record_demos.py @@ -1,8 +1,7 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """ Script to record demonstrations with Isaac Lab environments using human teleoperation. @@ -19,20 +18,32 @@ --dataset_file File path to export recorded demos. (default: "./datasets/dataset.hdf5") --step_hz Environment stepping rate in Hz. (default: 30) --num_demos Number of demonstrations to record. (default: 0) - --num_success_steps Number of continuous steps with task success for concluding a demo as successful. (default: 10) + --num_success_steps Number of continuous steps with task success for concluding a demo as successful. + (default: 10) """ """Launch Isaac Sim Simulator first.""" +# Standard library imports import argparse -import os +import contextlib +# Isaac Lab AppLauncher from isaaclab.app import AppLauncher # add argparse arguments parser = argparse.ArgumentParser(description="Record demonstrations for Isaac Lab environments.") -parser.add_argument("--task", type=str, default=None, help="Name of the task.") -parser.add_argument("--teleop_device", type=str, default="keyboard", help="Device for interacting with environment.") +parser.add_argument("--task", type=str, required=True, help="Name of the task.") +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." ) @@ -46,13 +57,31 @@ default=10, help="Number of continuous steps with task success for concluding a demo as successful. Default is 10.", ) +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) + # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # parse the arguments args_cli = parser.parse_args() -if args_cli.teleop_device.lower() == "handtracking": - vars(args_cli)["experience"] = f'{os.environ["ISAACLAB_PATH"]}/apps/isaaclab.python.xr.openxr.kit' +# Validate required arguments +if args_cli.task is None: + parser.error("--task is required") + +app_launcher_args = vars(args_cli) + +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 +if "handtracking" in args_cli.teleop_device.lower(): + app_launcher_args["xr"] = True # launch the simulator app_launcher = AppLauncher(args_cli) @@ -60,37 +89,62 @@ """Rest everything follows.""" -import contextlib -import gymnasium as gym + +# Third-party imports +import logging +import os import time + +import gymnasium as gym import torch -import omni.log +import omni.ui as ui + +from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices.openxr import remove_camera_configs +from isaaclab.devices.teleop_device_factory import create_teleop_device + +import isaaclab_mimic.envs # noqa: F401 +from isaaclab_mimic.ui.instruction_display import InstructionDisplay, show_subtask_instructions + +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 + +from collections.abc import Callable -from isaaclab.devices import Se3HandTracking, Se3Keyboard, Se3SpaceMouse -from isaaclab.envs import ViewerCfg +from isaaclab.envs import DirectRLEnvCfg, ManagerBasedRLEnvCfg from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg -from isaaclab.envs.ui import ViewportCameraController +from isaaclab.envs.ui import EmptyWindow +from isaaclab.managers import DatasetExportMode import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg +# import logger +logger = logging.getLogger(__name__) + class RateLimiter: """Convenience class for enforcing rates in loops.""" - def __init__(self, hz): - """ + def __init__(self, hz: int): + """Initialize a RateLimiter with specified frequency. + Args: - hz (int): frequency to enforce + hz: Frequency to enforce in Hertz. """ self.hz = hz self.last_time = time.time() self.sleep_duration = 1.0 / hz self.render_period = min(0.033, self.sleep_duration) - def sleep(self, env): - """Attempt to sleep at the specified rate in hz.""" + def sleep(self, env: gym.Env): + """Attempt to sleep at the specified rate in hz. + + Args: + env: Environment to render during sleep periods. + """ next_wakeup_time = self.last_time + self.sleep_duration while time.time() < next_wakeup_time: time.sleep(self.render_period) @@ -104,30 +158,17 @@ def sleep(self, env): self.last_time += self.sleep_duration -def pre_process_actions(delta_pose: torch.Tensor, gripper_command: bool) -> torch.Tensor: - """Pre-process actions for the environment.""" - # compute actions based on environment - if "Reach" in args_cli.task: - # note: reach is the only one that uses a different action space - # compute actions - return delta_pose - else: - # resolve gripper command - gripper_vel = torch.zeros((delta_pose.shape[0], 1), dtype=torch.float, device=delta_pose.device) - gripper_vel[:] = -1 if gripper_command else 1 - # compute actions - return torch.concat([delta_pose, gripper_vel], dim=1) - +def setup_output_directories() -> tuple[str, str]: + """Set up output directories for saving demonstrations. -def main(): - """Collect demonstrations from the environment using teleop interfaces.""" - - # if handtracking is selected, rate limiting is achieved via OpenXR - if args_cli.teleop_device.lower() == "handtracking": - rate_limiter = None - else: - rate_limiter = RateLimiter(args_cli.step_hz) + Creates the output directory if it doesn't exist and extracts the file name + from the dataset file path. + Returns: + tuple[str, str]: A tuple containing: + - output_dir: The directory path where the dataset will be saved + - output_file_name: The filename (without extension) for the dataset + """ # get directory path and file name (without extension) from cli arguments output_dir = os.path.dirname(args_cli.dataset_file) output_file_name = os.path.splitext(os.path.basename(args_cli.dataset_file))[0] @@ -135,10 +176,38 @@ def main(): # create directory if it does not exist if not os.path.exists(output_dir): os.makedirs(output_dir) + print(f"Created output directory: {output_dir}") + + return output_dir, output_file_name + + +def create_environment_config( + output_dir: str, output_file_name: str +) -> tuple[ManagerBasedRLEnvCfg | DirectRLEnvCfg, object | None]: + """Create and configure the environment configuration. + + Parses the environment configuration and makes necessary adjustments for demo recording. + Extracts the success termination function and configures the recorder manager. + + Args: + output_dir: Directory where recorded demonstrations will be saved + output_file_name: Name of the file to store the demonstrations + + Returns: + tuple[isaaclab_tasks.utils.parse_cfg.EnvCfg, Optional[object]]: A tuple containing: + - env_cfg: The configured environment configuration + - success_term: The success termination object or None if not available + Raises: + Exception: If parsing the environment configuration fails + """ # parse configuration - env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1) - env_cfg.env_name = args_cli.task + try: + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1) + env_cfg.env_name = args_cli.task.split(":")[-1] + except Exception as e: + logger.error(f"Failed to parse environment configuration: {e}") + exit(1) # extract success checking function to invoke in the main loop success_term = None @@ -146,106 +215,346 @@ def main(): success_term = env_cfg.terminations.success env_cfg.terminations.success = None else: - omni.log.warn( + logger.warning( "No success termination term was found in the environment." " Will not be able to mark recorded demos as successful." ) + if args_cli.xr: + # If cameras are not enabled and XR is enabled, remove camera configs + if not args_cli.enable_cameras: + env_cfg = remove_camera_configs(env_cfg) + env_cfg.sim.render.antialiasing_mode = "DLSS" + # modify configuration such that the environment runs indefinitely until # the goal is reached or other termination conditions are met env_cfg.terminations.time_out = None - env_cfg.observations.policy.concatenate_terms = False env_cfg.recorders: ActionStateRecorderManagerCfg = ActionStateRecorderManagerCfg() env_cfg.recorders.dataset_export_dir_path = output_dir env_cfg.recorders.dataset_filename = output_file_name + env_cfg.recorders.dataset_export_mode = DatasetExportMode.EXPORT_SUCCEEDED_ONLY + + return env_cfg, success_term + + +def create_environment(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg) -> gym.Env: + """Create the environment from the configuration. + + Args: + env_cfg: The environment configuration object that defines the environment properties. + This should be an instance of EnvCfg created by parse_env_cfg(). + + Returns: + gym.Env: A Gymnasium environment instance for the specified task. + + Raises: + Exception: If environment creation fails for any reason. + """ + try: + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + return env + except Exception as e: + logger.error(f"Failed to create environment: {e}") + exit(1) + + +def setup_teleop_device(callbacks: dict[str, Callable]) -> object: + """Set up the teleoperation device based on configuration. + + Attempts to create a teleoperation device based on the environment configuration. + Falls back to default devices if the specified device is not found in the configuration. + + Args: + callbacks: Dictionary mapping callback keys to functions that will be + attached to the teleop device + + Returns: + object: The configured teleoperation device interface + + Raises: + Exception: If teleop device creation fails + """ + teleop_interface = None + try: + if hasattr(env_cfg, "teleop_devices") and args_cli.teleop_device in env_cfg.teleop_devices.devices: + teleop_interface = create_teleop_device(args_cli.teleop_device, env_cfg.teleop_devices.devices, callbacks) + else: + logger.warning( + f"No teleop device '{args_cli.teleop_device}' found in environment config. Creating default." + ) + # Create fallback teleop device + if args_cli.teleop_device.lower() == "keyboard": + teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) + elif args_cli.teleop_device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse(Se3SpaceMouseCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) + else: + logger.error(f"Unsupported teleop device: {args_cli.teleop_device}") + logger.error("Supported devices: keyboard, spacemouse, handtracking") + exit(1) + + # Add callbacks to fallback device + for key, callback in callbacks.items(): + teleop_interface.add_callback(key, callback) + except Exception as e: + logger.error(f"Failed to create teleop device: {e}") + exit(1) + + if teleop_interface is None: + logger.error("Failed to create teleop interface") + exit(1) + + return teleop_interface + + +def setup_ui(label_text: str, env: gym.Env) -> InstructionDisplay: + """Set up the user interface elements. + + Creates instruction display and UI window with labels for showing information + to the user during demonstration recording. + + Args: + label_text: Text to display showing current recording status + env: The environment instance for which UI is being created + + Returns: + InstructionDisplay: The configured instruction display object + """ + instruction_display = InstructionDisplay(args_cli.xr) + if not args_cli.xr: + window = EmptyWindow(env, "Instruction") + with window.ui_window_elements["main_vstack"]: + demo_label = ui.Label(label_text) + subtask_label = ui.Label("") + instruction_display.set_labels(subtask_label, demo_label) + + return instruction_display + + +def process_success_condition(env: gym.Env, success_term: object | None, success_step_count: int) -> tuple[int, bool]: + """Process the success condition for the current step. + + Checks if the environment has met the success condition for the required + number of consecutive steps. Marks the episode as successful if criteria are met. + + Args: + env: The environment instance to check + success_term: The success termination object or None if not available + success_step_count: Current count of consecutive successful steps + + Returns: + tuple[int, bool]: A tuple containing: + - updated success_step_count: The updated count of consecutive successful steps + - success_reset_needed: Boolean indicating if reset is needed due to success + """ + if success_term is None: + return success_step_count, False + + if bool(success_term.func(env, **success_term.params)[0]): + success_step_count += 1 + if success_step_count >= args_cli.num_success_steps: + env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) + env.recorder_manager.set_success_to_episodes( + [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) + ) + env.recorder_manager.export_episodes([0]) + print("Success condition met! Recording completed.") + return success_step_count, True + else: + success_step_count = 0 + + return success_step_count, False + - # create environment - env = gym.make(args_cli.task, cfg=env_cfg).unwrapped +def handle_reset( + env: gym.Env, success_step_count: int, instruction_display: InstructionDisplay, label_text: str +) -> int: + """Handle resetting the environment. - # add teleoperation key for reset current recording instance + Resets the environment, recorder manager, and related state variables. + Updates the instruction display with current status. + + Args: + env: The environment instance to reset + success_step_count: Current count of consecutive successful steps + instruction_display: The display object to update + label_text: Text to display showing current recording status + + Returns: + int: Reset success step count (0) + """ + print("Resetting environment...") + env.sim.reset() + env.recorder_manager.reset() + env.reset() + success_step_count = 0 + instruction_display.show_demo(label_text) + return success_step_count + + +def run_simulation_loop( + env: gym.Env, + teleop_interface: object | None, + success_term: object | None, + rate_limiter: RateLimiter | None, +) -> int: + """Run the main simulation loop for collecting demonstrations. + + Sets up callback functions for teleop device, initializes the UI, + and runs the main loop that processes user inputs and environment steps. + Records demonstrations when success conditions are met. + + Args: + env: The environment instance + teleop_interface: Optional teleop interface (will be created if None) + success_term: The success termination object or None if not available + rate_limiter: Optional rate limiter to control simulation speed + + Returns: + int: Number of successful demonstrations recorded + """ + current_recorded_demo_count = 0 + success_step_count = 0 should_reset_recording_instance = False + running_recording_instance = not args_cli.xr + # Callback closures for the teleop device def reset_recording_instance(): nonlocal should_reset_recording_instance should_reset_recording_instance = True - - # create controller - if args_cli.teleop_device.lower() == "keyboard": - teleop_interface = Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5) - elif args_cli.teleop_device.lower() == "spacemouse": - teleop_interface = Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5) - elif args_cli.teleop_device.lower() == "handtracking": - from isaacsim.xr.openxr import OpenXRSpec - - teleop_interface = Se3HandTracking(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True) - teleop_interface.add_callback("RESET", reset_recording_instance) - viewer = ViewerCfg(eye=(-0.25, -0.3, 0.5), lookat=(0.6, 0, 0), asset_name="viewer") - ViewportCameraController(env, viewer) - else: - raise ValueError( - f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'handtracking'." - ) - + print("Recording instance reset requested") + + def start_recording_instance(): + nonlocal running_recording_instance + running_recording_instance = True + print("Recording started") + + def stop_recording_instance(): + nonlocal running_recording_instance + running_recording_instance = False + print("Recording paused") + + # Set up teleoperation callbacks + teleoperation_callbacks = { + "R": reset_recording_instance, + "START": start_recording_instance, + "STOP": stop_recording_instance, + "RESET": reset_recording_instance, + } + + teleop_interface = setup_teleop_device(teleoperation_callbacks) teleop_interface.add_callback("R", reset_recording_instance) - print(teleop_interface) - # reset before starting + # Reset before starting + env.sim.reset() env.reset() teleop_interface.reset() - # simulate environment -- run everything in inference mode - current_recorded_demo_count = 0 - success_step_count = 0 - with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): - while True: - # get keyboard command - delta_pose, gripper_command = teleop_interface.advance() - # convert to torch - delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=env.device).repeat(env.num_envs, 1) - # compute actions based on environment - actions = pre_process_actions(delta_pose, gripper_command) - - # perform action on environment - env.step(actions) - - if success_term is not None: - if bool(success_term.func(env, **success_term.params)[0]): - success_step_count += 1 - if success_step_count >= args_cli.num_success_steps: - env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) - env.recorder_manager.set_success_to_episodes( - [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) - ) - env.recorder_manager.export_episodes([0]) - should_reset_recording_instance = True - else: - success_step_count = 0 + label_text = f"Recorded {current_recorded_demo_count} successful demonstrations." + instruction_display = setup_ui(label_text, env) - if should_reset_recording_instance: - env.recorder_manager.reset() - env.reset() - should_reset_recording_instance = False - success_step_count = 0 + subtasks = {} - # print out the current demo count if it has changed + with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): + while simulation_app.is_running(): + # Get keyboard command + action = teleop_interface.advance() + # Expand to batch dimension + actions = action.repeat(env.num_envs, 1) + + # Perform action on environment + if running_recording_instance: + # Compute actions based on environment + obv = env.step(actions) + if subtasks is not None: + if subtasks == {}: + subtasks = obv[0].get("subtask_terms") + elif subtasks: + show_subtask_instructions(instruction_display, subtasks, obv, env.cfg) + else: + env.sim.render() + + # Check for success condition + success_step_count, success_reset_needed = process_success_condition(env, success_term, success_step_count) + if success_reset_needed: + should_reset_recording_instance = True + + # Update demo count if it has changed if env.recorder_manager.exported_successful_episode_count > current_recorded_demo_count: current_recorded_demo_count = env.recorder_manager.exported_successful_episode_count - print(f"Recorded {current_recorded_demo_count} successful demonstrations.") + label_text = f"Recorded {current_recorded_demo_count} successful demonstrations." + print(label_text) + # Check if we've reached the desired number of demos if args_cli.num_demos > 0 and env.recorder_manager.exported_successful_episode_count >= args_cli.num_demos: - print(f"All {args_cli.num_demos} demonstrations recorded. Exiting the app.") + label_text = f"All {current_recorded_demo_count} demonstrations recorded.\nExiting the app." + instruction_display.show_demo(label_text) + print(label_text) + target_time = time.time() + 0.8 + while time.time() < target_time: + if rate_limiter: + rate_limiter.sleep(env) + else: + env.sim.render() break - # check that simulation is stopped or not + # Handle reset if requested + if should_reset_recording_instance: + success_step_count = handle_reset(env, success_step_count, instruction_display, label_text) + should_reset_recording_instance = False + + # Check if simulation is stopped if env.sim.is_stopped(): break + # Rate limiting if rate_limiter: rate_limiter.sleep(env) + return current_recorded_demo_count + + +def main() -> None: + """Collect demonstrations from the environment using teleop interfaces. + + Main function that orchestrates the entire process: + 1. Sets up rate limiting based on configuration + 2. Creates output directories for saving demonstrations + 3. Configures the environment + 4. Runs the simulation loop to collect demonstrations + 5. Cleans up resources when done + + Raises: + Exception: Propagates exceptions from any of the called functions + """ + # if handtracking is selected, rate limiting is achieved via OpenXR + if args_cli.xr: + rate_limiter = None + from isaaclab.ui.xr_widgets import TeleopVisualizationManager, XRVisualization + + # Assign the teleop visualization manager to the visualization system + XRVisualization.assign_manager(TeleopVisualizationManager) + else: + rate_limiter = RateLimiter(args_cli.step_hz) + + # Set up output directories + output_dir, output_file_name = setup_output_directories() + + # Create and configure environment + global env_cfg # Make env_cfg available to setup_teleop_device + env_cfg, success_term = create_environment_config(output_dir, output_file_name) + + # Create environment + env = create_environment(env_cfg) + + # Run simulation loop + current_recorded_demo_count = run_simulation_loop(env, None, success_term, rate_limiter) + + # Clean up env.close() + print(f"Recording session completed with {current_recorded_demo_count} successful demonstrations") + print(f"Demonstrations saved to: {args_cli.dataset_file}") if __name__ == "__main__": diff --git a/scripts/tools/replay_demos.py b/scripts/tools/replay_demos.py index 74eeaf67df74..7d5e477267bf 100644 --- a/scripts/tools/replay_demos.py +++ b/scripts/tools/replay_demos.py @@ -1,12 +1,12 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """Script to replay demonstrations with Isaac Lab environments.""" """Launch Isaac Sim Simulator first.""" + import argparse from isaaclab.app import AppLauncher @@ -32,6 +32,18 @@ " --num_envs is 1." ), ) +parser.add_argument( + "--validate_success_rate", + action="store_true", + default=False, + help="Validate the replay success rate using the task environment termination criteria", +) +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) @@ -39,6 +51,12 @@ args_cli = parser.parse_args() # args_cli.headless = True +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 + # launch the simulator app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app @@ -46,13 +64,18 @@ """Rest everything follows.""" import contextlib -import gymnasium as gym import os + +import gymnasium as gym import torch -from isaaclab.devices import Se3Keyboard +from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler +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 + import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg @@ -120,7 +143,7 @@ def main(): episode_indices_to_replay = list(range(episode_count)) if args_cli.task is not None: - env_name = args_cli.task + 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.") @@ -128,14 +151,26 @@ def main(): env_cfg = parse_env_cfg(env_name, device=args_cli.device, num_envs=num_envs) + # extract success checking function to invoke in the main loop + success_term = None + if args_cli.validate_success_rate: + if hasattr(env_cfg.terminations, "success"): + success_term = env_cfg.terminations.success + env_cfg.terminations.success = None + else: + print( + "No success termination term was found in the environment." + " Will not be able to mark recorded demos as successful." + ) + # Disable all recorders and terminations env_cfg.recorders = {} env_cfg.terminations = {} # create environment from loaded config - env = gym.make(env_name, cfg=env_cfg).unwrapped + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped - teleop_interface = Se3Keyboard(pos_sensitivity=0.1, rot_sensitivity=0.1) + teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.1, rot_sensitivity=0.1)) teleop_interface.add_callback("N", play_cb) teleop_interface.add_callback("B", pause_cb) print('Press "B" to pause and "N" to resume the replayed actions.') @@ -147,6 +182,12 @@ def main(): elif args_cli.validate_states and num_envs > 1: print("Warning: State validation is only supported with a single environment. Skipping state validation.") + # Get idle action (idle actions are applied to envs without next action) + if hasattr(env_cfg, "idle_action"): + idle_action = env_cfg.idle_action.repeat(num_envs, 1) + else: + idle_action = torch.zeros(env.action_space.shape) + # reset before starting env.reset() teleop_interface.reset() @@ -154,28 +195,64 @@ def main(): # simulate environment -- run everything in inference mode episode_names = list(dataset_file_handler.get_episode_names()) replayed_episode_count = 0 + recorded_episode_count = 0 + + # Track current episode indices for each environment + current_episode_indices = [None] * num_envs + + # Track failed demo IDs + failed_demo_ids = [] + with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): while simulation_app.is_running() and not simulation_app.is_exiting(): env_episode_data_map = {index: EpisodeData() for index in range(num_envs)} first_loop = True has_next_action = True + episode_ended = [False] * num_envs while has_next_action: - # initialize actions with zeros so those without next action will not move - actions = torch.zeros(env.action_space.shape) + # initialize actions with idle action so those without next action will not move + actions = idle_action has_next_action = False for env_id in range(num_envs): env_next_action = env_episode_data_map[env_id].get_next_action() if env_next_action is None: + # check if the episode is successful after the whole episode_data is + if ( + (success_term is not None) + and (current_episode_indices[env_id]) is not None + and (not episode_ended[env_id]) + ): + if bool(success_term.func(env, **success_term.params)[env_id]): + recorded_episode_count += 1 + plural_trailing_s = "s" if recorded_episode_count > 1 else "" + + print( + f"Successfully replayed {recorded_episode_count} episode{plural_trailing_s} out" + f" of {replayed_episode_count} demos." + ) + else: + # if not successful, add to failed demo IDs list + if ( + current_episode_indices[env_id] is not None + and current_episode_indices[env_id] not in failed_demo_ids + ): + failed_demo_ids.append(current_episode_indices[env_id]) + + episode_ended[env_id] = True + next_episode_index = None while episode_indices_to_replay: next_episode_index = episode_indices_to_replay.pop(0) + if next_episode_index < episode_count: + episode_ended[env_id] = False break next_episode_index = None if next_episode_index is not None: replayed_episode_count += 1 - print(f"{replayed_episode_count :4}: Loading #{next_episode_index} episode to env_{env_id}") + current_episode_indices[env_id] = next_episode_index + print(f"{replayed_episode_count:4}: Loading #{next_episode_index} episode to env_{env_id}") episode_data = dataset_file_handler.load_episode( episode_names[next_episode_index], env.device ) @@ -203,7 +280,7 @@ def main(): state_from_dataset = env_episode_data_map[0].get_next_state() if state_from_dataset is not None: print( - f"Validating states at action-index: {env_episode_data_map[0].next_state_index - 1 :4}", + f"Validating states at action-index: {env_episode_data_map[0].next_state_index - 1:4}", end="", ) current_runtime_state = env.scene.get_state(is_relative=True) @@ -217,6 +294,16 @@ def main(): # Close environment after replay in complete plural_trailing_s = "s" if replayed_episode_count > 1 else "" print(f"Finished replaying {replayed_episode_count} episode{plural_trailing_s}.") + + # Print success statistics only if validation was enabled + if success_term is not None: + print(f"Successfully replayed: {recorded_episode_count}/{replayed_episode_count}") + + # Print failed demo IDs if any + if failed_demo_ids: + print(f"\nFailed demo IDs ({len(failed_demo_ids)} total):") + print(f" {sorted(failed_demo_ids)}") + env.close() diff --git a/scripts/tools/test/test_cosmos_prompt_gen.py b/scripts/tools/test/test_cosmos_prompt_gen.py new file mode 100644 index 000000000000..17f1764d914b --- /dev/null +++ b/scripts/tools/test/test_cosmos_prompt_gen.py @@ -0,0 +1,169 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test cases for Cosmos prompt generation script.""" + +import json +import os +import tempfile + +import pytest + +from scripts.tools.cosmos.cosmos_prompt_gen import generate_prompt, main + + +@pytest.fixture(scope="class") +def temp_templates_file(): + """Create temporary templates file.""" + temp_file = tempfile.NamedTemporaryFile(suffix=".json", delete=False) # noqa: SIM115 + + # Create test templates + test_templates = { + "lighting": ["with bright lighting", "with dim lighting", "with natural lighting"], + "color": ["in warm colors", "in cool colors", "in vibrant colors"], + "style": ["in a realistic style", "in an artistic style", "in a minimalist style"], + "empty_section": [], # Test empty section + "invalid_section": "not a list", # Test invalid section + } + + # Write templates to file + with open(temp_file.name, "w") as f: + json.dump(test_templates, f) + + yield temp_file.name + # Cleanup + os.remove(temp_file.name) + + +@pytest.fixture +def temp_output_file(): + """Create temporary output file.""" + temp_file = tempfile.NamedTemporaryFile(suffix=".txt", delete=False) # noqa: SIM115 + yield temp_file.name + # Cleanup + os.remove(temp_file.name) + + +class TestCosmosPromptGen: + """Test cases for Cosmos prompt generation functionality.""" + + def test_generate_prompt_valid_templates(self, temp_templates_file): + """Test generating a prompt with valid templates.""" + prompt = generate_prompt(temp_templates_file) + + # Check that prompt is a string + assert isinstance(prompt, str) + + # Check that prompt contains at least one word + assert len(prompt.split()) > 0 + + # Check that prompt contains valid sections + valid_sections = ["lighting", "color", "style"] + found_sections = [section for section in valid_sections if section in prompt.lower()] + assert len(found_sections) > 0 + + def test_generate_prompt_invalid_file(self): + """Test generating a prompt with invalid file path.""" + with pytest.raises(FileNotFoundError): + generate_prompt("nonexistent_file.json") + + def test_generate_prompt_invalid_json(self): + """Test generating a prompt with invalid JSON file.""" + # Create a temporary file with invalid JSON + with tempfile.NamedTemporaryFile(suffix=".json", delete=False) as temp_file: + temp_file.write(b"invalid json content") + temp_file.flush() + + try: + with pytest.raises(ValueError): + generate_prompt(temp_file.name) + finally: + os.remove(temp_file.name) + + def test_main_function_single_prompt(self, temp_templates_file, temp_output_file): + """Test main function with single prompt generation.""" + # Mock command line arguments + import sys + + original_argv = sys.argv + sys.argv = [ + "cosmos_prompt_gen.py", + "--templates_path", + temp_templates_file, + "--num_prompts", + "1", + "--output_path", + temp_output_file, + ] + + try: + main() + + # Check if output file was created + assert os.path.exists(temp_output_file) + + # Check content of output file + with open(temp_output_file) as f: + content = f.read().strip() + assert len(content) > 0 + assert len(content.split("\n")) == 1 + finally: + # Restore original argv + sys.argv = original_argv + + def test_main_function_multiple_prompts(self, temp_templates_file, temp_output_file): + """Test main function with multiple prompt generation.""" + # Mock command line arguments + import sys + + original_argv = sys.argv + sys.argv = [ + "cosmos_prompt_gen.py", + "--templates_path", + temp_templates_file, + "--num_prompts", + "3", + "--output_path", + temp_output_file, + ] + + try: + main() + + # Check if output file was created + assert os.path.exists(temp_output_file) + + # Check content of output file + with open(temp_output_file) as f: + content = f.read().strip() + assert len(content) > 0 + assert len(content.split("\n")) == 3 + + # Check that each line is a valid prompt + for line in content.split("\n"): + assert len(line) > 0 + finally: + # Restore original argv + sys.argv = original_argv + + def test_main_function_default_output(self, temp_templates_file): + """Test main function with default output path.""" + # Mock command line arguments + import sys + + original_argv = sys.argv + sys.argv = ["cosmos_prompt_gen.py", "--templates_path", temp_templates_file, "--num_prompts", "1"] + + try: + main() + + # Check if default output file was created + assert os.path.exists("prompts.txt") + + # Clean up default output file + os.remove("prompts.txt") + finally: + # Restore original argv + sys.argv = original_argv diff --git a/scripts/tools/test/test_hdf5_to_mp4.py b/scripts/tools/test/test_hdf5_to_mp4.py new file mode 100644 index 000000000000..33ccd0d1723e --- /dev/null +++ b/scripts/tools/test/test_hdf5_to_mp4.py @@ -0,0 +1,173 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test cases for HDF5 to MP4 conversion script.""" + +import os +import tempfile + +import h5py +import numpy as np +import pytest + +from scripts.tools.hdf5_to_mp4 import get_num_demos, main, write_demo_to_mp4 + + +@pytest.fixture(scope="class") +def temp_hdf5_file(): + """Create temporary HDF5 file with test data.""" + temp_file = tempfile.NamedTemporaryFile(suffix=".h5", delete=False) # noqa: SIM115 + with h5py.File(temp_file.name, "w") as h5f: + # Create test data structure + for demo_id in range(2): # Create 2 demos + demo_group = h5f.create_group(f"data/demo_{demo_id}/obs") + + # Create RGB frames (2 frames per demo) + rgb_data = np.random.randint(0, 255, (2, 704, 1280, 3), dtype=np.uint8) + demo_group.create_dataset("table_cam", data=rgb_data) + + # Create segmentation frames + seg_data = np.random.randint(0, 255, (2, 704, 1280, 4), dtype=np.uint8) + demo_group.create_dataset("table_cam_segmentation", data=seg_data) + + # Create normal maps + normals_data = np.random.rand(2, 704, 1280, 3).astype(np.float32) + demo_group.create_dataset("table_cam_normals", data=normals_data) + + # Create depth maps + depth_data = np.random.rand(2, 704, 1280, 1).astype(np.float32) + demo_group.create_dataset("table_cam_depth", data=depth_data) + + yield temp_file.name + # Cleanup + os.remove(temp_file.name) + + +@pytest.fixture +def temp_output_dir(): + """Create temporary output directory.""" + temp_dir = tempfile.mkdtemp() # noqa: SIM115 + yield temp_dir + # Cleanup + for file in os.listdir(temp_dir): + os.remove(os.path.join(temp_dir, file)) + os.rmdir(temp_dir) + + +class TestHDF5ToMP4: + """Test cases for HDF5 to MP4 conversion functionality.""" + + def test_get_num_demos(self, temp_hdf5_file): + """Test the get_num_demos function.""" + num_demos = get_num_demos(temp_hdf5_file) + assert num_demos == 2 + + def test_write_demo_to_mp4_rgb(self, temp_hdf5_file, temp_output_dir): + """Test writing RGB frames to MP4.""" + write_demo_to_mp4(temp_hdf5_file, 0, "data/demo_0/obs", "table_cam", temp_output_dir, 704, 1280) + + output_file = os.path.join(temp_output_dir, "demo_0_table_cam.mp4") + assert os.path.exists(output_file) + assert os.path.getsize(output_file) > 0 + + def test_write_demo_to_mp4_segmentation(self, temp_hdf5_file, temp_output_dir): + """Test writing segmentation frames to MP4.""" + write_demo_to_mp4(temp_hdf5_file, 0, "data/demo_0/obs", "table_cam_segmentation", temp_output_dir, 704, 1280) + + output_file = os.path.join(temp_output_dir, "demo_0_table_cam_segmentation.mp4") + assert os.path.exists(output_file) + assert os.path.getsize(output_file) > 0 + + def test_write_demo_to_mp4_normals(self, temp_hdf5_file, temp_output_dir): + """Test writing normal maps to MP4.""" + write_demo_to_mp4(temp_hdf5_file, 0, "data/demo_0/obs", "table_cam_normals", temp_output_dir, 704, 1280) + + output_file = os.path.join(temp_output_dir, "demo_0_table_cam_normals.mp4") + assert os.path.exists(output_file) + assert os.path.getsize(output_file) > 0 + + def test_write_demo_to_mp4_shaded_segmentation(self, temp_hdf5_file, temp_output_dir): + """Test writing shaded_segmentation frames to MP4.""" + write_demo_to_mp4( + temp_hdf5_file, + 0, + "data/demo_0/obs", + "table_cam_shaded_segmentation", + temp_output_dir, + 704, + 1280, + ) + + output_file = os.path.join(temp_output_dir, "demo_0_table_cam_shaded_segmentation.mp4") + assert os.path.exists(output_file) + assert os.path.getsize(output_file) > 0 + + def test_write_demo_to_mp4_depth(self, temp_hdf5_file, temp_output_dir): + """Test writing depth maps to MP4.""" + write_demo_to_mp4(temp_hdf5_file, 0, "data/demo_0/obs", "table_cam_depth", temp_output_dir, 704, 1280) + + output_file = os.path.join(temp_output_dir, "demo_0_table_cam_depth.mp4") + assert os.path.exists(output_file) + assert os.path.getsize(output_file) > 0 + + def test_write_demo_to_mp4_invalid_demo(self, temp_hdf5_file, temp_output_dir): + """Test writing with invalid demo ID.""" + with pytest.raises(KeyError): + write_demo_to_mp4( + temp_hdf5_file, + 999, # Invalid demo ID + "data/demo_999/obs", + "table_cam", + temp_output_dir, + 704, + 1280, + ) + + def test_write_demo_to_mp4_invalid_key(self, temp_hdf5_file, temp_output_dir): + """Test writing with invalid input key.""" + with pytest.raises(KeyError): + write_demo_to_mp4(temp_hdf5_file, 0, "data/demo_0/obs", "invalid_key", temp_output_dir, 704, 1280) + + def test_main_function(self, temp_hdf5_file, temp_output_dir): + """Test the main function.""" + # Mock command line arguments + import sys + + original_argv = sys.argv + sys.argv = [ + "hdf5_to_mp4.py", + "--input_file", + temp_hdf5_file, + "--output_dir", + temp_output_dir, + "--input_keys", + "table_cam", + "table_cam_segmentation", + "--video_height", + "704", + "--video_width", + "1280", + "--framerate", + "30", + ] + + try: + main() + + # Check if output files were created + expected_files = [ + "demo_0_table_cam.mp4", + "demo_0_table_cam_segmentation.mp4", + "demo_1_table_cam.mp4", + "demo_1_table_cam_segmentation.mp4", + ] + + for file in expected_files: + output_file = os.path.join(temp_output_dir, file) + assert os.path.exists(output_file) + assert os.path.getsize(output_file) > 0 + finally: + # Restore original argv + sys.argv = original_argv diff --git a/scripts/tools/test/test_mp4_to_hdf5.py b/scripts/tools/test/test_mp4_to_hdf5.py new file mode 100644 index 000000000000..631ac41da228 --- /dev/null +++ b/scripts/tools/test/test_mp4_to_hdf5.py @@ -0,0 +1,181 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test cases for MP4 to HDF5 conversion script.""" + +import os +import tempfile + +import cv2 +import h5py +import numpy as np +import pytest + +from scripts.tools.mp4_to_hdf5 import get_frames_from_mp4, main, process_video_and_demo + + +@pytest.fixture(scope="class") +def temp_hdf5_file(): + """Create temporary HDF5 file with test data.""" + temp_file = tempfile.NamedTemporaryFile(suffix=".h5", delete=False) # noqa: SIM115 + with h5py.File(temp_file.name, "w") as h5f: + # Create test data structure for 2 demos + for demo_id in range(2): + demo_group = h5f.create_group(f"data/demo_{demo_id}") + obs_group = demo_group.create_group("obs") + + # Create actions data + actions_data = np.random.rand(10, 7).astype(np.float32) + demo_group.create_dataset("actions", data=actions_data) + + # Create robot state data + eef_pos_data = np.random.rand(10, 3).astype(np.float32) + eef_quat_data = np.random.rand(10, 4).astype(np.float32) + gripper_pos_data = np.random.rand(10, 1).astype(np.float32) + obs_group.create_dataset("eef_pos", data=eef_pos_data) + obs_group.create_dataset("eef_quat", data=eef_quat_data) + obs_group.create_dataset("gripper_pos", data=gripper_pos_data) + + # Create camera data + table_cam_data = np.random.randint(0, 255, (10, 704, 1280, 3), dtype=np.uint8) + wrist_cam_data = np.random.randint(0, 255, (10, 704, 1280, 3), dtype=np.uint8) + obs_group.create_dataset("table_cam", data=table_cam_data) + obs_group.create_dataset("wrist_cam", data=wrist_cam_data) + + # Set attributes + demo_group.attrs["num_samples"] = 10 + + yield temp_file.name + # Cleanup + os.remove(temp_file.name) + + +@pytest.fixture(scope="class") +def temp_videos_dir(): + """Create temporary MP4 files.""" + temp_dir = tempfile.mkdtemp() # noqa: SIM115 + video_paths = [] + + for demo_id in range(2): + video_path = os.path.join(temp_dir, f"demo_{demo_id}_table_cam.mp4") + video_paths.append(video_path) + + # Create a test video + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + video = cv2.VideoWriter(video_path, fourcc, 30, (1280, 704)) + + # Write some random frames + for _ in range(10): + frame = np.random.randint(0, 255, (704, 1280, 3), dtype=np.uint8) + video.write(frame) + video.release() + + yield temp_dir, video_paths + + # Cleanup + for video_path in video_paths: + os.remove(video_path) + os.rmdir(temp_dir) + + +@pytest.fixture +def temp_output_file(): + """Create temporary output file.""" + temp_file = tempfile.NamedTemporaryFile(suffix=".h5", delete=False) # noqa: SIM115 + yield temp_file.name + # Cleanup + os.remove(temp_file.name) + + +class TestMP4ToHDF5: + """Test cases for MP4 to HDF5 conversion functionality.""" + + def test_get_frames_from_mp4(self, temp_videos_dir): + """Test extracting frames from MP4 video.""" + _, video_paths = temp_videos_dir + frames = get_frames_from_mp4(video_paths[0]) + + # Check frame properties + assert frames.shape[0] == 10 # Number of frames + assert frames.shape[1:] == (704, 1280, 3) # Frame dimensions + assert frames.dtype == np.uint8 # Data type + + def test_get_frames_from_mp4_resize(self, temp_videos_dir): + """Test extracting frames with resizing.""" + _, video_paths = temp_videos_dir + target_height, target_width = 352, 640 + frames = get_frames_from_mp4(video_paths[0], target_height, target_width) + + # Check resized frame properties + assert frames.shape[0] == 10 # Number of frames + assert frames.shape[1:] == (target_height, target_width, 3) # Resized dimensions + assert frames.dtype == np.uint8 # Data type + + def test_process_video_and_demo(self, temp_hdf5_file, temp_videos_dir, temp_output_file): + """Test processing a single video and creating a new demo.""" + _, video_paths = temp_videos_dir + with h5py.File(temp_hdf5_file, "r") as f_in, h5py.File(temp_output_file, "w") as f_out: + process_video_and_demo(f_in, f_out, video_paths[0], 0, 2) + + # Check if new demo was created with correct data + assert "data/demo_2" in f_out + assert "data/demo_2/actions" in f_out + assert "data/demo_2/obs/eef_pos" in f_out + assert "data/demo_2/obs/eef_quat" in f_out + assert "data/demo_2/obs/gripper_pos" in f_out + assert "data/demo_2/obs/table_cam" in f_out + assert "data/demo_2/obs/wrist_cam" in f_out + + # Check data shapes + assert f_out["data/demo_2/actions"].shape == (10, 7) + assert f_out["data/demo_2/obs/eef_pos"].shape == (10, 3) + assert f_out["data/demo_2/obs/eef_quat"].shape == (10, 4) + assert f_out["data/demo_2/obs/gripper_pos"].shape == (10, 1) + assert f_out["data/demo_2/obs/table_cam"].shape == (10, 704, 1280, 3) + assert f_out["data/demo_2/obs/wrist_cam"].shape == (10, 704, 1280, 3) + + # Check attributes + assert f_out["data/demo_2"].attrs["num_samples"] == 10 + + def test_main_function(self, temp_hdf5_file, temp_videos_dir, temp_output_file): + """Test the main function.""" + # Mock command line arguments + import sys + + original_argv = sys.argv + sys.argv = [ + "mp4_to_hdf5.py", + "--input_file", + temp_hdf5_file, + "--videos_dir", + temp_videos_dir[0], + "--output_file", + temp_output_file, + ] + + try: + main() + + # Check if output file was created with correct data + with h5py.File(temp_output_file, "r") as f: + # Check if original demos were copied + assert "data/demo_0" in f + assert "data/demo_1" in f + + # Check if new demos were created + assert "data/demo_2" in f + assert "data/demo_3" in f + + # Check data in new demos + for demo_id in [2, 3]: + assert f"data/demo_{demo_id}/actions" in f + assert f"data/demo_{demo_id}/obs/eef_pos" in f + assert f"data/demo_{demo_id}/obs/eef_quat" in f + assert f"data/demo_{demo_id}/obs/gripper_pos" in f + assert f"data/demo_{demo_id}/obs/table_cam" in f + assert f"data/demo_{demo_id}/obs/wrist_cam" in f + finally: + # Restore original argv + sys.argv = original_argv diff --git a/scripts/tools/pretrained_checkpoint.py b/scripts/tools/train_and_publish_checkpoints.py similarity index 82% rename from scripts/tools/pretrained_checkpoint.py rename to scripts/tools/train_and_publish_checkpoints.py index 44ebf6f1ce24..97ebb6f4c5f7 100644 --- a/scripts/tools/pretrained_checkpoint.py +++ b/scripts/tools/train_and_publish_checkpoints.py @@ -1,10 +1,44 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -""" -Script to manage pretrained checkpoints for our environments. +"""Script to manage pretrained checkpoints for Isaac Lab environments. + +This script is used to train and publish pretrained checkpoints for Isaac Lab environments. +It supports multiple workflows: rl_games, rsl_rl, sb3, and skrl. + +* To train an agent using the rl_games workflow on the Isaac-Cartpole-v0 environment: + + .. code-block:: shell + + python scripts/tools/train_and_publish_checkpoints.py --train rl_games:Isaac-Cartpole-v0 + +* To train and publish the checkpoints for all workflows on only the direct Cartpole environments: + + .. code-block:: shell + + python scripts/tools/train_and_publish_checkpoints.py \ + -tp "*:Isaac-Cartpole-*Direct-v0" \ + --/persistent/isaaclab/asset_root/pretrained_checkpoints="/some/path" + +* To review all repose cube jobs, excluding the 'Play' tasks and 'skrl' workflows: + + .. code-block:: shell + + python scripts/tools/train_and_publish_checkpoints.py \ + -r "*:*Repose-Cube*" \ + --exclude "*:*Play*" \ + --exclude skrl:* + +* To publish all results (that have been reviewed and approved). + + .. code-block:: shell + + python scripts/tools/train_and_publish_checkpoints.py \ + --publish --all \ + --/persistent/isaaclab/asset_root/pretrained_checkpoints="/some/path" + """ import argparse @@ -14,18 +48,21 @@ # Initialize the parser parser = argparse.ArgumentParser( description=""" -Script used for the training and publishing of pre-trained checkpoints for Isaac Lab. +Script for training and publishing pre-trained checkpoints in Isaac Lab. + +Examples: + # Train an agent using the rl_games workflow for the Isaac-Cartpole-v0 environment. + train_and_publish_checkpoints.py --train rl_games:Isaac-Cartpole-v0 -Examples : - # Train an agent using the rl_games workflow on the Isaac-Cartpole-v0 environment. - pretrained_checkpoint.py --train rl_games:Isaac-Cartpole-v0 - # Train and publish the checkpoints for all workflows on only the direct Cartpole environments. - pretrained_checkpoint.py -tp "*:Isaac-Cartpole-*Direct-v0" \\ + # Train and publish checkpoints for all workflows, targeting only direct Cartpole environments. + train_and_publish_checkpoints.py -tp "*:Isaac-Cartpole-*Direct-v0" \\ --/persistent/isaaclab/asset_root/pretrained_checkpoints="/some/path" - # Review all repose cube jobs, excluding the Play tasks and skrl - pretrained_checkpoint.py -r "*:*Repose-Cube*" --exclude "*:*Play*" --exclude skrl:* - # Publish all results (that have been reviewed and approved). - pretrained_checkpoint.py --publish --all \\ + + # Review all Repose Cube jobs, excluding Play tasks and skrl jobs. + train_and_publish_checkpoints.py -r "*:*Repose-Cube*" --exclude "*:*Play*" --exclude skrl:* + + # Publish all results that have been reviewed and approved. + train_and_publish_checkpoints.py --publish --all \\ --/persistent/isaaclab/asset_root/pretrained_checkpoints="/some/path" """, formatter_class=argparse.RawTextHelpFormatter, @@ -36,10 +73,13 @@ "jobs", nargs="*", help=""" -A job consists of a workflow and a task name separated by a colon (wildcards optional), for example : - rl_games:Isaac-Humanoid-*v0 - rsl_rl:Isaac-Ant-*-v0 - *:Isaac-Velocity-Flat-Spot-v0 +A job consists of a workflow and a task name, separated by a colon (wildcards are optional). Examples: + + rl_games:Isaac-Humanoid-*v0 # Wildcard for any Humanoid version + rsl_rl:Isaac-Ant-*-v0 # Wildcard for any Ant environment + *:Isaac-Velocity-Flat-Spot-v0 # Wildcard for any workflow, specific task + +Wildcards can be used in either the workflow or task name to match multiple entries. """, ) parser.add_argument("-t", "--train", action="store_true", help="Train checkpoints for later publishing.") @@ -84,17 +124,18 @@ # Now everything else import fnmatch -import gymnasium as gym import json -import numpy as np import os import subprocess import sys +import gymnasium as gym +import numpy as np + import omni.client from omni.client._omniclient import CopyBehavior -from isaaclab.utils.pretrained_checkpoint import ( +from isaaclab_rl.utils.pretrained_checkpoint import ( WORKFLOW_EXPERIMENT_NAME_VARIABLE, WORKFLOW_PLAYER, WORKFLOW_TRAINER, @@ -277,7 +318,6 @@ def publish_pretrained_checkpoint(workflow, task_name, force_publish=False): # Not forcing, need to check review results if not force_publish: - # Grab the review if it exists review = get_pretrained_checkpoint_review(workflow, task_name) @@ -315,7 +355,6 @@ def get_job_summary_row(workflow, task_name): def main(): - # Figure out what workflows and tasks we'll be using if args.all: jobs = ["*:*"] @@ -365,7 +404,6 @@ def main(): if __name__ == "__main__": - try: # Run the main function main() diff --git a/scripts/tutorials/00_sim/create_empty.py b/scripts/tutorials/00_sim/create_empty.py index 31c03115de06..6fa283a68f1f 100644 --- a/scripts/tutorials/00_sim/create_empty.py +++ b/scripts/tutorials/00_sim/create_empty.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/scripts/tutorials/00_sim/launch_app.py b/scripts/tutorials/00_sim/launch_app.py index fb800871e6cb..1622d3ba956e 100644 --- a/scripts/tutorials/00_sim/launch_app.py +++ b/scripts/tutorials/00_sim/launch_app.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/scripts/tutorials/00_sim/log_time.py b/scripts/tutorials/00_sim/log_time.py index 99a157239cca..2e4445c3d470 100644 --- a/scripts/tutorials/00_sim/log_time.py +++ b/scripts/tutorials/00_sim/log_time.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/scripts/tutorials/00_sim/set_rendering_mode.py b/scripts/tutorials/00_sim/set_rendering_mode.py new file mode 100644 index 000000000000..38a1d5b2ba88 --- /dev/null +++ b/scripts/tutorials/00_sim/set_rendering_mode.py @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script demonstrates how to spawn prims into the scene. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p scripts/tutorials/00_sim/set_rendering_mode.py + +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser( + description="Tutorial on viewing a warehouse scene with a given rendering mode preset." +) +# 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 isaaclab.sim as sim_utils +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + + +def main(): + """Main function.""" + + # rendering modes include performance, balanced, and quality + # note, the rendering_mode specified in the CLI argument (--rendering_mode) takes precedence over + # this Render Config setting + rendering_mode = "performance" + + # carb setting dictionary can include any rtx carb setting which will overwrite the native preset setting + carb_settings = {"rtx.reflections.enabled": True} + + # Initialize render config + render_cfg = sim_utils.RenderCfg( + rendering_mode=rendering_mode, + carb_settings=carb_settings, + ) + + # Initialize the simulation context with render coofig + sim_cfg = sim_utils.SimulationCfg(render=render_cfg) + sim = sim_utils.SimulationContext(sim_cfg) + + # Pose camera in the hospital lobby area + sim.set_camera_view([-11, -0.5, 2], [0, 0, 0.5]) + + # Load hospital scene + hospital_usd_path = f"{ISAAC_NUCLEUS_DIR}/Environments/Hospital/hospital.usd" + cfg = sim_utils.UsdFileCfg(usd_path=hospital_usd_path) + cfg.func("/Scene", cfg) + + # Play the simulator + sim.reset() + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Run simulation and view scene + while simulation_app.is_running(): + sim.step() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/tutorials/00_sim/spawn_prims.py b/scripts/tutorials/00_sim/spawn_prims.py index ccd783d913a6..7c120bd308dd 100644 --- a/scripts/tutorials/00_sim/spawn_prims.py +++ b/scripts/tutorials/00_sim/spawn_prims.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -31,8 +31,6 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -51,7 +49,7 @@ def design_scene(): cfg_light_distant.func("/World/lightDistant", cfg_light_distant, translation=(1, 0, 10)) # create a new xform prim for all objects to be spawned under - prim_utils.create_prim("/World/Objects", "Xform") + sim_utils.create_prim("/World/Objects", "Xform") # spawn a red cone cfg_cone = sim_utils.ConeCfg( radius=0.15, @@ -96,10 +94,8 @@ def main(): sim = sim_utils.SimulationContext(sim_cfg) # Set main camera sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) - - # Design scene by adding assets to it + # Design scene design_scene() - # Play the simulator sim.reset() # Now we are ready! diff --git a/scripts/tutorials/01_assets/add_new_robot.py b/scripts/tutorials/01_assets/add_new_robot.py new file mode 100644 index 000000000000..bc322d109479 --- /dev/null +++ b/scripts/tutorials/01_assets/add_new_robot.py @@ -0,0 +1,179 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser( + description="This script demonstrates adding a custom robot to an Isaac Lab environment." +) +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.") +# 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 + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import AssetBaseCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +JETBOT_CONFIG = ArticulationCfg( + 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)}, +) + +DOFBOT_CONFIG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Yahboom/Dofbot/dofbot.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 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "joint1": 0.0, + "joint2": 0.0, + "joint3": 0.0, + "joint4": 0.0, + }, + pos=(0.25, -0.25, 0.0), + ), + actuators={ + "front_joints": ImplicitActuatorCfg( + joint_names_expr=["joint[1-2]"], + effort_limit_sim=100.0, + velocity_limit_sim=100.0, + stiffness=10000.0, + damping=100.0, + ), + "joint3_act": ImplicitActuatorCfg( + joint_names_expr=["joint3"], + effort_limit_sim=100.0, + velocity_limit_sim=100.0, + stiffness=10000.0, + damping=100.0, + ), + "joint4_act": ImplicitActuatorCfg( + joint_names_expr=["joint4"], + effort_limit_sim=100.0, + velocity_limit_sim=100.0, + stiffness=10000.0, + damping=100.0, + ), + }, +) + + +class NewRobotsSceneCfg(InteractiveSceneCfg): + """Designs the scene.""" + + # Ground-plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # robot + Jetbot = JETBOT_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Jetbot") + Dofbot = DOFBOT_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Dofbot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + sim_dt = sim.get_physics_dt() + sim_time = 0.0 + count = 0 + + while simulation_app.is_running(): + # reset + if count % 500 == 0: + # reset counters + count = 0 + # reset the scene entities to their initial positions offset by the environment origins + root_jetbot_state = scene["Jetbot"].data.default_root_state.clone() + root_jetbot_state[:, :3] += scene.env_origins + root_dofbot_state = scene["Dofbot"].data.default_root_state.clone() + root_dofbot_state[:, :3] += scene.env_origins + + # copy the default root state to the sim for the jetbot's orientation and velocity + scene["Jetbot"].write_root_pose_to_sim(root_jetbot_state[:, :7]) + scene["Jetbot"].write_root_velocity_to_sim(root_jetbot_state[:, 7:]) + scene["Dofbot"].write_root_pose_to_sim(root_dofbot_state[:, :7]) + scene["Dofbot"].write_root_velocity_to_sim(root_dofbot_state[:, 7:]) + + # copy the default joint states to the sim + joint_pos, joint_vel = ( + scene["Jetbot"].data.default_joint_pos.clone(), + scene["Jetbot"].data.default_joint_vel.clone(), + ) + scene["Jetbot"].write_joint_state_to_sim(joint_pos, joint_vel) + joint_pos, joint_vel = ( + scene["Dofbot"].data.default_joint_pos.clone(), + scene["Dofbot"].data.default_joint_vel.clone(), + ) + scene["Dofbot"].write_joint_state_to_sim(joint_pos, joint_vel) + # clear internal buffers + scene.reset() + print("[INFO]: Resetting Jetbot and Dofbot state...") + + # drive around + if count % 100 < 75: + # Drive straight by setting equal wheel velocities + action = torch.Tensor([[10.0, 10.0]]) + else: + # Turn by applying different velocities + action = torch.Tensor([[5.0, -5.0]]) + + scene["Jetbot"].set_joint_velocity_target(action) + + # wave + wave_action = scene["Dofbot"].data.default_joint_pos + wave_action[:, 0:4] = 0.25 * np.sin(2 * np.pi * 0.5 * sim_time) + scene["Dofbot"].set_joint_position_target(wave_action) + + scene.write_data_to_sim() + sim.step() + sim_time += sim_dt + count += 1 + scene.update(sim_dt) + + +def main(): + """Main function.""" + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + sim.set_camera_view([3.5, 0.0, 3.2], [0.0, 0.0, 0.5]) + # Design scene + scene_cfg = NewRobotsSceneCfg(args_cli.num_envs, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/scripts/tutorials/01_assets/run_articulation.py b/scripts/tutorials/01_assets/run_articulation.py index 6cace5f09ad2..bc4254cbae1f 100644 --- a/scripts/tutorials/01_assets/run_articulation.py +++ b/scripts/tutorials/01_assets/run_articulation.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -34,8 +34,6 @@ import torch -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils from isaaclab.assets import Articulation from isaaclab.sim import SimulationContext @@ -59,9 +57,9 @@ def design_scene() -> tuple[dict, list[list[float]]]: # Each group will have a robot in it origins = [[0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]] # Origin 1 - prim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) + sim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) # Origin 2 - prim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) + sim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) # Articulation cartpole_cfg = CARTPOLE_CFG.copy() diff --git a/scripts/tutorials/01_assets/run_deformable_object.py b/scripts/tutorials/01_assets/run_deformable_object.py index b7d32c14ebaa..3623bb3d8a19 100644 --- a/scripts/tutorials/01_assets/run_deformable_object.py +++ b/scripts/tutorials/01_assets/run_deformable_object.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,8 +35,6 @@ 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 @@ -56,7 +54,7 @@ def design_scene(): # 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) + sim_utils.create_prim(f"/World/Origin{i}", "Xform", translation=origin) # Deformable Object cfg = DeformableObjectCfg( diff --git a/scripts/tutorials/01_assets/run_rigid_object.py b/scripts/tutorials/01_assets/run_rigid_object.py index 24d6bd23a563..dc1a88c57eb1 100644 --- a/scripts/tutorials/01_assets/run_rigid_object.py +++ b/scripts/tutorials/01_assets/run_rigid_object.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,8 +35,6 @@ 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 RigidObject, RigidObjectCfg @@ -56,7 +54,7 @@ def design_scene(): # 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) + sim_utils.create_prim(f"/World/Origin{i}", "Xform", translation=origin) # Rigid Object cone_cfg = RigidObjectCfg( @@ -120,7 +118,7 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, RigidObj cone_object.update(sim_dt) # print the root position if count % 50 == 0: - print(f"Root position (in world): {cone_object.data.root_state_w[:, :3]}") + print(f"Root position (in world): {cone_object.data.root_pos_w}") def main(): diff --git a/scripts/tutorials/01_assets/run_surface_gripper.py b/scripts/tutorials/01_assets/run_surface_gripper.py new file mode 100644 index 000000000000..066dd9a077d2 --- /dev/null +++ b/scripts/tutorials/01_assets/run_surface_gripper.py @@ -0,0 +1,181 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script demonstrates how to spawn a pick-and-place robot equipped with a surface gripper and interact with it. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p scripts/tutorials/01_assets/run_surface_gripper.py --device=cpu + +When running this script make sure the --device flag is set to cpu. This is because the surface gripper is +currently only supported on the CPU. +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Tutorial on spawning and interacting with a Surface Gripper.") +# 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 isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, SurfaceGripper, SurfaceGripperCfg +from isaaclab.sim import SimulationContext + +## +# Pre-defined configs +## +from isaaclab_assets import PICK_AND_PLACE_CFG # isort:skip + + +def design_scene(): + """Designs the scene.""" + # Ground-plane + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/defaultGroundPlane", cfg) + # Lights + cfg = sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + cfg.func("/World/Light", cfg) + + # Create separate groups called "Origin1", "Origin2" + # Each group will have a robot in it + origins = [[2.75, 0.0, 0.0], [-2.75, 0.0, 0.0]] + # Origin 1 + sim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) + # Origin 2 + sim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) + + # Articulation: First we define the robot config + pick_and_place_robot_cfg = PICK_AND_PLACE_CFG.copy() + pick_and_place_robot_cfg.prim_path = "/World/Origin.*/Robot" + pick_and_place_robot = Articulation(cfg=pick_and_place_robot_cfg) + + # Surface Gripper: Next we define the surface gripper config + surface_gripper_cfg = SurfaceGripperCfg() + # We need to tell the View which prim to use for the surface gripper + surface_gripper_cfg.prim_path = "/World/Origin.*/Robot/picker_head/SurfaceGripper" + # We can then set different parameters for the surface gripper, note that if these parameters are not set, + # the View will try to read them from the prim. + surface_gripper_cfg.max_grip_distance = 0.1 # [m] (Maximum distance at which the gripper can grasp an object) + surface_gripper_cfg.shear_force_limit = 500.0 # [N] (Force limit in the direction perpendicular direction) + surface_gripper_cfg.coaxial_force_limit = 500.0 # [N] (Force limit in the direction of the gripper's axis) + surface_gripper_cfg.retry_interval = 0.1 # seconds (Time the gripper will stay in a grasping state) + # We can now spawn the surface gripper + surface_gripper = SurfaceGripper(cfg=surface_gripper_cfg) + + # return the scene information + scene_entities = {"pick_and_place_robot": pick_and_place_robot, "surface_gripper": surface_gripper} + return scene_entities, origins + + +def run_simulator( + sim: sim_utils.SimulationContext, entities: dict[str, Articulation | SurfaceGripper], origins: torch.Tensor +): + """Runs the simulation loop.""" + # Extract scene entities + robot: Articulation = entities["pick_and_place_robot"] + surface_gripper: SurfaceGripper = entities["surface_gripper"] + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + count = 0 + # Simulation loop + while simulation_app.is_running(): + # Reset + if count % 500 == 0: + # reset counter + count = 0 + # reset the scene entities + # root state + # we offset the root state by the origin since the states are written in simulation world frame + # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world + root_state = robot.data.default_root_state.clone() + root_state[:, :3] += origins + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) + # set joint positions with some noise + joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() + joint_pos += torch.rand_like(joint_pos) * 0.1 + robot.write_joint_state_to_sim(joint_pos, joint_vel) + # clear internal buffers + robot.reset() + print("[INFO]: Resetting robot state...") + # Opens the gripper and makes sure the gripper is in the open state + surface_gripper.reset() + print("[INFO]: Resetting gripper state...") + + # Sample a random command between -1 and 1. + gripper_commands = torch.rand(surface_gripper.num_instances) * 2.0 - 1.0 + # The gripper behavior is as follows: + # -1 < command < -0.3 --> Gripper is Opening + # -0.3 < command < 0.3 --> Gripper is Idle + # 0.3 < command < 1 --> Gripper is Closing + print(f"[INFO]: Gripper commands: {gripper_commands}") + mapped_commands = [ + "Opening" if command < -0.3 else "Closing" if command > 0.3 else "Idle" for command in gripper_commands + ] + print(f"[INFO]: Mapped commands: {mapped_commands}") + # Set the gripper command + surface_gripper.set_grippers_command(gripper_commands) + # Write data to sim + surface_gripper.write_data_to_sim() + # Perform step + sim.step() + # Increment counter + count += 1 + # Read the gripper state from the simulation + surface_gripper.update(sim_dt) + # Read the gripper state from the buffer + surface_gripper_state = surface_gripper.state + # The gripper state is a list of integers that can be mapped to the following: + # -1 --> Open + # 0 --> Closing + # 1 --> Closed + # Print the gripper state + print(f"[INFO]: Gripper state: {surface_gripper_state}") + mapped_commands = [ + "Open" if state == -1 else "Closing" if state == 0 else "Closed" for state in surface_gripper_state.tolist() + ] + print(f"[INFO]: Mapped commands: {mapped_commands}") + + +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([2.75, 7.5, 10.0], [2.75, 0.0, 0.0]) + # 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/scripts/tutorials/02_scene/create_scene.py b/scripts/tutorials/02_scene/create_scene.py index e9fa2c73cffa..82b5b21c0097 100644 --- a/scripts/tutorials/02_scene/create_scene.py +++ b/scripts/tutorials/02_scene/create_scene.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/scripts/tutorials/03_envs/create_cartpole_base_env.py b/scripts/tutorials/03_envs/create_cartpole_base_env.py index 5734cde39910..35c3650e6811 100644 --- a/scripts/tutorials/03_envs/create_cartpole_base_env.py +++ b/scripts/tutorials/03_envs/create_cartpole_base_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -36,6 +36,7 @@ """Rest everything follows.""" import math + import torch import isaaclab.envs.mdp as mdp @@ -140,6 +141,7 @@ def main(): # parse the arguments env_cfg = CartpoleEnvCfg() env_cfg.scene.num_envs = args_cli.num_envs + env_cfg.sim.device = args_cli.device # setup base environment env = ManagerBasedEnv(cfg=env_cfg) diff --git a/scripts/tutorials/03_envs/create_cube_base_env.py b/scripts/tutorials/03_envs/create_cube_base_env.py index 772e283da9bc..641512607e31 100644 --- a/scripts/tutorials/03_envs/create_cube_base_env.py +++ b/scripts/tutorials/03_envs/create_cube_base_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -56,11 +56,10 @@ import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg, RigidObject, RigidObjectCfg from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg -from isaaclab.managers import ActionTerm, ActionTermCfg +from isaaclab.managers import ActionTerm, ActionTermCfg, SceneEntityCfg 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.scene import InteractiveSceneCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass @@ -304,6 +303,7 @@ def __post_init__(self): self.sim.dt = 0.01 self.sim.physics_material = self.scene.terrain.physics_material self.sim.render_interval = 2 # render interval should be a multiple of decimation + self.sim.device = args_cli.device # viewer settings self.viewer.eye = (5.0, 5.0, 5.0) self.viewer.lookat = (0.0, 0.0, 2.0) @@ -313,7 +313,8 @@ def main(): """Main function.""" # setup base environment - env = ManagerBasedEnv(cfg=CubeEnvCfg()) + env_cfg = CubeEnvCfg() + env = ManagerBasedEnv(cfg=env_cfg) # setup target position commands target_position = torch.rand(env.num_envs, 3, device=env.device) * 2 diff --git a/scripts/tutorials/03_envs/create_quadruped_base_env.py b/scripts/tutorials/03_envs/create_quadruped_base_env.py index 931cf378b964..78f5b75ec5f8 100644 --- a/scripts/tutorials/03_envs/create_quadruped_base_env.py +++ b/scripts/tutorials/03_envs/create_quadruped_base_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -105,7 +105,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], @@ -194,6 +194,7 @@ def __post_init__(self): # simulation settings self.sim.dt = 0.005 # simulation timestep -> 200 Hz physics self.sim.physics_material = self.scene.terrain.physics_material + self.sim.device = args_cli.device # 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: diff --git a/scripts/tutorials/03_envs/policy_inference_in_usd.py b/scripts/tutorials/03_envs/policy_inference_in_usd.py index bee12f4db6ae..f4add0f617cf 100644 --- a/scripts/tutorials/03_envs/policy_inference_in_usd.py +++ b/scripts/tutorials/03_envs/policy_inference_in_usd.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -39,6 +39,7 @@ """Rest everything follows.""" import io import os + import torch import omni @@ -56,7 +57,9 @@ def main(): policy_path = os.path.abspath(args_cli.checkpoint) file_content = omni.client.read_file(policy_path)[2] file = io.BytesIO(memoryview(file_content).tobytes()) - policy = torch.jit.load(file) + policy = torch.jit.load(file, map_location=args_cli.device) + + # setup environment env_cfg = H1RoughEnvCfg_PLAY() env_cfg.scene.num_envs = 1 env_cfg.curriculum = None @@ -65,13 +68,19 @@ def main(): terrain_type="usd", usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Simple_Warehouse/warehouse.usd", ) - env_cfg.sim.device = "cpu" - env_cfg.sim.use_fabric = False + env_cfg.sim.device = args_cli.device + if args_cli.device == "cpu": + env_cfg.sim.use_fabric = False + + # create environment env = ManagerBasedRLEnv(cfg=env_cfg) + + # run inference with the policy obs, _ = env.reset() - while simulation_app.is_running(): - action = policy(obs["policy"]) # run inference - obs, _, _, _, _ = env.step(action) + with torch.inference_mode(): + while simulation_app.is_running(): + action = policy(obs["policy"]) + obs, _, _, _, _ = env.step(action) if __name__ == "__main__": diff --git a/scripts/tutorials/03_envs/run_cartpole_rl_env.py b/scripts/tutorials/03_envs/run_cartpole_rl_env.py index a4e355e04786..eb66a744b958 100644 --- a/scripts/tutorials/03_envs/run_cartpole_rl_env.py +++ b/scripts/tutorials/03_envs/run_cartpole_rl_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -45,6 +45,7 @@ def main(): # create environment configuration env_cfg = CartpoleEnvCfg() env_cfg.scene.num_envs = args_cli.num_envs + env_cfg.sim.device = args_cli.device # setup RL environment env = ManagerBasedRLEnv(cfg=env_cfg) diff --git a/scripts/tutorials/04_sensors/add_sensors_on_robot.py b/scripts/tutorials/04_sensors/add_sensors_on_robot.py index 0cf57447d586..5cc6de6778b6 100644 --- a/scripts/tutorials/04_sensors/add_sensors_on_robot.py +++ b/scripts/tutorials/04_sensors/add_sensors_on_robot.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -84,7 +84,7 @@ class SensorsSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Robot/base", update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/defaultGroundPlane"], @@ -161,7 +161,7 @@ def main(): sim = sim_utils.SimulationContext(sim_cfg) # Set main camera sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) - # design scene + # Design scene scene_cfg = SensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) # Play the simulator diff --git a/scripts/tutorials/04_sensors/run_frame_transformer.py b/scripts/tutorials/04_sensors/run_frame_transformer.py index 3e1194b83de1..d6d12665ada9 100644 --- a/scripts/tutorials/04_sensors/run_frame_transformer.py +++ b/scripts/tutorials/04_sensors/run_frame_transformer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -33,6 +33,7 @@ """Rest everything follows.""" import math + import torch import isaacsim.util.debug_draw._debug_draw as omni_debug_draw @@ -168,7 +169,7 @@ def main(): sim = SimulationContext(sim_cfg) # Set main camera sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0]) - # Design the scene + # Design scene scene_entities = design_scene() # Play the simulator sim.reset() diff --git a/scripts/tutorials/04_sensors/run_ray_caster.py b/scripts/tutorials/04_sensors/run_ray_caster.py index 867d88a37d09..51780accbdd2 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster.py +++ b/scripts/tutorials/04_sensors/run_ray_caster.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -33,8 +33,6 @@ import torch -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.sensors.ray_caster import RayCaster, RayCasterCfg, patterns @@ -49,7 +47,7 @@ def define_sensor() -> RayCaster: prim_path="/World/Origin.*/ball", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(2.0, 2.0)), - attach_yaw_only=True, + ray_alignment="yaw", debug_vis=not args_cli.headless, ) ray_caster = RayCaster(cfg=ray_caster_cfg) @@ -71,7 +69,7 @@ def design_scene() -> dict: # 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) + sim_utils.create_prim(f"/World/Origin{i}", "Xform", translation=origin) # -- Balls cfg = RigidObjectCfg( prim_path="/World/Origin.*/ball", @@ -134,7 +132,7 @@ def main(): sim = sim_utils.SimulationContext(sim_cfg) # Set main camera sim.set_camera_view([0.0, 15.0, 15.0], [0.0, 0.0, -2.5]) - # Design the scene + # Design scene scene_entities = design_scene() # Play simulator sim.reset() diff --git a/scripts/tutorials/04_sensors/run_ray_caster_camera.py b/scripts/tutorials/04_sensors/run_ray_caster_camera.py index 651fcf5d7c0e..375a0cf8f08b 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster_camera.py +++ b/scripts/tutorials/04_sensors/run_ray_caster_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -36,9 +36,9 @@ """Rest everything follows.""" import os + import torch -import isaacsim.core.utils.prims as prim_utils import omni.replicator.core as rep import isaaclab.sim as sim_utils @@ -53,8 +53,8 @@ def define_sensor() -> RayCasterCamera: # Camera base frames # In contras to the USD camera, we associate the sensor to the prims at these locations. # This means that parent prim of the sensor is the prim at this location. - prim_utils.create_prim("/World/Origin_00/CameraSensor", "Xform") - prim_utils.create_prim("/World/Origin_01/CameraSensor", "Xform") + sim_utils.create_prim("/World/Origin_00/CameraSensor", "Xform") + sim_utils.create_prim("/World/Origin_01/CameraSensor", "Xform") # Setup camera sensor camera_cfg = RayCasterCameraCfg( @@ -163,10 +163,11 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict): def main(): """Main function.""" # Load kit helper - sim = sim_utils.SimulationContext() + sim_cfg = sim_utils.SimulationCfg() + sim = sim_utils.SimulationContext(sim_cfg) # Set main camera sim.set_camera_view([2.5, 2.5, 3.5], [0.0, 0.0, 0.0]) - # design the scene + # Design scene scene_entities = design_scene() # Play simulator sim.reset() diff --git a/scripts/tutorials/04_sensors/run_usd_camera.py b/scripts/tutorials/04_sensors/run_usd_camera.py index 90b92b9fc13e..c2462aaaec89 100644 --- a/scripts/tutorials/04_sensors/run_usd_camera.py +++ b/scripts/tutorials/04_sensors/run_usd_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -60,12 +60,12 @@ """Rest everything follows.""" -import numpy as np import os import random + +import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils import omni.replicator.core as rep import isaaclab.sim as sim_utils @@ -82,8 +82,8 @@ def define_sensor() -> Camera: # Setup camera sensor # In contrast to the ray-cast camera, we spawn the prim at these locations. # This means the camera sensor will be attached to these prims. - prim_utils.create_prim("/World/Origin_00", "Xform") - prim_utils.create_prim("/World/Origin_01", "Xform") + sim_utils.create_prim("/World/Origin_00", "Xform") + sim_utils.create_prim("/World/Origin_01", "Xform") camera_cfg = CameraCfg( prim_path="/World/Origin_.*/CameraSensor", update_period=0, @@ -124,7 +124,7 @@ def design_scene() -> dict: scene_entities = {} # Xform to hold objects - prim_utils.create_prim("/World/Objects", "Xform") + sim_utils.create_prim("/World/Objects", "Xform") # Random objects for i in range(8): # sample random position @@ -272,7 +272,7 @@ def main(): sim = sim_utils.SimulationContext(sim_cfg) # Set main camera sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) - # design the scene + # Design scene scene_entities = design_scene() # Play simulator sim.reset() diff --git a/scripts/tutorials/05_controllers/run_diff_ik.py b/scripts/tutorials/05_controllers/run_diff_ik.py index 2c1da4d9f355..22d17963235f 100644 --- a/scripts/tutorials/05_controllers/run_diff_ik.py +++ b/scripts/tutorials/05_controllers/run_diff_ik.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -160,8 +160,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): else: # obtain quantities from simulation jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids] - ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7] - root_pose_w = robot.data.root_state_w[:, 0:7] + ee_pose_w = robot.data.body_pose_w[:, robot_entity_cfg.body_ids[0]] + root_pose_w = robot.data.root_pose_w joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids] # compute frame in root frame ee_pos_b, ee_quat_b = subtract_frame_transforms( diff --git a/scripts/tutorials/05_controllers/run_osc.py b/scripts/tutorials/05_controllers/run_osc.py index fada46209050..98b2532a0a2d 100644 --- a/scripts/tutorials/05_controllers/run_osc.py +++ b/scripts/tutorials/05_controllers/run_osc.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -49,8 +49,8 @@ from isaaclab.utils.math import ( combine_frame_transforms, matrix_from_quat, + quat_apply_inverse, quat_inv, - quat_rotate_inverse, subtract_frame_transforms, ) @@ -336,8 +336,8 @@ def update_states( ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame - ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame - ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) + ee_lin_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame + ee_ang_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1) # Calculate the contact force diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index b47bd022ef46..9939dcb511b4 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.36.3" +version = "0.54.2" # Description title = "Isaac Lab framework for Robot Learning" @@ -18,7 +18,8 @@ requirements = [ "toml", "hidapi", "gymnasium==0.29.0", - "trimesh" + "trimesh", + "websockets" ] modules = [ @@ -27,7 +28,8 @@ modules = [ "toml", "hid", "gymnasium", - "trimesh" + "trimesh", + "websockets" ] use_online_index=true diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 7ab5e86c40fd..3e7c97f33545 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,19 +1,1764 @@ Changelog --------- -0.36.4 (2025-03-24) +0.54.2 (2026-01-28) ~~~~~~~~~~~~~~~~~~~ Changed ^^^^^^^ -* Definition of render settings in :class:`~isaaclab.sim.SimulationCfg` is changed to None, which means that - the default settings will be used from the experience files and the double definition is removed. +* Moved :mod:`isaaclab.sensors.tacsl_sensor` to :mod:`isaaclab_contrib.sensors.tacsl_sensor` module, + since it is not completely ready for release yet. + + +0.54.1 (2026-01-25) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added test suite for ray caster patterns with comprehensive parameterized tests. + +Fixed +^^^^^ + +* Fixed incorrect horizontal angle calculation in :func:`~isaaclab.sensors.ray_caster.patterns.patterns.lidar_pattern` + that caused the actual angular resolution to differ from the requested resolution. + + +0.54.0 (2026-01-13) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added Fabric backend support to :class:`~isaaclab.sim.views.XformPrimView` for GPU-accelerated + batch transform operations on all Boundable prims using Warp kernels. +* Added :mod:`~isaaclab.sim.utils.fabric_utils` module with Warp kernels for efficient Fabric matrix operations. + +Changed +^^^^^^^ + +* Changed :class:`~isaaclab.sensors.camera.Camera` to use Fabric backend for faster pose queries. + + +0.53.2 (2026-01-14) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.assets.utils.wrench_composer.WrenchComposer` to compose forces and torques at the body's center of mass frame. +* Added :meth:`~isaaclab.assets.Articulation.instantaneous_wrench_composer` to add or set instantaneous external wrenches to the articulation. +* Added :meth:`~isaaclab.assets.Articulation.permanent_wrench_composer` to add or set permanent external wrenches to the articulation. +* Added :meth:`~isaaclab.assets.RigidObject.instantaneous_wrench_composer` to add or set instantaneous external wrenches to the rigid object. +* Added :meth:`~isaaclab.assets.RigidObject.permanent_wrench_composer` to add or set permanent external wrenches to the rigid object. +* Added :meth:`~isaaclab.assets.RigidObjectCollection.instantaneous_wrench_composer` to add or set instantaneous external wrenches to the rigid object collection. +* Added :meth:`~isaaclab.assets.RigidObjectCollection.permanent_wrench_composer` to add or set permanent external wrenches to the rigid object collection. +* Added unit tests for the wrench composer. +* Added kernels for the wrench composer in the :mod:`isaaclab.utils.warp.kernels` module. + +Changed +^^^^^^^ + +* Deprecated :meth:`~isaaclab.assets.Articulation.set_external_force_and_torque` in favor of :meth:`~isaaclab.assets.Articulation.permanent_wrench_composer.set_forces_and_torques`. +* Deprecated :meth:`~isaaclab.assets.RigidObject.set_external_force_and_torque` in favor of :meth:`~isaaclab.assets.RigidObject.permanent_wrench_composer.set_forces_and_torques`. +* Deprecated :meth:`~isaaclab.assets.RigidObjectCollection.set_external_force_and_torque` in favor of :meth:`~isaaclab.assets.RigidObjectCollection.permanent_wrench_composer.set_forces_and_torques`. +* Modified the tests of the articulation, rigid object, and rigid object collection to use the new permanent and instantaneous external wrench functions and test them. + +0.53.1 (2026-01-08) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added function :func:`~isaaclab.sim.utils.prims.change_prim_property` to change attributes on a USD prim. + This replaces the previously used USD command ``ChangeProperty`` that depends on Omniverse Kit API. + +Changed +^^^^^^^ + +* Replaced occurrences of ``ChangeProperty`` USD command to :func:`~isaaclab.sim.utils.prims.change_prim_property`. + + +0.53.0 (2026-01-07) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.sim.views.XformPrimView` class to provide a + view of the USD Xform operations. Compared to Isaac Sim implementation, + this class optimizes several operations using USD SDF API. + +Changed +^^^^^^^ + +* Switched the sensor classes to use the :class:`~isaaclab.sim.views.XformPrimView` + class for the internal view wherever applicable. + +Removed +^^^^^^^ + +* Removed the usage of :class:`isaacsim.core.utils.prims.XformPrim` + class from the sensor classes. + + +0.52.2 (2026-01-06) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Improved logic for the URDF importer extension version pinning: the older extension version + is now pinned only on Isaac Sim 5.1 and later, while older Isaac Sim versions no longer + attempt to pin to a version that does not exist. + + +0.52.1 (2026-01-02) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed FrameTransformer body name collision when tracking bodies with the same name but different hierarchical paths + (e.g., Robot/left_hand vs Robot_1/left_hand). The sensor now uses the full prim path (with env_* patterns normalized) + as the unique body identifier instead of just the leaf body name. This ensures bodies at different hierarchy levels + are tracked separately. The change is backwards compatible: user-facing frame names still default to leaf names when + not explicitly provided, while internal body tracking uses full paths to avoid collisions. Works for both + environment-scoped paths (e.g., /World/envs/env_0/Robot) and non-environment paths (e.g., /World/Robot). + + +0.52.0 (2026-01-02) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :mod:`~isaaclab.sim.utils.transforms` module to handle USD Xform operations. +* Added passing of ``stage`` to the :func:`~isaaclab.sim.utils.prims.create_prim` function + inside spawning functions to allow for the creation of prims in a specific stage. + +Changed +^^^^^^^ + +* Changed :func:`~isaaclab.sim.utils.prims.create_prim` function to use the :mod:`~isaaclab.sim.utils.transforms` + module for USD Xform operations. It removes the usage of Isaac Sim's XformPrim class. + + +0.51.2 (2025-12-30) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :attr:`~isaaclab.managers.ObservationManager.get_active_iterable_terms` + to handle observation data when not concatenated along the last dimension. + + +0.51.1 (2025-12-29) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :func:`~isaaclab.utils.version.get_isaac_sim_version` to get the version of Isaac Sim. + This function caches the version of Isaac Sim and returns it immediately if it has already been computed. + This helps avoid parsing the VERSION file from disk multiple times. + +Changed +^^^^^^^ + +* Changed the function :meth:`~isaaclab.utils.version.compare_versions` to use :mod:`packaging.version.Version` module. +* Changed occurrences of :func:`isaacsim.core.version.get_version` to :func:`~isaaclab.utils.version.get_isaac_sim_version`. + +Removed +^^^^^^^ + +* Removed storing of Isaac Sim version inside the environment base classes defined inside + :mod:`isaaclab.envs` module. + + +0.51.0 (2025-12-29) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added tests for the :mod:`isaaclab.sim.utils.prims` module. +* Added tests for the :mod:`isaaclab.sim.utils.stage` module. +* Created :mod:`isaaclab.sim.utils.legacy` sub-module to keep deprecated functions. + +Removed +^^^^^^^ + +* Removed many unused USD prim and stage related operations from the :mod:`isaaclab.sim.utils` module. +* Moved :mod:`isaaclab.sim.utils.nucleus` sub-module to the ``tests/deps/isaacsim`` directory as it + is only being used for Isaac Sim check scripts. + +Changed +^^^^^^^ + +* Changed the organization of the :mod:`isaaclab.sim.utils` module to make it clearer which functions + are related to the stage and which are related to the prims. +* Modified imports of :mod:`~isaaclab.sim.utils.stage` and :mod:`~isaaclab.sim.utils.prims` modules + to only use the :mod:`isaaclab.sim.utils` module. +* Moved ``logger.py`` to the :mod:`isaaclab.utils` module. + + +0.50.7 (2025-12-29) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Moved ``pretrained_checkpoint.py`` to the :mod:`isaaclab_rl.utils` module. + + +0.50.6 (2025-12-18) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed issue where :meth:~isaaclab.envs.mdp.observations.body_pose_w` was modifying the original body pose data + when using slice or int for body_ids in the observation config. A clone of the data is now created to avoid modifying + the original data. + + +0.50.5 (2025-12-15) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.sensors.MultiMeshRayCaster` sensor to support tracking of dynamic meshes for ray-casting. + We keep the previous implementation of :class:`~isaaclab.sensors.RayCaster` for backwards compatibility. +* Added :mod:`isaaclab.utils.mesh` sub-module to perform various Trimesh and USD Mesh related operations. + + +0.50.4 (2025-12-15) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`~isaaclab.sim.PhysxCfg.enable_external_forces_every_iteration` to enable external forces every position + iteration. This can help improve the accuracy of velocity updates. Consider enabling this flag if the velocities + generated by the simulation are noisy. +* Added warning when :attr:`~isaaclab.sim.PhysxCfg.enable_external_forces_every_iteration` is set to False and + the solver type is TGS. + + +0.50.3 (2025-12-11) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed missing mesh collision approximation attribute when running :class:`~isaaclab.sim.converters.MeshConverter`. + The collision approximation attribute is now properly set on the USD prim when converting meshes with mesh collision + properties. + + +0.50.2 (2025-11-21) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Prevent randomizing mass to zero in :meth:`~isaaclab.envs.mdp.events.randomize_mass_by_scale` to avoid physics errors. + + +0.50.1 (2025-11-25) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed advanced indexing issue in resetting prev action + in :class:`~isaaclab.envs.mdp.actions.JointPositionToLimitsAction` . + + +0.50.0 (2025-12-8) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Implemented ability to attach an imu sensor to xform primitives in a usd file. This PR is based on work by '@GiulioRomualdi' + here: #3094 Addressing issue #3088. + + +0.49.3 (2025-12-03) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`G1TriHandUpperBodyMotionControllerGripperRetargeter` and :class:`G1TriHandUpperBodyMotionControllerGripperRetargeterCfg` for retargeting the gripper state from motion controllers. +* Added unit tests for the retargeters. + + +0.49.2 (2025-11-17) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.track_friction_forces` to toggle tracking of friction forces between sensor bodies and filtered bodies. +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorData.friction_forces_w` data field for tracking friction forces. + + +0.49.1 (2025-11-26) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed import from ``isaacsim.core.utils.prims`` to ``isaaclab.sim.utils.prims`` across repo to reduce IsaacLab dependencies. + +0.49.0 (2025-11-10) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Updated the URDF Importer version to 2.4.31 to avoid issues with merging joints on the latest URDF importer in Isaac Sim 5.1 + + +0.48.9 (2025-11-21) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Add navigation state API to IsaacLabManagerBasedRLMimicEnv +* Add optional custom recorder config to MimicEnvCfg + + +0.48.8 (2025-10-15) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`preserve_order` flag to :class:`~isaaclab.envs.mdp.actions.actions_cfg.JointPositionToLimitsActionCfg` + + +0.48.7 (2025-11-25) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed import from ``isaaclab.sim.utils`` to ``isaaclab.sim.utils.stage`` in ``isaaclab.devices.openxr.xr_anchor_utils.py`` + to properly propagate the Isaac Sim stage context. + + + + +0.48.6 (2025-11-18) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added OpenXR motion controller support for the G1 robot locomanipulation environment + ``Isaac-PickPlace-Locomanipulation-G1-Abs-v0``. This enables teleoperation using XR motion controllers + in addition to hand tracking. +* Added :class:`OpenXRDeviceMotionController` for motion controller-based teleoperation with headset anchoring control. +* Added motion controller-specific retargeters: + * :class:`G1TriHandControllerUpperBodyRetargeterCfg` for upper body and hand control using motion controllers. + * :class:`G1LowerBodyStandingControllerRetargeterCfg` for lower body control using motion controllers. + + +0.48.5 (2025-11-14) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed import from ``isaacsim.core.utils.stage`` to ``isaaclab.sim.utils.stage`` to reduce IsaacLab dependencies. + + +0.48.4 (2025-11-14) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Refactored modules related to the actuator configs in order to remediate a circular import necessary to support future + actuator drive model improvements. + + +0.48.3 (2025-11-13) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Moved retargeter and device declaration out of factory and into the devices/retargeters themselves. + + +0.48.2 (2025-11-13) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed from using :meth:`isaacsim.core.utils.torch.set_seed` to :meth:`~isaaclab.utils.seed.configure_seed` + + +0.48.1 (2025-11-10) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.devices.haply.HaplyDevice` class for SE(3) teleoperation with dual Haply Inverse3 and Versegrip devices, + supporting robot manipulation with haptic feedback. +* Added demo script ``scripts/demos/haply_teleoperation.py`` and documentation guide in + ``docs/source/how-to/haply_teleoperation.rst`` for Haply-based robot teleoperation. + + +0.48.0 (2025-11-03) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Detected contacts are reported with the threshold of 0.0 (instead of 1.0). This increases the sensitivity of contact + detection. + +Fixed +^^^^^ + +* Removed passing the boolean flag to :meth:`isaaclab.sim.schemas.activate_contact_sensors` when activating contact + sensors. This was incorrectly modifying the threshold attribute to 1.0 when contact sensors were activated. + + +0.47.11 (2025-11-03) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed the bug where effort limits were being overridden in :class:`~isaaclab.actuators.ActuatorBase` when the ``effort_limit`` parameter is set to None. +* Corrected the unit tests for three effort limit scenarios with proper assertions + + +0.47.10 (2025-11-06) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``num_rerenders_on_reset`` parameter to ManagerBasedEnvCfg and DirectRLEnvCfg to configure the number + of render steps to perform after reset. This enables more control over DLSS rendering behavior after reset. + +Changed +^^^^^^^ + +* Added deprecation warning for ``rerender_on_reset`` parameter in ManagerBasedEnv and DirectRLEnv. + + +0.47.9 (2025-11-05) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Fixed termination term bookkeeping in :class:`~isaaclab.managers.TerminationManager`: + per-step termination and last-episode termination bookkeeping are now separated. + last-episode dones are now updated once per step from all term outputs, avoiding per-term overwrites + and ensuring Episode_Termination metrics reflect the actual triggering terms. + + +0.47.8 (2025-11-06) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added parameter :attr:`~isaaclab.terrains.TerrainImporterCfg.use_terrain_origins` to allow generated sub terrains with grid origins. + + +0.47.7 (2025-10-31) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed Pink IK controller qpsolver from osqp to daqp. +* Changed Null Space matrix computation in Pink IK's Null Space Posture Task to a faster matrix pseudo inverse computation. + + +0.47.6 (2025-11-01) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed an issue in recurrent policy evaluation in RSL-RL framework where the recurrent state was not reset after an episode termination. + + +0.47.5 (2025-10-30) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Added docstrings notes to clarify the friction coefficient modeling in Isaac Sim 4.5 and 5.0. + + +0.47.4 (2025-10-30) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Enhanced :meth:`~isaaclab.managers.RecorderManager.export_episodes` method to support customizable sequence of demo IDs: + + - Added argument ``demo_ids`` to :meth:`~isaaclab.managers.RecorderManager.export_episodes` to accept a sequence of integers + for custom episode identifiers. + +* Enhanced :meth:`~isaaclab.utils.datasets.HDF5DatasetFileHandler.write_episode` method to support customizable episode identifiers: + + - Added argument ``demo_id`` to :meth:`~isaaclab.utils.datasets.HDF5DatasetFileHandler.write_episode` to accept a custom integer + for episode identifier. + + +0.47.3 (2025-10-22) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* 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.2 (2025-10-17) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :meth:`~isaaclab.sim.utils.resolve_prim_pose` to resolve the pose of a prim with respect to another prim. +* Added :meth:`~isaaclab.sim.utils.resolve_prim_scale` to resolve the scale of a prim in the world frame. + + +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 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.45.16 (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) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added action terms for using RMPFlow in Manager-Based environments. + + +0.45.14 (2025-09-08) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.ui.xr_widgets.TeleopVisualizationManager` and :class:`~isaaclab.ui.xr_widgets.XRVisualization` + classes to provide real-time visualization of teleoperation and inverse kinematics status in XR environments. + + +0.45.13 (2025-09-08) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.devices.openxr.manus_vive.ManusVive` to support teleoperation with Manus gloves and Vive trackers. + + +0.45.12 (2025-09-05) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.envs.mdp.actions.SurfaceGripperBinaryAction` for supporting surface grippers in Manager-Based workflows. + +Changed +^^^^^^^ + +* Added AssetBase inheritance for :class:`~isaaclab.assets.surface_gripper.SurfaceGripper`. + + +0.45.11 (2025-09-04) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixes a high memory usage and perf slowdown issue in episode data by removing the use of torch.cat when appending to the episode data + at each timestep. The use of torch.cat was causing the episode data to be copied at each timestep, which causes high memory usage and + significant performance slowdown when recording longer episode data. +* Patches the configclass to allow validate dict with key is not a string. + +Added +^^^^^ + +* Added optional episode metadata (ep_meta) to be stored in the HDF5 data attributes. +* Added option to record data pre-physics step. +* Added joint_target data to episode data. Joint target data can be optionally recorded by the user and replayed to improve + determinism of replay. + + +0.45.10 (2025-09-02) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed regression in reach task configuration where the gripper command was being returned. +* Added :attr:`~isaaclab.devices.Se3GamepadCfg.gripper_term` to :class:`~isaaclab.devices.Se3GamepadCfg` + to control whether the gamepad device should return a gripper command. +* Added :attr:`~isaaclab.devices.Se3SpaceMouseCfg.gripper_term` to :class:`~isaaclab.devices.Se3SpaceMouseCfg` + to control whether the spacemouse device should return a gripper command. +* Added :attr:`~isaaclab.devices.Se3KeyboardCfg.gripper_term` to :class:`~isaaclab.devices.Se3KeyboardCfg` + to control whether the keyboard device should return a gripper command. + + +0.45.9 (2025-08-27) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed removing import of pink_ik controller from isaaclab.controllers which is causing pinocchio import error. + + +0.45.8 (2025-07-25) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Created :attr:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg.target_eef_link_names` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg` + to specify the target end-effector link names for the pink inverse kinematics controller. + +Changed +^^^^^^^ + +* Updated pink inverse kinematics controller configuration for the following tasks (Isaac-PickPlace-GR1T2, Isaac-NutPour-GR1T2, Isaac-ExhaustPipe-GR1T2) + to increase end-effector tracking accuracy and speed. Also added a null-space regularizer that enables turning on of waist degrees-of-freedom. +* Improved the test_pink_ik script to more comprehensive test on controller accuracy. Also, migrated to use pytest. With the current IK controller + improvements, our unit tests pass position and orientation accuracy test within **(1 mm, 1 degree)**. Previously, the position accuracy tolerances + were set to **(30 mm, 10 degrees)**. +* Included a new config parameter :attr:`fail_on_ik_error` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg` + to control whether the IK controller raise an exception if robot joint limits are exceeded. In the case of an exception, the controller will hold the + last joint position. This adds to stability of the controller and avoids operator experiencing what is perceived as sudden large delays in robot control. + + +0.45.7 (2025-08-21) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added periodic logging when checking if a USD path exists on a Nucleus server + to improve user experience when the checks takes a while. + + +0.45.6 (2025-08-22) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab.envs.mdp.events.randomize_rigid_body_com` to broadcasts the environment ids. + + +0.45.5 (2025-08-21) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab.assets.Articulation.write_joint_friction_coefficient_to_sim` to set the friction coefficients in the simulation. +* Fixed :meth:`~isaaclab.assets.Articulation.write_joint_dynamic_friction_coefficient_to_sim` to set the friction coefficients in the simulation.* Added :meth:`~isaaclab.envs.ManagerBasedEnvCfg.export_io_descriptors` to toggle the export of the IO descriptors. +* Fixed :meth:`~isaaclab.assets.Articulation.write_joint_viscous_friction_coefficient_to_sim` to set the friction coefficients in the simulation. + + + +0.45.4 (2025-08-21) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added unit tests for :class:`~isaaclab.sensor.sensor_base` + + +0.45.3 (2025-08-20) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`isaaclab.envs.mdp.terminations.joint_effort_out_of_limit` so that it correctly reports whether a joint + effort limit has been violated. Previously, the implementation marked a violation when the applied and computed + torques were equal; in fact, equality should indicate no violation, and vice versa. + + +0.45.2 (2025-08-18) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :meth:`~isaaclab.managers.ObservationManager.get_IO_descriptors` to export the IO descriptors for the observation manager. +* Added :meth:`~isaaclab.envs.ManagerBasedEnvCfg.io_descriptors_output_dir` to configure the directory to export the IO descriptors to. +* Added :meth:`~isaaclab.envs.ManagerBasedEnvCfg.export_io_descriptors` to toggle the export of the IO descriptors. +* Added the option to export the Observation and Action of the managed environments into a YAML file. This can be used to more easily + deploy policies trained in Isaac Lab. + + +0.45.1 (2025-08-16) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added validations for scale-based randomization ranges across mass, actuator, joint, and tendon parameters. + +Changed +^^^^^^^ + +* Refactored randomization functions into classes with initialization-time checks to avoid runtime overhead. + + +0.45.0 (2025-08-07) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.track_contact_points` to toggle tracking of contact + point locations between sensor bodies and filtered bodies. +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.max_contact_data_per_prim` to configure the maximum + amount of contacts per sensor body. +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorData.contact_pos_w` data field for tracking contact point + locations. + + +0.44.12 (2025-08-12) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed IndexError in :meth:`isaaclab.envs.mdp.events.reset_joints_by_scale`, + :meth:`isaaclab.envs.mdp.events.reset_joints_by_offsets` by adding dimension to env_ids when indexing. + + +0.44.11 (2025-08-11) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed rendering preset mode when an experience CLI arg is provided. + + +0.44.10 (2025-08-06) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed the old termination manager in :class:`~isaaclab.managers.TerminationManager` term_done logging that + logs the instantaneous term done count at reset. This let to inaccurate aggregation of termination count, + obscuring the what really happening during the training. Instead we log the episodic term done. + + +0.44.9 (2025-07-30) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``from __future__ import annotations`` to manager_based_rl_mimic_env.py to fix Sphinx + doc warnings for IsaacLab Mimic docs. + + +0.44.8 (2025-07-30) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Improved handling of deprecated flag :attr:`~isaaclab.sensors.RayCasterCfg.attach_yaw_only`. + Previously, the flag was only handled if it was set to True. This led to a bug where the yaw was not accounted for + when the flag was set to False. +* Fixed the handling of interval-based events inside :class:`~isaaclab.managers.EventManager` to properly handle + their resets. Previously, only class-based events were properly handled. + + +0.44.7 (2025-07-30) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added a new argument ``is_global`` to :meth:`~isaaclab.assets.Articulation.set_external_force_and_torque`, + :meth:`~isaaclab.assets.RigidObject.set_external_force_and_torque`, and + :meth:`~isaaclab.assets.RigidObjectCollection.set_external_force_and_torque` allowing to set external wrenches + in the global frame directly from the method call rather than having to set the frame in the configuration. + +Removed +^^^^^^^^ + +* Removed :attr:`xxx_external_wrench_frame` flag from asset configuration classes in favor of direct argument + passed to the :meth:`set_external_force_and_torque` function. + + +0.44.6 (2025-07-28) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Tweak default behavior for rendering preset modes. + + +0.44.5 (2025-07-28) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`isaaclab.scene.reset_to` to properly accept None as valid argument. +* Added tests to verify that argument types. + + +0.44.4 (2025-07-22) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added safe callbacks for stage in memory attaching. +* Remove on prim deletion callback workaround + + +0.44.3 (2025-07-21) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed rendering preset mode regression. + + +0.44.2 (2025-07-22) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated teleop scripts to print to console vs omni log. + + +0.44.1 (2025-07-17) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated test_pink_ik.py test case to pytest format. + + +0.44.0 (2025-07-21) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed the way clipping is handled for :class:`~isaaclab.actuator.DCMotor` for torque-speed points in when in + negative power regions. + +Added +^^^^^ + +* Added unit tests for :class:`~isaaclab.actuator.ImplicitActuator`, :class:`~isaaclab.actuator.IdealPDActuator`, + and :class:`~isaaclab.actuator.DCMotor` independent of :class:`~isaaclab.assets.Articulation` + + +0.43.0 (2025-07-21) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updates torch version to 2.7.0 and torchvision to 0.22.0. + Some dependencies now require torch>=2.6, and given the vulnerabilities in Torch 2.5.1, + we are updating the torch version to 2.7.0 to also include Blackwell support. Since Isaac Sim 4.5 has not updated the + torch version, we are now overwriting the torch installation step in isaaclab.sh when running ``./isaaclab.sh -i``. + + +0.42.26 (2025-06-29) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added MangerBasedRLEnv support for composite gym observation spaces. +* A test for the composite gym observation spaces in ManagerBasedRLEnv is added to ensure that the observation spaces + are correctly configured base on the clip. + + +0.42.25 (2025-07-11) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`~isaaclab.sensors.ContactSensorData.force_matrix_w_history` that tracks the history of the filtered + contact forces in the world frame. + + +0.42.24 (2025-06-25) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added new curriculum mdp :func:`~isaaclab.envs.mdp.curriculums.modify_env_param` and + :func:`~isaaclab.envs.mdp.curriculums.modify_env_param` that enables flexible changes to any configurations in the + env instance + + +0.42.23 (2025-07-11) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`isaaclab.envs.mdp.events.reset_joints_by_scale`, :meth:`isaaclab.envs.mdp.events.reset_joints_by_offsets` + restricting the resetting joint indices be that user defined joint indices. + + +0.42.22 (2025-07-11) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed missing attribute in :class:`~isaaclab.sensors.ray_caster.RayCasterCamera` class and its reset method when no + env_ids are passed. + + +0.42.21 (2025-07-09) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added input param ``update_history`` to :meth:`~isaaclab.managers.ObservationManager.compute` + to control whether the history buffer should be updated. +* Added unit test for :class:`~isaaclab.envs.ManagerBasedEnv`. + +Fixed +^^^^^ + +* Fixed :class:`~isaaclab.envs.ManagerBasedEnv` and :class:`~isaaclab.envs.ManagerBasedRLEnv` to not update the history + buffer on recording. + + +0.42.20 (2025-07-10) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added unit tests for multiple math functions: + :func:`~isaaclab.utils.math.scale_transform`. + :func:`~isaaclab.utils.math.unscale_transform`. + :func:`~isaaclab.utils.math.saturate`. + :func:`~isaaclab.utils.math.normalize`. + :func:`~isaaclab.utils.math.copysign`. + :func:`~isaaclab.utils.math.convert_quat`. + :func:`~isaaclab.utils.math.quat_conjugate`. + :func:`~isaaclab.utils.math.quat_from_euler_xyz`. + :func:`~isaaclab.utils.math.quat_from_matrix`. + :func:`~isaaclab.utils.math.euler_xyz_from_quat`. + :func:`~isaaclab.utils.math.matrix_from_euler`. + :func:`~isaaclab.utils.math.quat_from_angle_axis`. + :func:`~isaaclab.utils.math.axis_angle_from_quat`. + :func:`~isaaclab.utils.math.skew_symmetric_matrix`. + :func:`~isaaclab.utils.math.combine_transform`. + :func:`~isaaclab.utils.math.subtract_transform`. + :func:`~isaaclab.utils.math.compute_pose_error`. + +Changed +^^^^^^^ + +* Changed the implementation of :func:`~isaaclab.utils.math.copysign` to better reflect the documented functionality. + + +0.42.19 (2025-07-09) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Added clone_in_fabric config flag to :class:`~isaaclab.scene.interactive_scene_cfg.InteractiveSceneCfg` +* Enable clone_in_fabric for envs which work with limited benchmark_non_rl.py script + + +0.42.18 (2025-07-07) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed texture and color randomization to use new replicator functional APIs. + + +0.42.17 (2025-07-08) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed hanging quat_rotate calls to point to quat_apply in :class:`~isaaclab.assets.articulation.ArticulationData` and + :class:`~isaaclab.assets.articulation.RigidObjectCollectionData` + + +0.42.16 (2025-07-08) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ability to set platform height independent of object height for trimesh terrains. + + +0.42.15 (2025-07-01) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`abs_height_noise` and :attr:`rel_height_noise` to give minimum and maximum absolute and relative noise to + :class:`isaaclab.terrrains.trimesh.MeshRepeatedObjectsTerrainCfg` +* Added deprecation warnings to the existing :attr:`max_height_noise` but still functions. + + +0.42.14 (2025-07-03) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed unittest tests that are floating inside pytests for articulation and rendering + + +0.42.13 (2025-07-07) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated gymnasium to v1.2.0. This update includes fixes for a memory leak that appears when recording + videos with the ``--video`` flag. + + +0.42.12 (2025-06-27) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added unit test for :func:`~isaaclab.utils.math.quat_inv`. + +Fixed +^^^^^ + +* Fixed the implementation mistake in :func:`~isaaclab.utils.math.quat_inv`. + + +0.42.11 (2025-06-25) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :func:`~isaaclab.utils.dict.update_class_from_dict` preventing setting flat Iterables with different lengths. + + +0.42.10 (2025-06-25) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``sample_bias_per_component`` flag to :class:`~isaaclab.utils.noise.noise_model.NoiseModelWithAdditiveBias` + to enable independent per-component bias sampling, which is now the default behavior. If set to False, the previous + behavior of sharing the same bias value across all components is retained. + + +0.42.9 (2025-06-18) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed data inconsistency between read_body, read_link, read_com when write_body, write_com, write_joint performed, in + :class:`~isaaclab.assets.Articulation`, :class:`~isaaclab.assets.RigidObject`, and + :class:`~isaaclab.assets.RigidObjectCollection` +* added pytest that check against these data consistencies + + +0.42.8 (2025-06-24) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* :class:`~isaaclab.utils.noise.NoiseModel` support for manager-based workflows. + +Changed +^^^^^^^ + +* Renamed :func:`~isaaclab.utils.noise.NoiseModel.apply` method to :func:`~isaaclab.utils.noise.NoiseModel.__call__`. + + +0.42.7 (2025-06-12) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed potential issues in :func:`~isaaclab.envs.mdp.events.randomize_visual_texture_material` related to handling + visual prims during texture randomization. + + +0.42.6 (2025-06-11) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Remove deprecated usage of quat_rotate from articulation data class and replace with quat_apply. + + +0.42.5 (2025-05-22) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed collision filtering logic for CPU simulation. The automatic collision filtering feature + currently has limitations for CPU simulation. Collision filtering needs to be manually enabled when using + CPU simulation. + + +0.42.4 (2025-06-03) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Removes the hardcoding to :class:`~isaaclab.terrains.terrain_generator.TerrainGenerator` in + :class:`~isaaclab.terrains.terrain_generator.TerrainImporter` and instead the ``class_type`` is used which is + passed in the ``TerrainGeneratorCfg``. + + +0.42.3 (2025-03-20) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Made separate data buffers for poses and velocities for the :class:`~isaaclab.assets.Articulation`, + :class:`~isaaclab.assets.RigidObject`, and :class:`~isaaclab.assets.RigidObjectCollection` classes. + Previously, the two data buffers were stored together in a single buffer requiring an additional + concatenation operation when accessing the data. +* Cleaned up ordering of members inside the data classes for the assets to make them easier + to comprehend. This reduced the code duplication within the class and made the class + more readable. + + +0.42.2 (2025-05-31) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Updated gymnasium to >= 1.0 +* Added support for specifying module:task_name as task name to avoid module import for ``gym.make`` + + +0.42.1 (2025-06-02) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added time observation functions to ~isaaclab.envs.mdp.observations module, + :func:`~isaaclab.envs.mdp.observations.current_time_s` and :func:`~isaaclab.envs.mdp.observations.remaining_time_s`. + +Changed +^^^^^^^ + +* Moved initialization of ``episode_length_buf`` outside of :meth:`load_managers()` of + :class:`~isaaclab.envs.ManagerBasedRLEnv` to make it available for mdp functions. + + +0.42.0 (2025-06-02) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added support for stage in memory and cloning in fabric. This will help improve performance for scene setup and lower + overall startup time. + + +0.41.0 (2025-05-19) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added simulation schemas for spatial tendons. These can be configured for assets imported + from file formats. +* Added support for spatial tendons. + + +0.40.14 (2025-05-29) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added deprecation warning for :meth:`~isaaclab.utils.math.quat_rotate` and + :meth:`~isaaclab.utils.math.quat_rotate_inverse` + +Changed +^^^^^^^ + +* Changed all calls to :meth:`~isaaclab.utils.math.quat_rotate` and :meth:`~isaaclab.utils.math.quat_rotate_inverse` to + :meth:`~isaaclab.utils.math.quat_apply` and :meth:`~isaaclab.utils.math.quat_apply_inverse` for speed. + + +0.40.13 (2025-05-19) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Raising exceptions in step, render and reset if they occurred inside the initialization callbacks + of assets and sensors.used from the experience files and the double definition is removed. + + +0.40.12 (2025-01-30) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added method :meth:`omni.isaac.lab.assets.AssetBase.set_visibility` to set the visibility of the asset + in the simulation. + + +0.40.11 (2025-05-16) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added support for concatenation of observations along different dimensions in + :class:`~isaaclab.managers.observation_manager.ObservationManager`. + +Changed +^^^^^^^ + +* Updated the :class:`~isaaclab.managers.command_manager.CommandManager` to update the command counter after the + resampling call. + + +0.40.10 (2025-05-16) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed penetration issue for negative border height in :class:`~isaaclab.terrains.terrain_generator.TerrainGeneratorCfg`. + + +0.40.9 (2025-05-20) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed the implementation of :meth:`~isaaclab.utils.math.quat_box_minus` + +Added +^^^^^ + +* Added :meth:`~isaaclab.utils.math.quat_box_plus` +* Added :meth:`~isaaclab.utils.math.rigid_body_twist_transform` + + +0.40.8 (2025-05-15) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`omni.isaac.lab.sensors.camera.camera.Camera.set_intrinsic_matrices` preventing setting of unused USD + camera parameters. +* Fixed :meth:`omni.isaac.lab.sensors.camera.camera.Camera._update_intrinsic_matrices` preventing unused USD camera + parameters from being used to calculate :attr:`omni.isaac.lab.sensors.camera.CameraData.intrinsic_matrices` +* Fixed :meth:`omni.isaac.lab.spawners.sensors.sensors_cfg.PinholeCameraCfg.from_intrinsic_matrix` preventing setting of + unused USD camera parameters. + + +0.40.7 (2025-05-14) +~~~~~~~~~~~~~~~~~~~ + +* Added a new attribute :attr:`articulation_root_prim_path` to the :class:`~isaaclab.assets.ArticulationCfg` class + to allow explicitly specifying the prim path of the articulation root. + + +0.40.6 (2025-05-14) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Enabled external cameras in XR. + + +0.40.5 (2025-05-23) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added feature for animation recording through baking physics operations into OVD files. + + +0.40.4 (2025-05-17) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed livestreaming options to use ``LIVESTREAM=1`` for WebRTC over public networks and ``LIVESTREAM=2`` for WebRTC over private networks. + + +0.40.3 (2025-05-20) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Made modifications to :func:`isaaclab.envs.mdp.image` to handle image normalization for normal maps. + + +0.40.2 (2025-05-14) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Refactored remove_camera_configs to be a function that can be used in the record_demos and teleop scripts. + + +0.40.1 (2025-05-14) +~~~~~~~~~~~~~~~~~~~ +Fixed +^^^^^ + +* Fixed spacemouse device add callback function to work with record_demos/teleop_se3_agent scripts. + + +0.40.0 (2025-05-03) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added check in RecorderManager to ensure that the success indicator is only set if the termination manager is present. +* Added semantic tags in :func:`isaaclab.sim.spawners.from_files.spawn_ground_plane`. + This allows for :attr:`semantic_segmentation_mapping` to be used when using the ground plane spawner. + + +0.39.0 (2025-04-01) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ -0.36.3 (2025-03-17) +* Added the :meth:`~isaaclab.env.mdp.observations.joint_effort` + + +0.38.0 (2025-04-01) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :meth:`~isaaclab.envs.mdp.observations.body_pose_w` +* Added :meth:`~isaaclab.envs.mdp.observations.body_projected_gravity_b` + + +0.37.5 (2025-05-12) ~~~~~~~~~~~~~~~~~~~ +Added +^^^^^ + +* Added a new teleop configuration class :class:`~isaaclab.devices.DevicesCfg` to support multiple teleoperation + devices declared in the environment configuration file. +* Implemented a factory function to create teleoperation devices based on the device configuration. + + +0.37.4 (2025-05-12) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Remove isaacsim.xr.openxr from openxr experience file. +* Use Performance AR profile for XR rendering. + + +0.37.3 (2025-05-08) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Updated PINK task space action to record processed actions. +* Added new recorder term for recording post step processed actions. + + +0.37.2 (2025-05-06) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Migrated OpenXR device to use the new OpenXR handtracking API from omni.kit.xr.core. + + +0.37.1 (2025-05-05) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Removed xr rendering mode. + + +0.37.0 (2025-04-24) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated pytorch to latest 2.7.0 with cuda 12.8 for Blackwell support. + Torch is now installed as part of the isaaclab.sh/bat scripts to ensure the correct version is installed. +* Removed :attr:`~isaaclab.sim.spawners.PhysicsMaterialCfg.improve_patch_friction` as it has been deprecated and removed from the simulation. + The simulation will always behave as if this attribute is set to true. + + +0.36.23 (2025-04-24) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed ``return_latest_camera_pose`` option in :class:`~isaaclab.sensors.TiledCameraCfg` from not being used to the + argument ``update_latest_camera_pose`` in :class:`~isaaclab.sensors.CameraCfg` with application in both + :class:`~isaaclab.sensors.Camera` and :class:`~isaaclab.sensors.TiledCamera`. + + +0.36.22 (2025-04-23) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^^^ + +* Adds correct type check for ManagerTermBase class in event_manager.py. + + +0.36.21 (2025-04-15) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Removed direct call of qpsovlers library from pink_ik controller and changed solver from quadprog to osqp. + + +0.36.20 (2025-04-09) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Added call to set cuda device after each ``app.update()`` call in :class:`~isaaclab.sim.SimulationContext`. + This is now required for multi-GPU workflows because some underlying logic in ``app.update()`` is modifying + the cuda device, which results in NCCL errors on distributed setups. + + +0.36.19 (2025-04-01) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Added check in RecorderManager to ensure that the success indicator is only set if the termination manager is present. + + +0.36.18 (2025-03-26) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added a dynamic text instruction widget that provides real-time feedback + on the number of successful recordings during demonstration sessions. + + +0.36.17 (2025-03-26) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Added override in AppLauncher to apply patch for ``pxr.Gf.Matrix4d`` to work with Pinocchio 2.7.0. + + +0.36.16 (2025-03-25) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Modified rendering mode default behavior when the launcher arg :attr:`enable_cameras` is not set. + + +0.36.15 (2025-03-25) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added near plane distance configuration for XR device. + + +0.36.14 (2025-03-24) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed default render settings in :class:`~isaaclab.sim.SimulationCfg` to None, which means that + the default settings will be used from the experience files and the double definition is removed. + + +0.36.13 (2025-03-24) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added headpose support to OpenXRDevice. + + +0.36.12 (2025-03-19) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added parameter to show warning if Pink IK solver fails to find a solution. + + +0.36.11 (2025-03-19) +~~~~~~~~~~~~~~~~~~~~ + Fixed ^^^^^ @@ -21,17 +1766,100 @@ Fixed :attr:`effort_limit` is set. -0.36.2 (2025-03-12) +0.36.10 (2025-03-17) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* App launcher to update the cli arguments if conditional defaults are used. + + +0.36.9 (2025-03-18) ~~~~~~~~~~~~~~~~~~~ Added +^^^^^^^ + +* Xr rendering mode, which is default when xr is used. + + +0.36.8 (2025-03-17) +~~~~~~~~~~~~~~~~~~~ + +Fixed ^^^^^ -* Added a new event mode called "prestartup", which gets called right after the scene design is complete - and before the simulation is played. -* Added a callback to resolve the scene entity configurations separately once the simulation plays, - since the scene entities cannot be resolved before the simulation starts playing - (as we currently rely on PhysX to provide us with the joint/body ordering) +* Removed ``scalar_first`` from scipy function usage to support older versions of scipy. + + +0.36.7 (2025-03-14) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Changed the import structure to only import ``pinocchio`` when ``pink-ik`` or ``dex-retargeting`` is being used. + This also solves for the problem that ``pink-ik`` and ``dex-retargeting`` are not supported in windows. +* Removed ``isaacsim.robot_motion.lula`` and ``isaacsim.robot_motion.motion_generation`` from the default loaded Isaac Sim extensions. +* Moved pink ik action config to a separate file. + + +0.36.6 (2025-03-13) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Worked around an issue where the render mode is set to ``"RayTracedLighting"`` instead of ``"RaytracedLighting"`` by + some dependencies. + + +0.36.5 (2025-03-11) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^^^ + +* Added 3 rendering mode presets: performance, balanced, and quality. +* Preset settings are stored in ``apps/rendering_modes``. +* Presets can be set with cli arg ``--rendering_mode`` or with :class:`RenderCfg`. +* Preset rendering settings can be overwritten with :class:`RenderCfg`. +* :class:`RenderCfg` supports all native RTX carb settings. + +Changed +^^^^^^^ +* :class:`RenderCfg` default settings are unset. + + +0.36.4 (2025-03-11) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated the OpenXR kit file ``isaaclab.python.xr.openxr.kit`` to inherit from ``isaaclab.python.kit`` instead of + ``isaaclab.python.rendering.kit`` which is not appropriate. + + +0.36.3 (2025-03-10) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Added the PinkIKController controller class that interfaces Isaac Lab with the Pink differential inverse kinematics solver + to allow control of multiple links in a robot using a single solver. + + +0.36.2 (2025-03-07) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Allowed users to exit on 1 Ctrl+C instead of consecutive 2 key strokes. +* Allowed physics reset during simulation through :meth:`reset` in :class:`~isaaclab.sim.SimulationContext`. 0.36.1 (2025-03-10) @@ -98,19 +1926,44 @@ Changed * ``set_fixed_tendon_limit`` โ†’ ``set_fixed_tendon_position_limit`` -0.34.9 (2025-03-04) -~~~~~~~~~~~~~~~~~~~ +0.34.13 (2025-03-06) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added a new event mode called "prestartup", which gets called right after the scene design is complete + and before the simulation is played. +* Added a callback to resolve the scene entity configurations separately once the simulation plays, + since the scene entities cannot be resolved before the simulation starts playing + (as we currently rely on PhysX to provide us with the joint/body ordering) + + +0.34.12 (2025-03-06) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Updated the mimic API :meth:`target_eef_pose_to_action` in :class:`isaaclab.envs.ManagerBasedRLMimicEnv` to take a dictionary of + eef noise values instead of a single noise value. +* Added support for optional subtask constraints based on DexMimicGen to the mimic configuration class :class:`isaaclab.envs.MimicEnvCfg`. +* Enabled data compression in HDF5 dataset file handler :class:`isaaclab.utils.datasets.hdf5_dataset_file_handler.HDF5DatasetFileHandler`. + + +0.34.11 (2025-03-04) +~~~~~~~~~~~~~~~~~~~~ Fixed ^^^^^ -* Fixed issue in :class:`~isaaclab.sensors.TiledCamera` where segmentation outputs only display the first tile +* Fixed issue in :class:`~isaaclab.sensors.TiledCamera` and :class:`~isaaclab.sensors.Camera` where segmentation outputs only display the first tile when scene instancing is enabled. A workaround is added for now to disable instancing when segmentation outputs are requested. -0.34.8 (2025-03-04) -~~~~~~~~~~~~~~~~~~~ +0.34.10 (2025-03-04) +~~~~~~~~~~~~~~~~~~~~ Fixed ^^^^^ @@ -119,7 +1972,7 @@ Fixed with other modalities such as RGBA and depth. -0.34.7 (2025-03-04) +0.34.9 (2025-03-04) ~~~~~~~~~~~~~~~~~~~ Added @@ -131,7 +1984,7 @@ Added which didn't allow setting the joint position and velocity separately. -0.34.6 (2025-03-02) +0.34.8 (2025-03-02) ~~~~~~~~~~~~~~~~~~~ Fixed @@ -142,7 +1995,7 @@ Fixed was always set to False, which led to incorrect contact sensor settings for the spawned assets. -0.34.5 (2025-03-02) +0.34.7 (2025-03-02) ~~~~~~~~~~~~~~~~~~~ Changed @@ -160,7 +2013,7 @@ Removed * Removed the attribute ``disable_contact_processing`` from :class:`~isaaclab.sim.SimulationContact`. -0.34.4 (2025-03-01) +0.34.6 (2025-03-01) ~~~~~~~~~~~~~~~~~~~ Added @@ -189,7 +2042,7 @@ Changed they should use the :attr:`isaaclab.actuators.ImplicitActuatorCfg.velocity_limit_sim` attribute instead. -0.34.3 (2025-02-28) +0.34.5 (2025-02-28) ~~~~~~~~~~~~~~~~~~~ Added @@ -200,6 +2053,26 @@ Added Support for other Isaac Sim builds will become available in Isaac Sim 5.0. +0.34.4 (2025-02-27) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Refactored retargeting code from Se3Handtracking class into separate modules for better modularity +* Added scaffolding for developing additional retargeters (e.g. dex) + + +0.34.3 (2025-02-26) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Enablec specifying the placement of the simulation when viewed in an XR device. This is achieved by + adding an ``XrCfg`` environment configuration with ``anchor_pos`` and ``anchor_rot`` parameters. + + 0.34.2 (2025-02-21) ~~~~~~~~~~~~~~~~~~~ @@ -462,7 +2335,12 @@ Changed Changed ^^^^^^^ -* Previously, physx returns the rigid bodies and articulations velocities in the com of bodies rather than the link frame, while poses are in link frames. We now explicitly provide :attr:`body_link_state` and :attr:`body_com_state` APIs replacing the previous :attr:`body_state` API. Previous APIs are now marked as deprecated. Please update any code using the previous pose and velocity APIs to use the new ``*_link_*`` or ``*_com_*`` APIs in :attr:`isaaclab.assets.RigidBody`, :attr:`isaaclab.assets.RigidBodyCollection`, and :attr:`isaaclab.assets.Articulation`. +* Previously, physx returns the rigid bodies and articulations velocities in the com of bodies rather than the + link frame, while poses are in link frames. We now explicitly provide :attr:`body_link_state` and + :attr:`body_com_state` APIs replacing the previous :attr:`body_state` API. Previous APIs are now marked as + deprecated. Please update any code using the previous pose and velocity APIs to use the new + ``*_link_*`` or ``*_com_*`` APIs in :attr:`isaaclab.assets.RigidBody`, + :attr:`isaaclab.assets.RigidBodyCollection`, and :attr:`isaaclab.assets.Articulation`. 0.31.0 (2024-12-16) @@ -480,8 +2358,9 @@ Added Fixed ^^^^^ -* Fixed ordering of logging and resamping in the command manager, where we were logging the metrics after resampling the commands. - This leads to incorrect logging of metrics when inside the resample call, the metrics tensors get reset. +* Fixed ordering of logging and resamping in the command manager, where we were logging the metrics + after resampling the commands. This leads to incorrect logging of metrics when inside the resample call, + the metrics tensors get reset. 0.30.2 (2024-12-16) @@ -507,8 +2386,9 @@ Added Changed ^^^^^^^ -* Added call to update articulation kinematics after reset to ensure states are updated for non-rendering sensors. Previously, some changes - in reset such as modifying joint states would not be reflected in the rigid body states immediately after reset. +* Added call to update articulation kinematics after reset to ensure states are updated for non-rendering sensors. + Previously, some changes in reset such as modifying joint states would not be reflected in the rigid body + states immediately after reset. 0.30.0 (2024-12-15) @@ -519,13 +2399,15 @@ Added * Added UI interface to the Managers in the ManagerBasedEnv and MangerBasedRLEnv classes. * Added UI widgets for :class:`LiveLinePlot` and :class:`ImagePlot`. -* Added ``ManagerLiveVisualizer/Cfg``: Given a ManagerBase (i.e. action_manager, observation_manager, etc) and a config file this class creates - the the interface between managers and the UI. -* Added :class:`EnvLiveVisualizer`: A 'manager' of ManagerLiveVisualizer. This is added to the ManagerBasedEnv but is only called during - the initialization of the managers in load_managers -* Added ``get_active_iterable_terms`` implementation methods to ActionManager, ObservationManager, CommandsManager, CurriculumManager, - RewardManager, and TerminationManager. This method exports the active term data and labels for each manager and is called by ManagerLiveVisualizer. -* Additions to :class:`BaseEnvWindow` and :class:`RLEnvWindow` to register ManagerLiveVisualizer UI interfaces for the chosen managers. +* Added ``ManagerLiveVisualizer/Cfg``: Given a ManagerBase (i.e. action_manager, observation_manager, etc) and a + config file this class creates the the interface between managers and the UI. +* Added :class:`EnvLiveVisualizer`: A 'manager' of ManagerLiveVisualizer. This is added to the ManagerBasedEnv + but is only called during the initialization of the managers in load_managers +* Added ``get_active_iterable_terms`` implementation methods to ActionManager, ObservationManager, CommandsManager, + CurriculumManager, RewardManager, and TerminationManager. This method exports the active term data and labels + for each manager and is called by ManagerLiveVisualizer. +* Additions to :class:`BaseEnvWindow` and :class:`RLEnvWindow` to register ManagerLiveVisualizer UI interfaces + for the chosen managers. 0.29.0 (2024-12-15) @@ -565,8 +2447,8 @@ Fixed ^^^^^ * Fixed the shape of ``quat_w`` in the ``apply_actions`` method of :attr:`~isaaclab.env.mdp.NonHolonomicAction` - (previously (N,B,4), now (N,4) since the number of root bodies B is required to be 1). Previously ``apply_actions`` errored - because ``euler_xyz_from_quat`` requires inputs of shape (N,4). + (previously (N,B,4), now (N,4) since the number of root bodies B is required to be 1). Previously ``apply_actions`` + errored because ``euler_xyz_from_quat`` requires inputs of shape (N,4). 0.28.1 (2024-12-13) @@ -575,7 +2457,8 @@ Fixed Fixed ^^^^^ -* Fixed the internal buffers for ``set_external_force_and_torque`` where the buffer values would be stale if zero values are sent to the APIs. +* Fixed the internal buffers for ``set_external_force_and_torque`` where the buffer values would be stale if zero + values are sent to the APIs. 0.28.0 (2024-12-12) @@ -584,8 +2467,8 @@ Fixed Changed ^^^^^^^ -* Adapted the :class:`~isaaclab.sim.converters.UrdfConverter` to use the latest URDF converter API from Isaac Sim 4.5. The - physics articulation root can now be set separately, and the joint drive gains can be set on a per joint basis. +* Adapted the :class:`~isaaclab.sim.converters.UrdfConverter` to use the latest URDF converter API from Isaac Sim 4.5. + The physics articulation root can now be set separately, and the joint drive gains can be set on a per joint basis. 0.27.33 (2024-12-11) @@ -594,9 +2477,11 @@ Changed Added ^^^^^ -* Introduced an optional ``sensor_cfg`` parameter to the :meth:`~isaaclab.envs.mdp.rewards.base_height_l2` function, enabling the use of - :class:`~isaaclab.sensors.RayCaster` for height adjustments. For flat terrains, the function retains its previous behavior. -* Improved documentation to clarify the usage of the :meth:`~isaaclab.envs.mdp.rewards.base_height_l2` function in both flat and rough terrain settings. +* Introduced an optional ``sensor_cfg`` parameter to the :meth:`~isaaclab.envs.mdp.rewards.base_height_l2` function, + enabling the use of :class:`~isaaclab.sensors.RayCaster` for height adjustments. For flat terrains, the function + retains its previous behavior. +* Improved documentation to clarify the usage of the :meth:`~isaaclab.envs.mdp.rewards.base_height_l2` function in + both flat and rough terrain settings. 0.27.32 (2024-12-11) @@ -616,11 +2501,13 @@ Changed ^^^^^^^ * Introduced configuration options in :class:`Se3HandTracking` to: + - Zero out rotation around the x/y axes - Apply smoothing and thresholding to position and rotation deltas for reduced jitter - Use wrist-based rotation reference as an alternative to fingertip-based rotation -* Switched the default position reference in :class:`Se3HandTracking` to the wrist joint pose, providing more stable relative-based positioning. +* Switched the default position reference in :class:`Se3HandTracking` to the wrist joint pose, providing more stable + relative-based positioning. 0.27.30 (2024-12-09) @@ -708,8 +2595,10 @@ Fixed Fixed ^^^^^ -* Added the attributes :attr:`~isaaclab.envs.DirectRLEnvCfg.wait_for_textures` and :attr:`~isaaclab.envs.ManagerBasedEnvCfg.wait_for_textures` - to enable assets loading check during :class:`~isaaclab.DirectRLEnv` and :class:`~isaaclab.ManagerBasedEnv` reset method when rtx sensors are added to the scene. +* Added the attributes :attr:`~isaaclab.envs.DirectRLEnvCfg.wait_for_textures` and + :attr:`~isaaclab.envs.ManagerBasedEnvCfg.wait_for_textures` to enable assets loading check + during :class:`~isaaclab.DirectRLEnv` and :class:`~isaaclab.ManagerBasedEnv` reset method when + rtx sensors are added to the scene. 0.27.22 (2024-12-04) @@ -718,7 +2607,8 @@ Fixed Fixed ^^^^^ -* Fixed the order of the incoming parameters in :class:`isaaclab.envs.DirectMARLEnv` to correctly use ``NoiseModel`` in marl-envs. +* Fixed the order of the incoming parameters in :class:`isaaclab.envs.DirectMARLEnv` to correctly use + ``NoiseModel`` in marl-envs. 0.27.21 (2024-12-04) @@ -742,7 +2632,8 @@ Added Changed ^^^^^^^ -* Changed :class:`isaaclab.envs.DirectMARLEnv` to inherit from ``Gymnasium.Env`` due to requirement from Gymnasium v1.0.0 requiring all environments to be a subclass of ``Gymnasium.Env`` when using the ``make`` interface. +* Changed :class:`isaaclab.envs.DirectMARLEnv` to inherit from ``Gymnasium.Env`` due to requirement from Gymnasium + v1.0.0 requiring all environments to be a subclass of ``Gymnasium.Env`` when using the ``make`` interface. 0.27.19 (2024-12-02) @@ -770,7 +2661,8 @@ Changed Added ^^^^^ -* Added ``create_new_stage`` setting in :class:`~isaaclab.app.AppLauncher` to avoid creating a default new stage on startup in Isaac Sim. This helps reduce the startup time when launching Isaac Lab. +* Added ``create_new_stage`` setting in :class:`~isaaclab.app.AppLauncher` to avoid creating a default new + stage on startup in Isaac Sim. This helps reduce the startup time when launching Isaac Lab. 0.27.16 (2024-11-15) @@ -788,7 +2680,8 @@ Added Fixed ^^^^^ -* Fixed indexing in :meth:`isaaclab.assets.Articulation.write_joint_limits_to_sim` to correctly process non-None ``env_ids`` and ``joint_ids``. +* Fixed indexing in :meth:`isaaclab.assets.Articulation.write_joint_limits_to_sim` to correctly process + non-None ``env_ids`` and ``joint_ids``. 0.27.14 (2024-10-23) @@ -877,8 +2770,10 @@ Added Fixed ^^^^^ -* Fixed usage of ``meshes`` property in :class:`isaaclab.sensors.RayCasterCamera` to use ``self.meshes`` instead of the undefined ``RayCaster.meshes``. -* Fixed issue in :class:`isaaclab.envs.ui.BaseEnvWindow` where undefined configs were being accessed when creating debug visualization elements in UI. +* Fixed usage of ``meshes`` property in :class:`isaaclab.sensors.RayCasterCamera` to use ``self.meshes`` + instead of the undefined ``RayCaster.meshes``. +* Fixed issue in :class:`isaaclab.envs.ui.BaseEnvWindow` where undefined configs were being accessed when + creating debug visualization elements in UI. 0.27.5 (2024-10-25) @@ -896,7 +2791,8 @@ Added Fixed ^^^^^ -* Updated installation path instructions for Windows in the Isaac Lab documentation to remove redundancy in the use of %USERPROFILE% for path definitions. +* Updated installation path instructions for Windows in the Isaac Lab documentation to remove redundancy in the + use of %USERPROFILE% for path definitions. 0.27.3 (2024-10-22) @@ -915,7 +2811,8 @@ Fixed Added ^^^^^ -* Added ``--kit_args`` to :class:`~isaaclab.app.AppLauncher` to allow passing command line arguments directly to Omniverse Kit SDK. +* Added ``--kit_args`` to :class:`~isaaclab.app.AppLauncher` to allow passing command line arguments directly to + Omniverse Kit SDK. 0.27.1 (2024-10-20) @@ -954,8 +2851,10 @@ Added * Added Imu sensor implementation that directly accesses the physx view :class:`isaaclab.sensors.Imu`. The sensor comes with a configuration class :class:`isaaclab.sensors.ImuCfg` and data class :class:`isaaclab.sensors.ImuData`. -* Moved and renamed :meth:`isaaclab.sensors.camera.utils.convert_orientation_convention` to :meth:`isaaclab.utils.math.convert_camera_frame_orientation_convention` -* Moved :meth:`isaaclab.sensors.camera.utils.create_rotation_matrix_from_view` to :meth:`isaaclab.utils.math.create_rotation_matrix_from_view` +* Moved and renamed :meth:`isaaclab.sensors.camera.utils.convert_orientation_convention` to + :meth:`isaaclab.utils.math.convert_camera_frame_orientation_convention` +* Moved :meth:`isaaclab.sensors.camera.utils.create_rotation_matrix_from_view` to + :meth:`isaaclab.utils.math.create_rotation_matrix_from_view` 0.25.2 (2024-10-16) @@ -3763,8 +5662,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/__init__.py b/source/isaaclab/isaaclab/__init__.py index 4b98e0125900..73bd3e99d3ab 100644 --- a/source/isaaclab/isaaclab/__init__.py +++ b/source/isaaclab/isaaclab/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/actuators/__init__.py b/source/isaaclab/isaaclab/actuators/__init__.py index 12b1909f4b70..db7d36b00a5e 100644 --- a/source/isaaclab/isaaclab/actuators/__init__.py +++ b/source/isaaclab/isaaclab/actuators/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -23,15 +23,14 @@ """ from .actuator_base import ActuatorBase -from .actuator_cfg import ( - ActuatorBaseCfg, - ActuatorNetLSTMCfg, - ActuatorNetMLPCfg, +from .actuator_base_cfg import ActuatorBaseCfg +from .actuator_net import ActuatorNetLSTM, ActuatorNetMLP +from .actuator_net_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg +from .actuator_pd import DCMotor, DelayedPDActuator, IdealPDActuator, ImplicitActuator, RemotizedPDActuator +from .actuator_pd_cfg import ( DCMotorCfg, DelayedPDActuatorCfg, IdealPDActuatorCfg, ImplicitActuatorCfg, RemotizedPDActuatorCfg, ) -from .actuator_net import ActuatorNetLSTM, ActuatorNetMLP -from .actuator_pd import DCMotor, DelayedPDActuator, IdealPDActuator, ImplicitActuator, RemotizedPDActuator diff --git a/source/isaaclab/isaaclab/actuators/actuator_base.py b/source/isaaclab/isaaclab/actuators/actuator_base.py index d8a3333f1f98..4489983366d3 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_base.py +++ b/source/isaaclab/isaaclab/actuators/actuator_base.py @@ -1,20 +1,21 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from abc import ABC, abstractmethod from collections.abc import Sequence from typing import TYPE_CHECKING, ClassVar +import torch + import isaaclab.utils.string as string_utils from isaaclab.utils.types import ArticulationActions if TYPE_CHECKING: - from .actuator_cfg import ActuatorBaseCfg + from .actuator_base_cfg import ActuatorBaseCfg class ActuatorBase(ABC): @@ -55,13 +56,21 @@ class ActuatorBase(ABC): 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. + This limit is used differently depending on the actuator type: + + - **Explicit actuators**: Used for internal torque clipping within the actuator model + (e.g., motor torque limits in DC motor models). + - **Implicit actuators**: Same as :attr:`effort_limit_sim` (aliased for consistency). """ 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. + + - **Explicit actuators**: Typically set to a large value (1.0e9) to avoid double-clipping, + since the actuator model already clips efforts using :attr:`effort_limit`. + - **Implicit actuators**: Same as :attr:`effort_limit` (both values are synchronized). """ velocity_limit: torch.Tensor @@ -86,7 +95,13 @@ class ActuatorBase(ABC): """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).""" + """The joint static friction of the actuator joints. Shape is (num_envs, num_joints).""" + + dynamic_friction: torch.Tensor + """The joint dynamic friction of the actuator joints. Shape is (num_envs, num_joints).""" + + viscous_friction: torch.Tensor + """The joint viscous 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. @@ -106,6 +121,8 @@ def __init__( damping: torch.Tensor | float = 0.0, armature: torch.Tensor | float = 0.0, friction: torch.Tensor | float = 0.0, + dynamic_friction: torch.Tensor | float = 0.0, + viscous_friction: torch.Tensor | float = 0.0, effort_limit: torch.Tensor | float = torch.inf, velocity_limit: torch.Tensor | float = torch.inf, ): @@ -115,8 +132,11 @@ def __init__( 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. + The values in the constructor are typically obtained through the USD values passed from the PhysX API calls + corresponding to the joints in the actuator model; these values serve as default values if the parameters + are not specified in the cfg. + + Args: cfg: The configuration of the actuator model. @@ -131,7 +151,11 @@ def __init__( 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. + friction: The default joint static friction. Defaults to 0.0. + If a tensor, then the shape is (num_envs, num_joints). + dynamic_friction: The default joint dynamic friction. Defaults to 0.0. + If a tensor, then the shape is (num_envs, num_joints). + viscous_friction: The default joint viscous 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). @@ -144,25 +168,52 @@ def __init__( self._device = device self._joint_names = joint_names self._joint_indices = joint_ids - + self.joint_property_resolution_table: dict[str, list] = {} # 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) + # resolve usd, actuator configuration values + # case 1: if usd_value == actuator_cfg_value: all good, + # case 2: if usd_value != actuator_cfg_value: we use actuator_cfg_value + # case 3: if actuator_cfg_value is None: we use usd_value + + to_check = [ + ("velocity_limit_sim", velocity_limit), + ("effort_limit_sim", effort_limit), + ("stiffness", stiffness), + ("damping", damping), + ("armature", armature), + ("friction", friction), + ("dynamic_friction", dynamic_friction), + ("viscous_friction", viscous_friction), + ] + for param_name, usd_val in to_check: + cfg_val = getattr(self.cfg, param_name) + setattr(self, param_name, self._parse_joint_parameter(cfg_val, usd_val)) + new_val = getattr(self, param_name) + + allclose = ( + torch.all(new_val == usd_val) if isinstance(usd_val, (float, int)) else torch.allclose(new_val, usd_val) + ) + if cfg_val is None or not allclose: + self._record_actuator_resolution( + cfg_val=getattr(self.cfg, param_name), + new_val=new_val[0], # new val always has the shape of (num_envs, num_joints) + usd_val=usd_val, + joint_names=joint_names, + joint_ids=joint_ids, + actuator_param=param_name, + ) + 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) + # Parse effort_limit with special default handling: + # - If cfg.effort_limit is None, use the original USD value (effort_limit parameter from constructor) + # - Otherwise, use effort_limit_sim as the default + # Please refer to the documentation of the effort_limit and effort_limit_sim parameters for more details. + effort_default = effort_limit if self.cfg.effort_limit is None else self.effort_limit_sim + self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, effort_default) # create commands buffers for allocation self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device) @@ -246,6 +297,18 @@ def compute( Helper functions. """ + def _record_actuator_resolution(self, cfg_val, new_val, usd_val, joint_names, joint_ids, actuator_param: str): + if actuator_param not in self.joint_property_resolution_table: + self.joint_property_resolution_table[actuator_param] = [] + table = self.joint_property_resolution_table[actuator_param] + + ids = joint_ids if isinstance(joint_ids, torch.Tensor) else list(range(len(joint_names))) + for idx, name in enumerate(joint_names): + cfg_val_log = "Not Specified" if cfg_val is None else float(new_val[idx]) + default_usd_val = usd_val if isinstance(usd_val, (float, int)) else float(usd_val[0][idx]) + applied_val_log = default_usd_val if cfg_val is None else float(new_val[idx]) + table.append([name, int(ids[idx]), default_usd_val, cfg_val_log, applied_val_log]) + def _parse_joint_parameter( self, cfg_value: float | dict[str, float] | None, default_value: float | torch.Tensor | None ) -> torch.Tensor: diff --git a/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py new file mode 100644 index 000000000000..e7a26b52aef4 --- /dev/null +++ b/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py @@ -0,0 +1,165 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + + +@configclass +class ActuatorBaseCfg: + """Configuration for default actuators in an articulation.""" + + class_type: type = 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 static friction coefficient of the joints in the group. Defaults to None. + + 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. + + 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. + + If None, the joint static friction is set to the value from the USD joint prim. + + Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + dynamic_friction: dict[str, float] | float | None = None + """The dynamic friction coefficient of the joints in the group. Defaults to None. + + Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + viscous_friction: dict[str, float] | float | None = None + """The viscous friction coefficient of the joints in the group. Defaults to None. + """ diff --git a/source/isaaclab/isaaclab/actuators/actuator_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_cfg.py index 5e42fede24e6..5cacb8ffa408 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_cfg.py +++ b/source/isaaclab/isaaclab/actuators/actuator_cfg.py @@ -1,277 +1,36 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -from 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. - """ +import sys +import warnings + +from . import actuator_base_cfg, actuator_net_cfg, actuator_pd_cfg + + +def __getattr__(name): + new_module = None + if name in dir(actuator_pd_cfg): + new_module = actuator_pd_cfg + elif name in dir(actuator_net_cfg): + new_module = actuator_net_cfg + elif name in dir(actuator_base_cfg): + new_module = actuator_base_cfg + + if new_module is not None: + warnings.warn( + f"The module actuator_cfg.py is deprecated. Please import {name} directly from the isaaclab.actuators" + f" package, or from its new module {new_module.__name__}.", + DeprecationWarning, + stacklevel=2, + ) + return getattr(new_module, name) + if name in dir(sys.modules[__name__]): + return vars(sys.modules[__name__])[name] + if name == "__path__": + return __file__ + raise ImportError( + f"Failed to import attribute {name} from actuator_cfg.py. Warning: actuator_cfg.py has been " + + "deprecated. Please import actuator config classes directly from the isaaclab.actuators package.", + ) diff --git a/source/isaaclab/isaaclab/actuators/actuator_net.py b/source/isaaclab/isaaclab/actuators/actuator_net.py index ae5a931030fe..2274d1b78db3 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_net.py +++ b/source/isaaclab/isaaclab/actuators/actuator_net.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,17 +14,18 @@ from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + 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 + from .actuator_net_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg class ActuatorNetLSTM(DCMotor): @@ -79,8 +80,6 @@ def compute( # 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(): diff --git a/source/isaaclab/isaaclab/actuators/actuator_net_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_net_cfg.py new file mode 100644 index 000000000000..eecfe8050ab3 --- /dev/null +++ b/source/isaaclab/isaaclab/actuators/actuator_net_cfg.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Iterable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from . import actuator_net +from .actuator_pd_cfg import DCMotorCfg + + +@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`. + """ diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd.py b/source/isaaclab/isaaclab/actuators/actuator_pd.py index 11548e84841f..ff014fa7a58e 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_pd.py +++ b/source/isaaclab/isaaclab/actuators/actuator_pd.py @@ -1,15 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch from isaaclab.utils import DelayBuffer, LinearInterpolation from isaaclab.utils.types import ArticulationActions @@ -17,7 +17,7 @@ from .actuator_base import ActuatorBase if TYPE_CHECKING: - from .actuator_cfg import ( + from .actuator_pd_cfg import ( DCMotorCfg, DelayedPDActuatorCfg, IdealPDActuatorCfg, @@ -25,6 +25,8 @@ RemotizedPDActuatorCfg, ) +# import logger +logger = logging.getLogger(__name__) """ Implicit Actuator Models. @@ -57,7 +59,7 @@ 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( + logger.warning( "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." @@ -79,7 +81,7 @@ def __init__(self, cfg: ImplicitActuatorCfg, *args, **kwargs): 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( + logger.warning( "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" @@ -152,7 +154,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}` @@ -201,8 +203,8 @@ def compute( 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. + It uses the same model as the :class:`IdealPDActuator` for computing the torques from input commands. + However, it implements a saturation model defined by a linear four quadrant DC motor torque-speed curve. 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. @@ -211,23 +213,29 @@ class DCMotor(IdealPDActuator): 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. + * No-load speed (:math:`\dot{q}_{motor, max}`) : The maximum-rated speed of the motor at + zero torque (:attr:`velocity_limit`). + * Stall torque (:math:`\tau_{motor, stall}`): The maximum-rated torque produced at + zero speed (:attr:`saturation_effort`). + * Continuous torque (:math:`\tau_{motor, con}`): The maximum torque that can be outputted for a short period. + This is often enforced on the current drives for a DC motor to limit overheating, prevent mechanical damage, + or enforced by electrical limitations (:attr:`effort_limit`). + * Corner velocity (:math:`V_{c}`): The velocity where the torque-speed curve intersects with continuous torque. - Based on these parameters, the instantaneous minimum and maximum torques are defined as follows: + Based on these parameters, the instantaneous minimum and maximum torques for velocities between corner velocities + (where torque-speed curve intersects with continuous torque) 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) + \tau_{j, max}(\dot{q}) & = clip \left (\tau_{j, stall} \times \left(1 - + \frac{\dot{q}}{\dot{q}_{j, max}}\right), -โˆž, \tau_{j, con} \right) \\ + \tau_{j, min}(\dot{q}) & = clip \left (\tau_{j, stall} \times \left( -1 - + \frac{\dot{q}}{\dot{q}_{j, max}}\right), - \tau_{j, con}, โˆž \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 + :math:`\dot{q}_{j, max} = \gamma^{-1} \times \dot{q}_{motor, max}`, :math:`\tau_{j, con} = + \gamma \times \tau_{motor, con}` and :math:`\tau_{j, stall} = \gamma \times \tau_{motor, stall}` + are the maximum joint velocity, continuous joint torque and stall 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 @@ -237,6 +245,16 @@ class DCMotor(IdealPDActuator): \tau_{j, applied} = clip(\tau_{computed}, \tau_{j, min}(\dot{q}), \tau_{j, max}(\dot{q})) + If the velocity of the joint is outside corner velocities (this would be due to external forces) the + applied output torque will be driven to the Continuous Torque (`effort_limit`). + + The figure below demonstrates the clipping action for example (velocity, torque) pairs. + + .. figure:: ../../_static/actuator-group/dc_motor_clipping.jpg + :align: center + :figwidth: 100% + :alt: The effort clipping as a function of joint velocity for a linear DC Motor. + """ cfg: DCMotorCfg @@ -245,10 +263,11 @@ class DCMotor(IdealPDActuator): 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 + if self.cfg.saturation_effort is None: + raise ValueError("The saturation_effort must be provided for the DC motor actuator model.") + self._saturation_effort = self.cfg.saturation_effort + # find the velocity on the torque-speed curve that intersects effort_limit in the second and fourth quadrant + self._vel_at_effort_lim = self.velocity_limit * (1 + self.effort_limit / self._saturation_effort) # prepare joint vel buffer for max effort computation self._joint_vel = torch.zeros_like(self.computed_effort) # create buffer for zeros effort @@ -274,16 +293,18 @@ def compute( """ def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: + # save current joint vel + self._joint_vel[:] = torch.clip(self._joint_vel, min=-self._vel_at_effort_lim, max=self._vel_at_effort_lim) # compute torque limits + torque_speed_top = self._saturation_effort * (1.0 - self._joint_vel / self.velocity_limit) + torque_speed_bottom = self._saturation_effort * (-1.0 - self._joint_vel / self.velocity_limit) # -- 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) + max_effort = torch.clip(torque_speed_top, 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) - + min_effort = torch.clip(torque_speed_bottom, min=-self.effort_limit) # clip the torques based on the motor limits - return torch.clip(effort, min=min_effort, max=max_effort) + clamped = torch.clip(effort, min=min_effort, max=max_effort) + return clamped class DelayedPDActuator(IdealPDActuator): @@ -367,6 +388,8 @@ def __init__( damping: torch.Tensor | float = 0.0, armature: torch.Tensor | float = 0.0, friction: torch.Tensor | float = 0.0, + dynamic_friction: torch.Tensor | float = 0.0, + viscous_friction: torch.Tensor | float = 0.0, effort_limit: torch.Tensor | float = torch.inf, velocity_limit: torch.Tensor | float = torch.inf, ): @@ -375,7 +398,19 @@ def __init__( 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 + cfg, + joint_names, + joint_ids, + num_envs, + device, + stiffness, + damping, + armature, + friction, + dynamic_friction, + viscous_friction, + effort_limit, + velocity_limit, ) self._joint_parameter_lookup = torch.tensor(cfg.joint_parameter_lookup, device=device) # define remotized joint torque limit diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_pd_cfg.py new file mode 100644 index 000000000000..d1f5a6282ad8 --- /dev/null +++ b/source/isaaclab/isaaclab/actuators/actuator_pd_cfg.py @@ -0,0 +1,81 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from . import actuator_pd +from .actuator_base_cfg import ActuatorBaseCfg + +""" +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 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/source/isaaclab/isaaclab/app/__init__.py b/source/isaaclab/isaaclab/app/__init__.py index d8e9692fbf46..b81b6e3c9e5e 100644 --- a/source/isaaclab/isaaclab/app/__init__.py +++ b/source/isaaclab/isaaclab/app/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -13,4 +13,3 @@ """ from .app_launcher import AppLauncher # noqa: F401, F403 -from .runners import run_tests # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/app/app_launcher.py b/source/isaaclab/isaaclab/app/app_launcher.py index 1e8d9d2ccd6f..e986d4b664a0 100644 --- a/source/isaaclab/isaaclab/app/app_launcher.py +++ b/source/isaaclab/isaaclab/app/app_launcher.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,11 +14,11 @@ import argparse import contextlib +import logging import os import re import signal import sys -import warnings from typing import Any, Literal with contextlib.suppress(ModuleNotFoundError): @@ -26,6 +26,19 @@ from isaacsim import SimulationApp +# import logger +logger = logging.getLogger(__name__) + + +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. @@ -67,7 +80,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 @@ -99,7 +112,7 @@ def __init__(self, launcher_args: argparse.Namespace | dict | None = None, **kwa # 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._livestream: Literal[0, 1, 2] # 0: Disabled, 1: WebRTC public, 2: WebRTC private self._offscreen_render: bool # 0: Disabled, 1: Enabled self._sim_experience_file: str # Experience file to load @@ -110,16 +123,39 @@ def __init__(self, launcher_args: argparse.Namespace | dict | None = None, **kwa # 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) + # Set animation recording settings + self._set_animation_recording_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 interrupts - signal.signal(signal.SIGINT, self._interrupt_signal_handle_callback) # -- during explicit `kill` commands signal.signal(signal.SIGTERM, self._abort_signal_handle_callback) # -- during segfaults @@ -162,8 +198,8 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: Valid options are: - ``0``: Disabled - - ``1``: `Native [DEPRECATED] `_ - - ``2``: `WebRTC `_ + - ``1``: `WebRTC`_ over public network + - ``2``: `WebRTC`_ over local/private network * ``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. @@ -181,15 +217,22 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: 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``. + * 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" + + .. _`WebRTC`: https://docs.isaacsim.omniverse.nvidia.com/latest/installation/manual_livestream_clients.html#isaac-sim-short-webrtc-streaming-client + Args: parser: An argument parser instance to be extended with the AppLauncher specific options. """ @@ -248,9 +291,16 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: 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', ) @@ -276,6 +326,17 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: " 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"}, + help=( + "Sets the rendering mode. Preset settings files can be found in apps/rendering_modes." + ' Can be "performance", "balanced", or "quality".' + " Individual settings can be overwritten by using the RenderCfg class." + ), + ) arg_group.add_argument( "--kit_args", type=str, @@ -285,6 +346,30 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: ' Example usage: --kit_args "--ext-folder=/path/to/ext1 --ext-folder=/path/to/ext2"' ), ) + arg_group.add_argument( + "--anim_recording_enabled", + action="store_true", + help="Enable recording time-sampled USD animations from IsaacLab PhysX simulations.", + ) + arg_group.add_argument( + "--anim_recording_start_time", + type=float, + default=0, + help=( + "Set time that animation recording begins playing. If not set, the recording will start from the" + " beginning." + ), + ) + arg_group.add_argument( + "--anim_recording_stop_time", + type=float, + default=10, + help=( + "Set time that animation recording stops playing. If the process is shutdown before the stop time is" + " exceeded, then the animation is not recorded." + ), + ) + # special flag for backwards compatibility # Corresponding to the beginning of the function, # if we have removed -h/--help handling, we add it back. @@ -300,8 +385,10 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: "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. @@ -396,10 +483,34 @@ def _config_resolution(self, launcher_args: dict): Args: launcher_args: A dictionary of all input arguments passed to the class object. """ - # Handle all control logic resolution + # 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) - # --LIVESTREAM logic-- - # + # Handle device and distributed settings + self._resolve_device_settings(launcher_args) + + # Handle experience file settings + self._resolve_experience_file(launcher_args) + + # Handle animation recording settings + self._resolve_anim_recording_settings(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} @@ -426,8 +537,36 @@ def _config_resolution(self, launcher_args: dict): else: self._livestream = livestream_env - # --HEADLESS logic-- - # + # 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: + # WebRTC public network + self._livestream_args += [ + f"--/app/livestream/publicEndpointAddress={public_ip_env}", + "--/app/livestream/port=49100", + "--enable", + "omni.services.livestream.nvcf", + ] + elif self._livestream == 2: + # WebRTC private network + self._livestream_args += [ + "--enable", + "omni.services.livestream.nvcf", + ] + 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, @@ -463,10 +602,10 @@ def _config_resolution(self, launcher_args: dict): # Headless needs to be passed to the SimulationApp so we keep it here launcher_args["headless"] = self._headless - # --enable_cameras logic-- - # + 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_arg = launcher_args.get("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( @@ -482,9 +621,25 @@ def _config_resolution(self, launcher_args: dict): 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.get("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 + # This is different from offscreen_render because this only affects the default viewport and + # not other render-products 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 @@ -497,14 +652,25 @@ def _config_resolution(self, launcher_args: dict): # avoid creating new stage at startup by default for performance reasons launcher_args["create_new_stage"] = False - # --simulation GPU device logic -- + 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]) @@ -519,6 +685,7 @@ def _config_resolution(self, launcher_args: dict): self.global_rank = int(os.getenv("RANK", "0")) + int(os.getenv("JAX_RANK", "0")) self.device_id = self.local_rank + device = "cuda:" + str(self.device_id) 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 @@ -530,10 +697,15 @@ def _config_resolution(self, launcher_args: dict): # pass command line variable to kit sys.argv.append(f"--/plugins/carb.tasking.plugin/threadCount={num_threads_per_process}") - # set physics and rendering device + # set rendering device. We do not need to set physics_gpu because it will automatically pick the same one + # as the active_gpu device. Setting physics_gpu explicitly may result in a different device to be used. 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", "") @@ -541,15 +713,28 @@ def _config_resolution(self, launcher_args: dict): # 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") + # For Isaac Sim 4.5 compatibility, we use the 4.5 app files in a different folder + # if launcher_args.get("use_isaacsim_45", False): + if self.is_isaac_sim_version_4_5(): + isaaclab_app_exp_path = os.path.join(isaaclab_app_exp_path, "isaacsim_4_5") + if self._sim_experience_file == "": - # check if the headless flag is setS - if self._enable_cameras: + # check if the headless flag is set + # xr rendering overrides camera rendering settings + if self._enable_cameras and not self._xr: 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: @@ -574,53 +759,32 @@ def _config_resolution(self, launcher_args: dict): " 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") + # 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}") - # 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." + def _resolve_anim_recording_settings(self, launcher_args: dict): + """Resolve animation recording settings.""" + + # Enable omni.physx.pvd extension if recording is enabled + recording_enabled = launcher_args.get("anim_recording_enabled", False) + if recording_enabled: + if self._headless: + raise ValueError("Animation recording is not supported in headless mode.") + if self.is_isaac_sim_version_4_5(): + raise RuntimeError( + "Animation recording is not supported in Isaac Sim 4.5. Please update to Isaac Sim 5.0." ) - 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 + sys.argv += ["--enable", "omni.physx.pvd"] + 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 - # 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}") - # 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 _create_app(self): """Launch and create the SimulationApp based on the parsed simulation config.""" # Initialize SimulationApp @@ -638,6 +802,23 @@ def _create_app(self): # 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 + + # pytest may have left some things in sys.argv, this will check for some of those + # do a mark and sweep to remove any -m pytest and -m isaacsim_ci and -c **/pyproject.toml + indexes_to_remove = [] + for idx, arg in enumerate(sys.argv[:-1]): + if arg == "-m": + value_for_dash_m = sys.argv[idx + 1] + if "pytest" in value_for_dash_m or "isaacsim_ci" in value_for_dash_m: + indexes_to_remove.append(idx) + indexes_to_remove.append(idx + 1) + if arg.startswith("--config-file=") and "pyproject.toml" in arg: + indexes_to_remove.append(idx) + if arg == "--capture=no": + indexes_to_remove.append(idx) + for idx in sorted(indexes_to_remove, reverse=True): + sys.argv = sys.argv[:idx] + sys.argv[idx + 1 :] + # launch simulation app self._app = SimulationApp(self._sim_app_config, experience=self._sim_experience_file) # enable sys stdout and stderr @@ -649,6 +830,7 @@ def _create_app(self): # 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] @@ -658,14 +840,14 @@ def _create_app(self): 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 + # 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() @@ -688,8 +870,9 @@ def _load_extensions(self): # 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) + # in theory, this should ensure that dt is consistent across time stepping, but this is not the case + # for now, we use the custom loop runner from Isaac Sim to achieve this + carb_settings_iface.set_bool("/app/player/useFixedTimeStepping", False) def _hide_stop_button(self): """Hide the stop button in the toolbar. @@ -710,6 +893,48 @@ def _hide_stop_button(self): 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: + """Store RTX rendering mode in carb settings.""" + import carb + + rendering_mode = launcher_args.get("rendering_mode") + + if rendering_mode is None: + # use default kit rendering settings if cameras are disabled and a rendering mode is not selected + if not self._enable_cameras: + return + rendering_mode = "" + + # store rendering mode in carb settings + carb_settings = carb.settings.get_settings() + carb_settings.set_string("/isaaclab/rendering/rendering_mode", rendering_mode) + + def _set_animation_recording_settings(self, launcher_args: dict) -> None: + """Store animation recording settings in carb settings.""" + import carb + + # check if recording is enabled + recording_enabled = launcher_args.get("anim_recording_enabled", False) + if not recording_enabled: + return + + # arg checks + if launcher_args.get("anim_recording_start_time") >= launcher_args.get("anim_recording_stop_time"): + raise ValueError( + f"'anim_recording_start_time' {launcher_args.get('anim_recording_start_time')} must be less than" + f" 'anim_recording_stop_time' {launcher_args.get('anim_recording_stop_time')}" + ) + + # grab config + start_time = launcher_args.get("anim_recording_start_time") + stop_time = launcher_args.get("anim_recording_stop_time") + + # store config in carb settings + carb_settings = carb.settings.get_settings() + carb_settings.set_bool("/isaaclab/anim_recording/enabled", recording_enabled) + carb_settings.set_float("/isaaclab/anim_recording/start_time", start_time) + carb_settings.set_float("/isaaclab/anim_recording/stop_time", stop_time) + def _interrupt_signal_handle_callback(self, signal, frame): """Handle the interrupt signal from the keyboard.""" # close the app @@ -717,7 +942,142 @@ def _interrupt_signal_handle_callback(self, signal, frame): # raise the error for keyboard interrupt raise KeyboardInterrupt + def is_isaac_sim_version_4_5(self) -> bool: + if not hasattr(self, "_is_sim_ver_4_5"): + # 1) Try to read the VERSION file (for manual / binary installs) + version_path = os.path.abspath(os.path.join(os.path.dirname(isaacsim.__file__), "../../VERSION")) + if os.path.isfile(version_path): + with open(version_path) as f: + ver = f.readline().strip() + if ver.startswith("4.5"): + self._is_sim_ver_4_5 = True + return True + + # 2) Fall back to metadata (for pip installs) + from importlib.metadata import version as pkg_version + + try: + ver = pkg_version("isaacsim") + if ver.startswith("4.5"): + self._is_sim_ver_4_5 = True + else: + self._is_sim_ver_4_5 = False + except Exception: + self._is_sim_ver_4_5 = False + return self._is_sim_ver_4_5 + + 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 + + from pxr import Gf + + logger.warning( + "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: + logger.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: + logger.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/source/isaaclab/isaaclab/app/runners.py b/source/isaaclab/isaaclab/app/runners.py deleted file mode 100644 index 4b9281a20f00..000000000000 --- a/source/isaaclab/isaaclab/app/runners.py +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Sub-module with runners to simplify running main via unittest.""" - -import unittest - - -def run_tests(verbosity: int = 2, **kwargs): - """Wrapper for running tests via ``unittest``. - - Args: - verbosity: Verbosity level for the test runner. - **kwargs: Additional arguments to pass to the `unittest.main` function. - """ - # run main - unittest.main(verbosity=verbosity, exit=True, **kwargs) diff --git a/source/isaaclab/isaaclab/assets/__init__.py b/source/isaaclab/isaaclab/assets/__init__.py index 53b64bd1a7d9..41640e5286f5 100644 --- a/source/isaaclab/isaaclab/assets/__init__.py +++ b/source/isaaclab/isaaclab/assets/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -44,3 +44,4 @@ from .deformable_object import DeformableObject, DeformableObjectCfg, DeformableObjectData from .rigid_object import RigidObject, RigidObjectCfg, RigidObjectData from .rigid_object_collection import RigidObjectCollection, RigidObjectCollectionCfg, RigidObjectCollectionData +from .surface_gripper import SurfaceGripper, SurfaceGripperCfg diff --git a/source/isaaclab/isaaclab/assets/articulation/__init__.py b/source/isaaclab/isaaclab/assets/articulation/__init__.py index 4d6d3675f1db..e24b3ba10f47 100644 --- a/source/isaaclab/isaaclab/assets/articulation/__init__.py +++ b/source/isaaclab/isaaclab/assets/articulation/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index de0789f22446..b67ded15ac48 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,14 +8,16 @@ from __future__ import annotations -import torch +import logging 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 torch +import warp as wp +from prettytable import PrettyTable + 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 @@ -23,6 +25,8 @@ import isaaclab.utils.string as string_utils from isaaclab.actuators import ActuatorBase, ActuatorBaseCfg, ImplicitActuator from isaaclab.utils.types import ArticulationActions +from isaaclab.utils.version import get_isaac_sim_version +from isaaclab.utils.wrench_composer import WrenchComposer from ..asset_base import AssetBase from .articulation_data import ArticulationData @@ -30,6 +34,9 @@ if TYPE_CHECKING: from .articulation_cfg import ArticulationCfg +# import logger +logger = logging.getLogger(__name__) + class Articulation(AssetBase): """An articulation asset class. @@ -125,6 +132,11 @@ def num_fixed_tendons(self) -> int: """Number of fixed tendons in articulation.""" return self.root_physx_view.max_fixed_tendons + @property + def num_spatial_tendons(self) -> int: + """Number of spatial tendons in articulation.""" + return self.root_physx_view.max_spatial_tendons + @property def num_bodies(self) -> int: """Number of bodies in articulation.""" @@ -140,6 +152,11 @@ def fixed_tendon_names(self) -> list[str]: """Ordered names of fixed tendons in articulation.""" return self._fixed_tendon_names + @property + def spatial_tendon_names(self) -> list[str]: + """Ordered names of spatial tendons in articulation.""" + return self._spatial_tendon_names + @property def body_names(self) -> list[str]: """Ordered names of bodies in articulation.""" @@ -154,6 +171,35 @@ def root_physx_view(self) -> physx.ArticulationView: """ return self._root_physx_view + @property + def instantaneous_wrench_composer(self) -> WrenchComposer: + """Instantaneous wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are only valid for the current simulation step. At the end of the simulation step, the wrenches set + to this object are discarded. This is useful to apply forces that change all the time, things like drag forces + for instance. + + Note: + Permanent wrenches are composed into the instantaneous wrench before the instantaneous wrenches are + applied to the simulation. + """ + return self._instantaneous_wrench_composer + + @property + def permanent_wrench_composer(self) -> WrenchComposer: + """Permanent wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are persistent and are applied to the simulation at every step. This is useful to apply forces that + are constant over a period of time, things like the thrust of a motor for instance. + + Note: + Permanent wrenches are composed into the instantaneous wrench before the instantaneous wrenches are + applied to the simulation. + """ + return self._permanent_wrench_composer + """ Operations. """ @@ -165,9 +211,9 @@ def reset(self, env_ids: Sequence[int] | None = 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 + # reset external wrenches. + self._instantaneous_wrench_composer.reset(env_ids) + self._permanent_wrench_composer.reset(env_ids) def write_data_to_sim(self): """Write external wrenches and joint commands to the simulation. @@ -180,14 +226,33 @@ def write_data_to_sim(self): 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, - ) + if self._instantaneous_wrench_composer.active or self._permanent_wrench_composer.active: + if self._instantaneous_wrench_composer.active: + # Compose instantaneous wrench with permanent wrench + self._instantaneous_wrench_composer.add_forces_and_torques( + forces=self._permanent_wrench_composer.composed_force, + torques=self._permanent_wrench_composer.composed_torque, + body_ids=self._ALL_BODY_INDICES_WP, + env_ids=self._ALL_INDICES_WP, + ) + # Apply both instantaneous and permanent wrench to the simulation + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._instantaneous_wrench_composer.composed_force_as_torch.view(-1, 3), + torque_data=self._instantaneous_wrench_composer.composed_torque_as_torch.view(-1, 3), + position_data=None, + indices=self._ALL_INDICES, + is_global=False, + ) + else: + # Apply permanent wrench to the simulation + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._permanent_wrench_composer.composed_force_as_torch.view(-1, 3), + torque_data=self._permanent_wrench_composer.composed_torque_as_torch.view(-1, 3), + position_data=None, + indices=self._ALL_INDICES, + is_global=False, + ) + self._instantaneous_wrench_composer.reset() # apply actuator models self._apply_actuator_model() @@ -266,6 +331,28 @@ def find_fixed_tendons( # find tendons return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order) + def find_spatial_tendons( + self, name_keys: str | Sequence[str], tendon_subsets: list[str] | None = None, preserve_order: bool = False + ) -> tuple[list[int], list[str]]: + """Find spatial 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 tendon names. + tendon_subsets: A subset of tendons to search for. Defaults to None, which means all tendons + 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: + tendon_subsets = self.spatial_tendon_names + # find tendons + return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order) + """ Operations - State Writers. """ @@ -280,10 +367,8 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """ - - # set into simulation - self.write_root_pose_to_sim(root_state[:, :7], env_ids=env_ids) - self.write_root_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + 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. @@ -295,7 +380,6 @@ def write_root_com_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequenc root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """ - # set into simulation 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) @@ -309,7 +393,6 @@ def write_root_link_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequen root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """ - # set into simulation 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) @@ -322,23 +405,7 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] 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_state_w[env_ids, :7] = root_pose.clone() - # convert root quaternion from wxyz to xyzw - root_poses_xyzw = self._data.root_state_w[:, :7].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 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_root_transforms(root_poses_xyzw, indices=physx_env_ids) + 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. @@ -354,17 +421,27 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence 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_state_w[env_ids, :7] = root_pose.clone() - self._data.root_state_w[env_ids, :7] = self._data.root_link_state_w[env_ids, :7] + 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_state_w[:, :7].clone() + 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 root pose. + + # 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) @@ -379,23 +456,31 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[ 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 - - com_pos = self.data.com_pos_b[env_ids, 0, :] - com_quat = self.data.com_quat_b[env_ids, 0, :] + 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_rotate(math_utils.quat_inv(com_quat), -com_pos), - math_utils.quat_inv(com_quat), + 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) - self.write_root_link_pose_to_sim(root_pose=root_link_pose, env_ids=physx_env_ids) + + # 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. @@ -407,17 +492,7 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque 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_state_w[env_ids, 7:] = root_velocity.clone() - self._data.body_acc_w[env_ids] = 0.0 - # set into simulation - self.root_physx_view.set_root_velocities(self._data.root_state_w[:, 7:], indices=physx_env_ids) + 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. @@ -429,19 +504,25 @@ def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: S 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_state_w[env_ids, 7:] = root_velocity.clone() - self._data.root_state_w[env_ids, 7:] = self._data.root_com_state_w[env_ids, 7:] + 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_state_w[:, 7:], indices=physx_env_ids) + 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. @@ -454,20 +535,28 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: 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 + 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() - quat = self.data.root_link_state_w[env_ids, 3:7] - com_pos_b = self.data.com_pos_b[env_ids, 0, :] - # transform given velocity to center of mass root_com_velocity[:, :3] += torch.linalg.cross( - root_com_velocity[:, 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1 + root_com_velocity[:, 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1 ) - # write center of mass velocity to sim - self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=physx_env_ids) + + # 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, @@ -514,6 +603,12 @@ def write_joint_position_to_sim( # 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_com_vel_w.timestamp = -1.0 + self._data._body_link_vel_w.timestamp = -1.0 + self._data._body_com_pose_b.timestamp = -1.0 + self._data._body_com_pose_w.timestamp = -1.0 + self._data._body_link_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 @@ -655,10 +750,10 @@ def write_joint_position_limit_to_sim( ) if warn_limit_violation: # warn level will show in console - omni.log.warn(violation_message) + logger.warning(violation_message) else: # info level is only written to log file - omni.log.info(violation_message) + logger.info(violation_message) # set into simulation self.root_physx_view.set_dof_limits(self._data.joint_pos_limits.cpu(), indices=physx_env_ids.cpu()) @@ -776,24 +871,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 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. + 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}\|`. + + 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: - 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. + - 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: 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). + 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 @@ -807,10 +911,90 @@ 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 - self.root_physx_view.set_dof_friction_coefficients( - self._data.joint_friction_coeff.cpu(), indices=physx_env_ids.cpu() - ) + if get_isaac_sim_version().major < 5: + self.root_physx_view.set_dof_friction_coefficients( + 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_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, + joint_dynamic_friction_coeff: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + if get_isaac_sim_version().major < 5: + logger.warning("Setting joint dynamic friction coefficients are not supported in Isaac Sim < 5.0") + return + # 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_dynamic_friction_coeff[env_ids, joint_ids] = joint_dynamic_friction_coeff + # set into simulation + friction_props = self.root_physx_view.get_dof_friction_properties() + friction_props[physx_env_ids.cpu(), :, 1] = self._data.joint_dynamic_friction_coeff[physx_env_ids, :].cpu() + self.root_physx_view.set_dof_friction_properties(friction_props, indices=physx_env_ids.cpu()) + + def write_joint_viscous_friction_coefficient_to_sim( + self, + joint_viscous_friction_coeff: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + if get_isaac_sim_version().major < 5: + logger.warning("Setting joint viscous friction coefficients are not supported in Isaac Sim < 5.0") + return + # 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_viscous_friction_coeff[env_ids, joint_ids] = joint_viscous_friction_coeff + # set into simulation + friction_props = self.root_physx_view.get_dof_friction_properties() + friction_props[physx_env_ids.cpu(), :, 2] = self._data.joint_viscous_friction_coeff[physx_env_ids, :].cpu() + self.root_physx_view.set_dof_friction_properties(friction_props, indices=physx_env_ids.cpu()) """ Operations - Setters. @@ -820,14 +1004,17 @@ def set_external_force_and_torque( self, forces: torch.Tensor, torques: torch.Tensor, + positions: torch.Tensor | None = None, body_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None, + is_global: bool = False, ): """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. + into buffers which are then applied to the simulation at every step. Optionally, set the position to apply the + external wrench at (in the local link frame of the bodies). .. caution:: If the function is called with empty forces and torques, then this function disables the application @@ -846,37 +1033,48 @@ def set_external_force_and_torque( 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). + positions: Positions to apply external wrench. Shape is (len(env_ids), len(body_ids), 3). Defaults to None. 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). + is_global: Whether to apply the external wrench in the global frame. Defaults to False. If set to False, + the external wrench is applied in the link frame of the articulations' bodies. """ - if forces.any() or torques.any(): - self.has_external_wrench = True - else: - self.has_external_wrench = False + logger.warning( + "The function 'set_external_force_and_torque' will be deprecated in a future release. Please" + " use 'permanent_wrench_composer.set_forces_and_torques' instead." + ) + if forces is None and torques is None: + logger.warning("No forces or torques provided. No permanent external wrench will be applied.") # resolve all indices # -- env_ids if env_ids is None: - env_ids = self._ALL_INDICES + env_ids = self._ALL_INDICES_WP elif not isinstance(env_ids, torch.Tensor): - env_ids = torch.tensor(env_ids, dtype=torch.long, device=self.device) + env_ids = wp.array(env_ids, dtype=wp.int32, device=self.device) + else: + env_ids = wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) # -- body_ids if body_ids is None: - body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device) + body_ids = self._ALL_BODY_INDICES_WP elif isinstance(body_ids, slice): - body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device)[body_ids] + body_ids = wp.from_torch( + torch.arange(self.num_bodies, dtype=torch.int32, device=self.device)[body_ids], dtype=wp.int32 + ) 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) + body_ids = wp.array(body_ids, dtype=wp.int32, device=self.device) + else: + body_ids = wp.from_torch(body_ids.to(torch.int32), dtype=wp.int32) + + # Write to wrench composer + self._permanent_wrench_composer.set_forces_and_torques( + forces=wp.from_torch(forces, dtype=wp.vec3f) if forces is not None else None, + torques=wp.from_torch(torques, dtype=wp.vec3f) if torques is not None else None, + positions=wp.from_torch(positions, dtype=wp.vec3f) if positions is not None else None, + body_ids=body_ids, + env_ids=env_ids, + is_global=is_global, + ) def set_joint_position_target( self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None @@ -963,7 +1161,8 @@ def set_fixed_tendon_stiffness( """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. + the desired values. To apply the tendon stiffness, call the + :meth:`write_fixed_tendon_properties_to_sim` method. Args: stiffness: Fixed tendon stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). @@ -1015,7 +1214,8 @@ def set_fixed_tendon_limit_stiffness( """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. + the desired values. To apply the tendon limit stiffness, call the + :meth:`write_fixed_tendon_properties_to_sim` method. Args: limit_stiffness: Fixed tendon limit stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). @@ -1067,7 +1267,8 @@ def set_fixed_tendon_rest_length( """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. + the desired values. To apply the tendon rest length, call the + :meth:`write_fixed_tendon_properties_to_sim` method. Args: rest_length: Fixed tendon rest length. Shape is (len(env_ids), len(fixed_tendon_ids)). @@ -1139,54 +1340,239 @@ def write_fixed_tendon_properties_to_sim( indices=physx_env_ids, ) + def set_spatial_tendon_stiffness( + self, + stiffness: torch.Tensor, + spatial_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set spatial 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_spatial_tendon_properties_to_sim` method. + + Args: + stiffness: Spatial tendon stiffness. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices to set the stiffness for. Defaults to None (all spatial tendons). + env_ids: The environment indices to set the stiffness for. Defaults to None (all environments). + """ + if get_isaac_sim_version().major < 5: + logger.warning( + "Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later." + ) + return + # resolve indices + if env_ids is None: + env_ids = slice(None) + if spatial_tendon_ids is None: + spatial_tendon_ids = slice(None) + if env_ids != slice(None) and spatial_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set stiffness + self._data.spatial_tendon_stiffness[env_ids, spatial_tendon_ids] = stiffness + + def set_spatial_tendon_damping( + self, + damping: torch.Tensor, + spatial_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set spatial 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_spatial_tendon_properties_to_sim` method. + + Args: + damping: Spatial tendon damping. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices to set the damping for. Defaults to None, + which means all spatial tendons. + env_ids: The environment indices to set the damping for. Defaults to None, which means all environments. + """ + if get_isaac_sim_version().major < 5: + logger.warning( + "Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later." + ) + return + # resolve indices + if env_ids is None: + env_ids = slice(None) + if spatial_tendon_ids is None: + spatial_tendon_ids = slice(None) + if env_ids != slice(None) and spatial_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set damping + self._data.spatial_tendon_damping[env_ids, spatial_tendon_ids] = damping + + def set_spatial_tendon_limit_stiffness( + self, + limit_stiffness: torch.Tensor, + spatial_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set spatial tendon limit stiffness 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_spatial_tendon_properties_to_sim` method. + + Args: + limit_stiffness: Spatial tendon limit stiffness. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices to set the limit stiffness for. Defaults to None, + which means all spatial tendons. + env_ids: The environment indices to set the limit stiffness for. Defaults to None (all environments). + """ + if get_isaac_sim_version().major < 5: + logger.warning( + "Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later." + ) + return + # resolve indices + if env_ids is None: + env_ids = slice(None) + if spatial_tendon_ids is None: + spatial_tendon_ids = slice(None) + if env_ids != slice(None) and spatial_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set limit stiffness + self._data.spatial_tendon_limit_stiffness[env_ids, spatial_tendon_ids] = limit_stiffness + + def set_spatial_tendon_offset( + self, + offset: torch.Tensor, + spatial_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set spatial 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_spatial_tendon_properties_to_sim` method. + + Args: + offset: Spatial tendon offset. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices to set the offset for. Defaults to None (all spatial tendons). + env_ids: The environment indices to set the offset for. Defaults to None (all environments). + """ + if get_isaac_sim_version().major < 5: + logger.warning( + "Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later." + ) + return + # resolve indices + if env_ids is None: + env_ids = slice(None) + if spatial_tendon_ids is None: + spatial_tendon_ids = slice(None) + if env_ids != slice(None) and spatial_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set offset + self._data.spatial_tendon_offset[env_ids, spatial_tendon_ids] = offset + + def write_spatial_tendon_properties_to_sim( + self, + spatial_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write spatial tendon properties into the simulation. + + Args: + spatial_tendon_ids: The spatial tendon indices to set the properties for. Defaults to None, + which means all spatial tendons. + env_ids: The environment indices to set the properties for. Defaults to None, + which means all environments. + """ + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + physx_env_ids = self._ALL_INDICES + if spatial_tendon_ids is None: + spatial_tendon_ids = slice(None) + + # set into simulation + self.root_physx_view.set_spatial_tendon_properties( + self._data.spatial_tendon_stiffness, + self._data.spatial_tendon_damping, + self._data.spatial_tendon_limit_stiffness, + self._data.spatial_tendon_offset, + indices=physx_env_ids, + ) + """ Internal helper. """ def _initialize_impl(self): - # create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") - # 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 articulation root prims - root_prims = sim_utils.get_all_matching_child_prims( - template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI) - ) - if len(root_prims) == 0: - raise RuntimeError( - f"Failed to find an articulation when resolving '{self.cfg.prim_path}'." - " Please ensure that the prim has 'USD ArticulationRootAPI' applied." - ) - if len(root_prims) > 1: - raise RuntimeError( - f"Failed to find a single articulation when resolving '{self.cfg.prim_path}'." - f" Found multiple '{root_prims}' under '{template_prim_path}'." - " Please ensure that there is only one articulation in the prim path tree." + # 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), + traverse_instance_prims=False, ) + 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 - # resolve articulation 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) :] # -- 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: {self.cfg.prim_path}. Please check PhysX logs.") + raise RuntimeError(f"Failed to create articulation at: {root_prim_path_expr}. Please check PhysX logs.") + + if get_isaac_sim_version().major < 5: + logger.warning( + "Spatial tendons are not supported in Isaac Sim < 5.0: patching spatial-tendon getter" + " and setter to use dummy value" + ) + self._root_physx_view.max_spatial_tendons = 0 + self._root_physx_view.get_spatial_tendon_stiffnesses = lambda: torch.empty(0, device=self.device) + self._root_physx_view.get_spatial_tendon_dampings = lambda: torch.empty(0, device=self.device) + self._root_physx_view.get_spatial_tendon_limit_stiffnesses = lambda: torch.empty(0, device=self.device) + self._root_physx_view.get_spatial_tendon_offsets = lambda: torch.empty(0, device=self.device) + self._root_physx_view.set_spatial_tendon_properties = lambda *args, **kwargs: logger.warning( + "Spatial tendons are not supported in Isaac Sim < 5.0: Calling" + " set_spatial_tendon_properties has no effect" + ) # 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}") + logger.info(f"Articulation initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.") + logger.info(f"Is fixed root: {self.is_fixed_base}") + logger.info(f"Number of bodies: {self.num_bodies}") + logger.info(f"Body names: {self.body_names}") + logger.info(f"Number of joints: {self.num_joints}") + logger.info(f"Joint names: {self.joint_names}") + logger.info(f"Number of fixed tendons: {self.num_fixed_tendons}") # container for data access self._data = ArticulationData(self.root_physx_view, self.device) @@ -1196,7 +1582,7 @@ def _initialize_impl(self): # process configuration self._process_cfg() self._process_actuators_cfg() - self._process_fixed_tendons() + self._process_tendons() # validate configuration self._validate_cfg() # update the robot data @@ -1207,25 +1593,35 @@ def _initialize_impl(self): def _create_buffers(self): # constants self._ALL_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) + self._ALL_BODY_INDICES = torch.arange(self.num_bodies, dtype=torch.long, device=self.device) + self._ALL_INDICES_WP = wp.from_torch(self._ALL_INDICES.to(torch.int32), dtype=wp.int32) + self._ALL_BODY_INDICES_WP = wp.from_torch(self._ALL_BODY_INDICES.to(torch.int32), dtype=wp.int32) - # 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) + # external wrench composer + self._instantaneous_wrench_composer = WrenchComposer(self) + self._permanent_wrench_composer = WrenchComposer(self) # 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 + # tendon names are set in _process_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() - ) + if get_isaac_sim_version().major < 5: + self._data.default_joint_friction_coeff = ( + self.root_physx_view.get_dof_friction_coefficients().to(self.device).clone() + ) + self._data.default_joint_dynamic_friction_coeff = torch.zeros_like(self._data.default_joint_friction_coeff) + self._data.default_joint_viscous_friction_coeff = torch.zeros_like(self._data.default_joint_friction_coeff) + else: + friction_props = self.root_physx_view.get_dof_friction_properties() + self._data.default_joint_friction_coeff = friction_props[:, :, 0].to(self.device).clone() + self._data.default_joint_dynamic_friction_coeff = friction_props[:, :, 1].to(self.device).clone() + self._data.default_joint_viscous_friction_coeff = friction_props[:, :, 2].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() @@ -1234,6 +1630,8 @@ def _create_buffers(self): 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() + self._data.joint_dynamic_friction_coeff = self._data.default_joint_dynamic_friction_coeff.clone() + self._data.joint_viscous_friction_coeff = self._data.default_joint_viscous_friction_coeff.clone() # -- body properties self._data.default_mass = self.root_physx_view.get_masses().clone() @@ -1301,8 +1699,6 @@ 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._physics_sim_view = None self._root_physx_view = None """ @@ -1347,12 +1743,14 @@ def _process_actuators_cfg(self): 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], + dynamic_friction=self._data.default_joint_dynamic_friction_coeff[:, joint_ids], + viscous_friction=self._data.default_joint_viscous_friction_coeff[:, joint_ids], + effort_limit=self._data.joint_effort_limits[:, joint_ids].clone(), 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( + logger.info( f"Actuator collection: {actuator_name} with model '{actuator_cfg.class_type.__name__}'" f" (type: {model_type}) and joint names: {joint_names} [{joint_ids}]." ) @@ -1375,6 +1773,13 @@ def _process_actuators_cfg(self): 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) + if get_isaac_sim_version().major >= 5: + self.write_joint_dynamic_friction_coefficient_to_sim( + actuator.dynamic_friction, joint_ids=actuator.joint_indices + ) + self.write_joint_viscous_friction_coefficient_to_sim( + actuator.viscous_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) @@ -1382,37 +1787,57 @@ def _process_actuators_cfg(self): 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 + if get_isaac_sim_version().major >= 5: + self._data.default_joint_dynamic_friction_coeff[:, actuator.joint_indices] = actuator.dynamic_friction + self._data.default_joint_viscous_friction_coeff[:, actuator.joint_indices] = actuator.viscous_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( + logger.warning( "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.""" + if self.cfg.actuator_value_resolution_debug_print: + t = PrettyTable(["Group", "Property", "Name", "ID", "USD Value", "ActutatorCfg Value", "Applied"]) + for actuator_group, actuator in self.actuators.items(): + group_count = 0 + for property, resolution_details in actuator.joint_property_resolution_table.items(): + for prop_idx, resolution_detail in enumerate(resolution_details): + actuator_group_str = actuator_group if group_count == 0 else "" + property_str = property if prop_idx == 0 else "" + fmt = [f"{v:.2e}" if isinstance(v, float) else str(v) for v in resolution_detail] + t.add_row([actuator_group_str, property_str, *fmt]) + group_count += 1 + logger.warning(f"\nActuatorCfg-USD Value Discrepancy Resolution (matching values are skipped): \n{t}") + + def _process_tendons(self): + """Process fixed and spatial tendons.""" # create a list to store the fixed tendon names self._fixed_tendon_names = list() - + self._spatial_tendon_names = list() # parse fixed tendons properties if they exist - if self.num_fixed_tendons > 0: - stage = stage_utils.get_current_stage() + if self.num_fixed_tendons > 0 or self.num_spatial_tendons > 0: 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) + joint = UsdPhysics.Joint.Get(self.stage, usd_joint_path) if joint.GetPrim().HasAPI(PhysxSchema.PhysxTendonAxisRootAPI): joint_name = usd_joint_path.split("/")[-1] self._fixed_tendon_names.append(joint_name) + elif joint.GetPrim().HasAPI(PhysxSchema.PhysxTendonAttachmentRootAPI) or joint.GetPrim().HasAPI( + PhysxSchema.PhysxTendonAttachmentLeafAPI + ): + joint_name = usd_joint_path.split("/")[-1] + self._spatial_tendon_names.append(joint_name) # store the fixed tendon names self._data.fixed_tendon_names = self._fixed_tendon_names - + self._data.spatial_tendon_names = self._spatial_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() @@ -1422,6 +1847,12 @@ def _process_fixed_tendons(self): 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() + self._data.default_spatial_tendon_stiffness = self.root_physx_view.get_spatial_tendon_stiffnesses().clone() + self._data.default_spatial_tendon_damping = self.root_physx_view.get_spatial_tendon_dampings().clone() + self._data.default_spatial_tendon_limit_stiffness = ( + self.root_physx_view.get_spatial_tendon_limit_stiffnesses().clone() + ) + self._data.default_spatial_tendon_offset = self.root_physx_view.get_spatial_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() @@ -1430,6 +1861,10 @@ def _process_fixed_tendons(self): 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() + self._data.spatial_tendon_stiffness = self._data.default_spatial_tendon_stiffness.clone() + self._data.spatial_tendon_damping = self._data.default_spatial_tendon_damping.clone() + self._data.spatial_tendon_limit_stiffness = self._data.default_spatial_tendon_limit_stiffness.clone() + self._data.spatial_tendon_offset = self._data.default_spatial_tendon_offset.clone() def _apply_actuator_model(self): """Processes joint commands for the articulation by forwarding them to the actuators. @@ -1519,13 +1954,35 @@ def _log_articulation_info(self): Note: We purposefully read the values from the simulator to ensure that the values are configured as expected. """ + + # define custom formatters for large numbers and limit ranges + def format_large_number(_, v: float) -> str: + """Format large numbers using scientific notation.""" + if abs(v) >= 1e3: + return f"{v:.1e}" + else: + return f"{v:.3f}" + + def format_limits(_, v: tuple[float, float]) -> str: + """Format limit ranges using scientific notation.""" + if abs(v[0]) >= 1e3 or abs(v[1]) >= 1e3: + return f"[{v[0]:.1e}, {v[1]:.1e}]" + else: + return f"[{v[0]:.3f}, {v[1]:.3f}]" + # 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() + if get_isaac_sim_version().major < 5: + static_frictions = self.root_physx_view.get_dof_friction_coefficients()[0].tolist() + else: + friction_props = self.root_physx_view.get_dof_friction_properties() + static_frictions = friction_props[:, :, 0][0].tolist() + dynamic_frictions = friction_props[:, :, 1][0].tolist() + viscous_frictions = friction_props[:, :, 2][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() @@ -1533,38 +1990,44 @@ def _log_articulation_info(self): # 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}]" + # build field names based on Isaac Sim version + field_names = ["Index", "Name", "Stiffness", "Damping", "Armature"] + if get_isaac_sim_version().major < 5: + field_names.append("Static Friction") + else: + field_names.extend(["Static Friction", "Dynamic Friction", "Viscous Friction"]) + field_names.extend(["Position Limits", "Velocity Limits", "Effort Limits"]) + joint_table.field_names = field_names + + # apply custom formatters to numeric columns + joint_table.custom_format["Stiffness"] = format_large_number + joint_table.custom_format["Damping"] = format_large_number + joint_table.custom_format["Armature"] = format_large_number + joint_table.custom_format["Static Friction"] = format_large_number + if get_isaac_sim_version().major >= 5: + joint_table.custom_format["Dynamic Friction"] = format_large_number + joint_table.custom_format["Viscous Friction"] = format_large_number + joint_table.custom_format["Position Limits"] = format_limits + joint_table.custom_format["Velocity Limits"] = format_large_number + joint_table.custom_format["Effort Limits"] = format_large_number + # 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], - ]) + # build row data based on Isaac Sim version + row_data = [index, name, stiffnesses[index], dampings[index], armatures[index]] + if get_isaac_sim_version().major < 5: + row_data.append(static_frictions[index]) + else: + row_data.extend([static_frictions[index], dynamic_frictions[index], viscous_frictions[index]]) + row_data.extend([position_limits[index], velocity_limits[index], effort_limits[index]]) + # add row to table + joint_table.add_row(row_data) # convert table to string - omni.log.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + joint_table.get_string()) + logger.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + joint_table.get_string()) - # read out all tendon parameters from simulation + # read out all fixed tendon parameters from simulation if self.num_fixed_tendons > 0: # -- gains ft_stiffnesses = self.root_physx_view.get_fixed_tendon_stiffnesses()[0].tolist() @@ -1587,20 +2050,66 @@ def _log_articulation_info(self): "Offset", ] tendon_table.float_format = ".3" - joint_table.custom_format["Limits"] = lambda f, v: f"[{v[0]:.3f}, {v[1]:.3f}]" + + # apply custom formatters to tendon table columns + tendon_table.custom_format["Stiffness"] = format_large_number + tendon_table.custom_format["Damping"] = format_large_number + tendon_table.custom_format["Limit Stiffness"] = format_large_number + tendon_table.custom_format["Limits"] = format_limits + tendon_table.custom_format["Rest Length"] = format_large_number + tendon_table.custom_format["Offset"] = format_large_number + # 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], - ]) + 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()) + logger.info( + f"Simulation parameters for fixed tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string() + ) + + if self.num_spatial_tendons > 0: + # -- gains + st_stiffnesses = self.root_physx_view.get_spatial_tendon_stiffnesses()[0].tolist() + st_dampings = self.root_physx_view.get_spatial_tendon_dampings()[0].tolist() + # -- limits + st_limit_stiffnesses = self.root_physx_view.get_spatial_tendon_limit_stiffnesses()[0].tolist() + st_offsets = self.root_physx_view.get_spatial_tendon_offsets()[0].tolist() + # create table for term information + tendon_table = PrettyTable() + tendon_table.title = f"Simulation Spatial Tendon Information (Prim path: {self.cfg.prim_path})" + tendon_table.field_names = [ + "Index", + "Stiffness", + "Damping", + "Limit Stiffness", + "Offset", + ] + tendon_table.float_format = ".3" + # add info on each term + for index in range(self.num_spatial_tendons): + tendon_table.add_row( + [ + index, + st_stiffnesses[index], + st_dampings[index], + st_limit_stiffnesses[index], + st_offsets[index], + ] + ) + # convert table to string + logger.info( + f"Simulation parameters for spatial tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string() + ) """ Deprecated methods. @@ -1617,7 +2126,7 @@ def write_joint_friction_to_sim( .. deprecated:: 2.1.0 Please use :meth:`write_joint_friction_coefficient_to_sim` instead. """ - omni.log.warn( + logger.warning( "The function 'write_joint_friction_to_sim' will be deprecated in a future release. Please" " use 'write_joint_friction_coefficient_to_sim' instead." ) @@ -1635,7 +2144,7 @@ def write_joint_limits_to_sim( .. deprecated:: 2.1.0 Please use :meth:`write_joint_position_limit_to_sim` instead. """ - omni.log.warn( + logger.warning( "The function 'write_joint_limits_to_sim' will be deprecated in a future release. Please" " use 'write_joint_position_limit_to_sim' instead." ) @@ -1654,7 +2163,7 @@ def set_fixed_tendon_limit( .. deprecated:: 2.1.0 Please use :meth:`set_fixed_tendon_position_limit` instead. """ - omni.log.warn( + logger.warning( "The function 'set_fixed_tendon_limit' will be deprecated in a future release. Please" " use 'set_fixed_tendon_position_limit' instead." ) diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_cfg.py b/source/isaaclab/isaaclab/assets/articulation/articulation_cfg.py index ac3697bda5ea..fad16cd8360a 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_cfg.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -38,6 +38,18 @@ class InitialStateCfg(AssetBaseCfg.InitialStateCfg): class_type: type = Articulation + articulation_root_prim_path: str | None = None + """Path to the articulation root prim under the :attr:`prim_path`. Defaults to None, in which case the class + will search for a prim with the USD ArticulationRootAPI on it. + + This path should be relative to the :attr:`prim_path` of the asset. If the asset is loaded from a USD file, + this path should be relative to the root of the USD stage. For instance, if the loaded USD file at :attr:`prim_path` + contains two articulations, one at `/robot1` and another at `/robot2`, and you want to use `robot2`, + then you should set this to `/robot2`. + + The path must 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.""" @@ -53,3 +65,7 @@ class InitialStateCfg(AssetBaseCfg.InitialStateCfg): actuators: dict[str, ActuatorBaseCfg] = MISSING """Actuators for the robot with corresponding joint names.""" + + actuator_value_resolution_debug_print = False + """Print the resolution of actuator final value when input cfg is different from USD value, Defaults to False + """ diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py index 80e9375b6e78..47902edc0a59 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py @@ -1,17 +1,22 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch +import logging import weakref -import omni.log +import torch + 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 +# import logger +logger = logging.getLogger(__name__) + class ArticulationData: """Data container for an articulation. @@ -48,9 +53,8 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): # Set initial time stamp self._sim_timestamp = 0.0 - # Obtain global physics sim view - self._physics_sim_view = physx.create_simulation_view("torch") - self._physics_sim_view.set_subspace_roots("/") + # 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) @@ -64,16 +68,31 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): 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() - self._body_acc_w = TimestampedBuffer() + # -- joint state self._joint_pos = TimestampedBuffer() - self._joint_acc = 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 @@ -95,12 +114,16 @@ def update(self, dt: float): fixed_tendon_names: list[str] = None """Fixed tendon names in the order parsed by the simulation view.""" + spatial_tendon_names: list[str] = None + """Spatial 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). + """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. @@ -133,8 +156,10 @@ 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. """ @@ -172,56 +197,105 @@ def update(self, dt: float): """ default_joint_friction_coeff: torch.Tensor = None - """Default joint friction coefficient of all joints. Shape is (num_instances, num_joints). + """Default joint static 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. + + Note: + In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + default_joint_dynamic_friction_coeff: torch.Tensor = None + """Default joint dynamic friction coefficient of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the actuator model's + :attr:`isaaclab.actuators.ActuatorBaseCfg.dynamic_friction` parameter. If the parameter's value is None, + the value parsed from the USD schema, at the time of initialization, is used. + + Note: + In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + default_joint_viscous_friction_coeff: torch.Tensor = None + """Default joint viscous friction coefficient of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the actuator model's + :attr:`isaaclab.actuators.ActuatorBaseCfg.viscous_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. + 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). + """Default tendon stiffness of all fixed 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). + """Default tendon damping of all fixed 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). + """Default tendon limit stiffness of all fixed 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). + """Default tendon rest length of all fixed 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). + """Default tendon offset of all fixed 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). + """Default tendon position limits of all fixed 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. """ + default_spatial_tendon_stiffness: torch.Tensor = None + """Default tendon stiffness of all spatial tendons. Shape is (num_instances, num_spatial_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_spatial_tendon_damping: torch.Tensor = None + """Default tendon damping of all spatial tendons. Shape is (num_instances, num_spatial_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_spatial_tendon_limit_stiffness: torch.Tensor = None + """Default tendon limit stiffness of all spatial tendons. Shape is (num_instances, num_spatial_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_spatial_tendon_offset: torch.Tensor = None + """Default tendon offset of all spatial tendons. Shape is (num_instances, num_spatial_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + ## # Joint commands -- Set into simulation. ## @@ -289,7 +363,21 @@ def update(self, dt: float): """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 static friction coefficient provided to the simulation. Shape is (num_instances, num_joints). + + Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + joint_dynamic_friction_coeff: torch.Tensor = None + """Joint dynamic friction coefficient provided to the simulation. Shape is (num_instances, num_joints). + + Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + joint_viscous_friction_coeff: torch.Tensor = None + """Joint viscous 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). @@ -359,9 +447,93 @@ def update(self, dt: float): """Fixed tendon position limits provided to the simulation. Shape is (num_instances, num_fixed_tendons, 2).""" ## - # Properties. + # Spatial tendon properties. + ## + + spatial_tendon_stiffness: torch.Tensor = None + """Spatial tendon stiffness provided to the simulation. Shape is (num_instances, num_spatial_tendons).""" + + spatial_tendon_damping: torch.Tensor = None + """Spatial tendon damping provided to the simulation. Shape is (num_instances, num_spatial_tendons).""" + + spatial_tendon_limit_stiffness: torch.Tensor = None + """Spatial tendon limit stiffness provided to the simulation. Shape is (num_instances, num_spatial_tendons).""" + + spatial_tendon_offset: torch.Tensor = None + """Spatial tendon offset provided to the simulation. Shape is (num_instances, num_spatial_tendons).""" + + ## + # 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_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 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). @@ -370,13 +542,9 @@ def root_state_w(self): the linear and angular velocities are of the articulation root's center of mass frame. """ if self._root_state_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") - velocity = self._root_physx_view.get_root_velocities() - # set the buffer data and timestamp - self._root_state_w.data = torch.cat((pose, velocity), dim=-1) + 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 @@ -387,63 +555,116 @@ def root_link_state_w(self): world. """ if self._root_link_state_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") - velocity = self._root_physx_view.get_root_velocities().clone() - - # adjust linear velocity to link from center of mass - velocity[:, :3] += torch.linalg.cross( - velocity[:, 3:], math_utils.quat_rotate(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1 - ) - # set the buffer data and timestamp - self._root_link_state_w.data = torch.cat((pose, velocity), dim=-1) + 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). + """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: - # read data from simulation (pose is of link) - pose = self._root_physx_view.get_root_transforms().clone() - pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz") - velocity = self._root_physx_view.get_root_velocities() + 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_apply(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). - # adjust pose to center of mass + 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( - pose[:, :3], pose[:, 3:7], self.com_pos_b[:, 0, :], self.com_quat_b[:, 0, :] + self.body_link_pos_w, self.body_link_quat_w, self.body_com_pos_b, self.body_com_quat_b ) - pose = torch.cat((pos, quat), dim=-1) # set the buffer data and timestamp - self._root_com_state_w.data = torch.cat((pose, velocity), dim=-1) - self._root_com_state_w.timestamp = self._sim_timestamp - return self._root_com_state_w.data + 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's actor frame. Meanwhile, the linear and angular + 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._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") - velocities = self._root_physx_view.get_link_velocities() - # set the buffer data and timestamp - self._body_state_w.data = torch.cat((poses, velocities), dim=-1) + 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 @@ -454,18 +675,7 @@ def body_link_state_w(self): 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._physics_sim_view.update_articulations_kinematic() - # read data from simulation - pose = self._root_physx_view.get_link_transforms().clone() - pose[..., 3:7] = math_utils.convert_quat(pose[..., 3:7], to="wxyz") - velocity = self._root_physx_view.get_link_velocities() - - # adjust linear velocity to link from center of mass - velocity[..., :3] += torch.linalg.cross( - velocity[..., 3:], math_utils.quat_rotate(pose[..., 3:7], -self.com_pos_b), dim=-1 - ) - # set the buffer data and timestamp - self._body_link_state_w.data = torch.cat((pose, velocity), dim=-1) + 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 @@ -480,50 +690,65 @@ def body_com_state_w(self): principle inertia. """ if self._body_com_state_w.timestamp < self._sim_timestamp: - self._physics_sim_view.update_articulations_kinematic() - # read data from simulation (pose is of link) - pose = self._root_physx_view.get_link_transforms().clone() - pose[..., 3:7] = math_utils.convert_quat(pose[..., 3:7], to="wxyz") - velocity = self._root_physx_view.get_link_velocities() - - # adjust pose to center of mass - pos, quat = math_utils.combine_frame_transforms( - pose[..., :3], pose[..., 3:7], self.com_pos_b, self.com_quat_b - ) - pose = torch.cat((pos, quat), dim=-1) - # set the buffer data and timestamp - self._body_com_state_w.data = torch.cat((pose, velocity), dim=-1) + 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_acc_w(self): - """Acceleration of all bodies (center of mass). Shape is (num_instances, num_bodies, 6). + 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_acc_w.timestamp < self._sim_timestamp: + if self._body_com_acc_w.timestamp < self._sim_timestamp: # read data from simulation and set the buffer data and timestamp - self._body_acc_w.data = self._root_physx_view.get_link_accelerations() + self._body_com_acc_w.data = self._root_physx_view.get_link_accelerations() + self._body_com_acc_w.timestamp = self._sim_timestamp - self._body_acc_w.timestamp = self._sim_timestamp - return self._body_acc_w.data + return self._body_com_acc_w.data @property - def projected_gravity_b(self): - """Projection of the gravity direction on base frame. Shape is (num_instances, 3).""" - return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W) + 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 heading_w(self): - """Yaw heading of the base frame (in radians). Shape is (num_instances,). + def body_incoming_joint_wrench_b(self) -> torch.Tensor: + """Joint reaction wrench applied from body parent to child body in parent body frame. - 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)`. + 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`_. + + .. _`PhysX documentation`: https://nvidia-omniverse.github.io/PhysX/physx/5.5.1/docs/Articulations.html#link-incoming-joint-force + .. _`PhysX Tensor API`: https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.ArticulationView.get_link_incoming_joint_force """ - forward_w = math_utils.quat_apply(self.root_link_quat_w, self.FORWARD_VEC_B) - return torch.atan2(forward_w[:, 1], forward_w[:, 0]) + + 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): @@ -556,70 +781,63 @@ def joint_acc(self): return self._joint_acc.data ## - # Derived properties. + # Derived Properties. ## @property - def root_pos_w(self) -> torch.Tensor: - """Root position in simulation world frame. Shape is (num_instances, 3). - - This quantity is the position of the actor frame of the articulation root relative to the world. - """ - return self.root_state_w[:, :3] - - @property - def root_quat_w(self) -> torch.Tensor: - """Root 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 articulation root relative to the world. - """ - return self.root_state_w[:, 3:7] + 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 root_vel_w(self) -> torch.Tensor: - """Root velocity in simulation world frame. Shape is (num_instances, 6). + def heading_w(self): + """Yaw heading of the base frame (in radians). Shape is (num_instances,). - This quantity contains the linear and angular velocities of the articulation root's center of mass frame - relative to the world. + 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)`. """ - return self.root_state_w[:, 7:13] + 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_lin_vel_w(self) -> torch.Tensor: - """Root linear velocity in simulation world frame. Shape is (num_instances, 3). + 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 center of mass frame relative to the world. + This quantity is the linear velocity of the articulation root's actor frame with respect to the + its actor frame. """ - return self.root_state_w[:, 7:10] + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_lin_vel_w) @property - def root_ang_vel_w(self) -> torch.Tensor: - """Root angular velocity in simulation world frame. Shape is (num_instances, 3). + 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 center of mass frame relative to the world. + This quantity is the angular velocity of the articulation root's actor frame with respect to the + its actor frame. """ - return self.root_state_w[:, 10:13] + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) @property - def root_lin_vel_b(self) -> torch.Tensor: - """Root linear velocity in base frame. Shape is (num_instances, 3). + 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 relative to the world - with respect to the articulation root's actor frame. + 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_rotate_inverse(self.root_quat_w, self.root_lin_vel_w) + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_lin_vel_w) @property - def root_ang_vel_b(self) -> torch.Tensor: - """Root angular velocity in base world frame. Shape is (num_instances, 3). + 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 relative to the world with - respect to the articulation root's actor frame. + 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_rotate_inverse(self.root_quat_w, self.root_ang_vel_w) + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_ang_vel_w) ## - # Derived Root Link Frame Properties + # Sliced properties. ## @property @@ -628,11 +846,7 @@ def root_link_pos_w(self) -> torch.Tensor: This quantity is the position of the actor frame of the root rigid body relative to the world. """ - if self._root_link_state_w.timestamp < self._sim_timestamp: - # read data from simulation (pose is of link) - pose = self._root_physx_view.get_root_transforms() - return pose[:, :3] - return self.root_link_state_w[:, :3] + return self.root_link_pose_w[:, :3] @property def root_link_quat_w(self) -> torch.Tensor: @@ -640,21 +854,7 @@ def root_link_quat_w(self) -> torch.Tensor: This quantity is the orientation of the actor frame of the root rigid body. """ - if self._root_link_state_w.timestamp < self._sim_timestamp: - # read data from simulation (pose is of link) - pose = self._root_physx_view.get_root_transforms().clone() - pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz") - return pose[:, 3:7] - return self.root_link_state_w[:, 3:7] - - @property - def root_link_vel_w(self) -> torch.Tensor: - """Root link velocity 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. - """ - return self.root_link_state_w[:, 7:13] + return self.root_link_pose_w[:, 3:7] @property def root_link_lin_vel_w(self) -> torch.Tensor: @@ -662,7 +862,7 @@ def root_link_lin_vel_w(self) -> torch.Tensor: This quantity is the linear velocity of the root rigid body's actor frame relative to the world. """ - return self.root_link_state_w[:, 7:10] + return self.root_link_vel_w[:, :3] @property def root_link_ang_vel_w(self) -> torch.Tensor: @@ -670,29 +870,7 @@ def root_link_ang_vel_w(self) -> torch.Tensor: This quantity is the angular velocity of the actor frame of the root rigid body relative to the world. """ - return self.root_link_state_w[:, 10:13] - - @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_rotate_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_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) - - ## - # Root Center of Mass state properties - ## + return self.root_link_vel_w[:, 3:6] @property def root_com_pos_w(self) -> torch.Tensor: @@ -700,7 +878,7 @@ def root_com_pos_w(self) -> torch.Tensor: This quantity is the position of the actor frame of the root rigid body relative to the world. """ - return self.root_com_state_w[:, :3] + return self.root_com_pose_w[:, :3] @property def root_com_quat_w(self) -> torch.Tensor: @@ -708,19 +886,7 @@ def root_com_quat_w(self) -> torch.Tensor: This quantity is the orientation of the actor frame of the root rigid body relative to the world. """ - return self.root_com_state_w[:, 3:7] - - @property - def root_com_vel_w(self) -> torch.Tensor: - """Root center of mass velocity 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_state_w.timestamp < self._sim_timestamp: - # read data from simulation (pose is of link) - velocity = self._root_physx_view.get_root_velocities() - return velocity - return self.root_com_state_w[:, 7:13] + return self.root_com_pose_w[:, 3:7] @property def root_com_lin_vel_w(self) -> torch.Tensor: @@ -728,11 +894,7 @@ def root_com_lin_vel_w(self) -> torch.Tensor: This quantity is the linear velocity of the root rigid body's center of mass frame relative to the world. """ - if self._root_com_state_w.timestamp < self._sim_timestamp: - # read data from simulation (pose is of link) - velocity = self._root_physx_view.get_root_velocities() - return velocity[:, 0:3] - return self.root_com_state_w[:, 7:10] + return self.root_com_vel_w[:, :3] @property def root_com_ang_vel_w(self) -> torch.Tensor: @@ -740,228 +902,210 @@ def root_com_ang_vel_w(self) -> torch.Tensor: This quantity is the angular velocity of the root rigid body's center of mass frame relative to the world. """ - if self._root_com_state_w.timestamp < self._sim_timestamp: - self._physics_sim_view.update_articulations_kinematic() - # read data from simulation (pose is of link) - velocity = self._root_physx_view.get_root_velocities() - return velocity[:, 3:6] - return self.root_com_state_w[:, 10:13] + return self.root_com_vel_w[:, 3:6] @property - def root_com_lin_vel_b(self) -> torch.Tensor: - """Root center of mass linear velocity in base frame. Shape is (num_instances, 3). + 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 linear velocity of the root rigid body's center of mass frame with respect to the - rigid body's actor frame. + This quantity is the position of the articulation bodies' actor frame relative to the world. """ - return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_lin_vel_w) + return self.body_link_pose_w[..., :3] @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). + 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 angular velocity of the root rigid body's center of mass frame with respect to the - rigid body's actor frame. + This quantity is the orientation of the articulation bodies' actor frame relative to the world. """ - return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_ang_vel_w) + return self.body_link_pose_w[..., 3:7] @property - def body_pos_w(self) -> torch.Tensor: - """Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + 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 position of the rigid bodies' actor frame relative to the world. + This quantity is the linear velocity of the articulation bodies' center of mass frame relative to the world. """ - return self.body_state_w[..., :3] + return self.body_link_vel_w[..., :3] @property - def body_quat_w(self) -> torch.Tensor: - """Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 4). + 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 orientation of the rigid bodies' actor frame relative to the world. + This quantity is the angular velocity of the articulation bodies' center of mass frame relative to the world. """ - return self.body_state_w[..., 3:7] + return self.body_link_vel_w[..., 3:6] @property - def body_vel_w(self) -> torch.Tensor: - """Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6). + 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 contains the linear and angular velocities of the rigid bodies' center of mass frame relative - to the world. + This quantity is the position of the articulation bodies' actor frame. """ - return self.body_state_w[..., 7:13] + return self.body_com_pose_w[..., :3] @property - def body_lin_vel_w(self) -> torch.Tensor: + 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 rigid bodies' center of mass frame relative to the world. + This quantity is the linear velocity of the articulation bodies' center of mass frame. """ - return self.body_state_w[..., 7:10] + return self.body_com_vel_w[..., :3] @property - def body_ang_vel_w(self) -> torch.Tensor: + 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 rigid bodies' center of mass frame relative to the world. + This quantity is the angular velocity of the articulation bodies' center of mass frame. """ - return self.body_state_w[..., 10:13] + return self.body_com_vel_w[..., 3:6] @property - def body_lin_acc_w(self) -> torch.Tensor: + 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 rigid bodies' center of mass frame relative to the world. + This quantity is the linear acceleration of the articulation bodies' center of mass frame. """ - return self.body_acc_w[..., 0:3] + return self.body_com_acc_w[..., :3] @property - def body_ang_acc_w(self) -> torch.Tensor: + 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 rigid bodies' center of mass frame relative to the world. + This quantity is the angular acceleration of the articulation bodies' center of mass frame. """ - return self.body_acc_w[..., 3:6] - - ## - # Link body properties - ## + return self.body_com_acc_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). + 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 position of the rigid bodies' actor frame relative to the world. + This quantity is the center of mass location relative to its body'slink frame. """ - if self._body_link_state_w.timestamp < self._sim_timestamp: - self._physics_sim_view.update_articulations_kinematic() - # read data from simulation - pose = self._root_physx_view.get_link_transforms() - return pose[..., :3] - return self._body_link_state_w.data[..., :3] + return self.body_com_pose_b[..., :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). + 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 rigid bodies' actor frame relative to the world. + This quantity is the orientation of the principles axes of inertia relative to its body's link frame. """ - if self._body_link_state_w.timestamp < self._sim_timestamp: - self._physics_sim_view.update_articulations_kinematic() - # read data from simulation - pose = self._root_physx_view.get_link_transforms().clone() - pose[..., 3:7] = math_utils.convert_quat(pose[..., 3:7], to="wxyz") - return pose[..., 3:7] - return self.body_link_state_w[..., 3:7] + return self.body_com_pose_b[..., 3:7] + + ## + # Backward compatibility. + ## @property - def body_link_vel_w(self) -> torch.Tensor: - """Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6). + def root_pose_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pose_w`.""" + return self.root_link_pose_w - This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame - relative to the world. - """ - return self.body_link_state_w[..., 7:13] + @property + def root_pos_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pos_w`.""" + return self.root_link_pos_w @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). + def root_quat_w(self) -> torch.Tensor: + """Same as :attr:`root_link_quat_w`.""" + return self.root_link_quat_w - This quantity is the linear velocity of the rigid bodies' center of mass frame relative to the world. - """ - return self.body_link_state_w[..., 7:10] + @property + def root_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_vel_w`.""" + return self.root_com_vel_w @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). + def root_lin_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_lin_vel_w`.""" + return self.root_com_lin_vel_w - This quantity is the angular velocity of the rigid bodies' center of mass frame relative to the world. - """ - return self.body_link_state_w[..., 10:13] + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_w`.""" + return self.root_com_ang_vel_w - ## - # Center of mass body properties - ## + @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 body_com_pos_w(self) -> torch.Tensor: - """Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + def root_ang_vel_b(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_b`.""" + return self.root_com_ang_vel_b - This quantity is the position of the rigid bodies' actor frame. - """ - return self.body_com_state_w[..., :3] + @property + def body_pose_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pose_w`.""" + return self.body_link_pose_w @property - def body_com_quat_w(self) -> torch.Tensor: - """Orientation (w, x, y, z) of the prinicple axies of inertia of all bodies in simulation world frame. + def body_pos_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pos_w`.""" + return self.body_link_pos_w - Shape is (num_instances, num_bodies, 4). This quantity is the orientation of the rigid bodies' actor frame. - """ - return self.body_com_state_w[..., 3:7] + @property + def body_quat_w(self) -> torch.Tensor: + """Same as :attr:`body_link_quat_w`.""" + return self.body_link_quat_w @property - def body_com_vel_w(self) -> torch.Tensor: - """Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6). + def body_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_vel_w`.""" + return self.body_com_vel_w - This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame. - """ - if self._body_com_state_w.timestamp < self._sim_timestamp: - self._physics_sim_view.update_articulations_kinematic() - # read data from simulation (velocity is of com) - velocity = self._root_physx_view.get_link_velocities() - return velocity - return self.body_com_state_w[..., 7:13] + @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_com_lin_vel_w(self) -> torch.Tensor: - """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + def body_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_ang_vel_w`.""" + return self.body_com_ang_vel_w - This quantity is the linear velocity of the rigid bodies' center of mass frame. - """ - if self._body_com_state_w.timestamp < self._sim_timestamp: - self._physics_sim_view.update_articulations_kinematic() - # read data from simulation (velocity is of com) - velocity = self._root_physx_view.get_link_velocities() - return velocity[..., 0:3] - return self.body_com_state_w[..., 7:10] + @property + def body_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_acc_w`.""" + return self.body_com_acc_w @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). + def body_lin_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_lin_acc_w`.""" + return self.body_com_lin_acc_w - This quantity is the angular velocity of the rigid bodies' center of mass frame. - """ - if self._body_com_state_w.timestamp < self._sim_timestamp: - self._physics_sim_view.update_articulations_kinematic() - # read data from simulation (velocity is of com) - velocity = self._root_physx_view.get_link_velocities() - return velocity[..., 3:6] - return self.body_com_state_w[..., 10:13] + @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: - """Center of mass of all of the bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). - - This quantity is the center of mass location relative to its body frame. - """ - return self._root_physx_view.get_coms().to(self.device)[..., :3] + """Same as :attr:`body_com_pos_b`.""" + return self.body_com_pos_b @property def com_quat_b(self) -> torch.Tensor: - """Orientation (w,x,y,z) of the principle axies of inertia of all of the bodies in simulation world frame. Shape is (num_instances, num_bodies, 4). - - This quantity is the orientation of the principles axes of inertia relative to its body frame. - """ - quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7] - return math_utils.convert_quat(quat, to="wxyz") - - ## - # Backward compatibility. - ## + """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( + logger.warning( "The `joint_limits` property will be deprecated in a future release. Please use `joint_pos_limits` instead." ) return self.joint_pos_limits @@ -969,7 +1113,7 @@ def joint_limits(self) -> torch.Tensor: @property def default_joint_limits(self) -> torch.Tensor: """Deprecated property. Please use :attr:`default_joint_pos_limits` instead.""" - omni.log.warn( + logger.warning( "The `default_joint_limits` property will be deprecated in a future release. Please use" " `default_joint_pos_limits` instead." ) @@ -978,7 +1122,7 @@ def default_joint_limits(self) -> torch.Tensor: @property def joint_velocity_limits(self) -> torch.Tensor: """Deprecated property. Please use :attr:`joint_vel_limits` instead.""" - omni.log.warn( + logger.warning( "The `joint_velocity_limits` property will be deprecated in a future release. Please use" " `joint_vel_limits` instead." ) @@ -987,7 +1131,7 @@ def joint_velocity_limits(self) -> torch.Tensor: @property def joint_friction(self) -> torch.Tensor: """Deprecated property. Please use :attr:`joint_friction_coeff` instead.""" - omni.log.warn( + logger.warning( "The `joint_friction` property will be deprecated in a future release. Please use" " `joint_friction_coeff` instead." ) @@ -996,7 +1140,7 @@ def joint_friction(self) -> torch.Tensor: @property def default_joint_friction(self) -> torch.Tensor: """Deprecated property. Please use :attr:`default_joint_friction_coeff` instead.""" - omni.log.warn( + logger.warning( "The `default_joint_friction` property will be deprecated in a future release. Please use" " `default_joint_friction_coeff` instead." ) @@ -1005,7 +1149,7 @@ def default_joint_friction(self) -> torch.Tensor: @property def fixed_tendon_limit(self) -> torch.Tensor: """Deprecated property. Please use :attr:`fixed_tendon_pos_limits` instead.""" - omni.log.warn( + logger.warning( "The `fixed_tendon_limit` property will be deprecated in a future release. Please use" " `fixed_tendon_pos_limits` instead." ) @@ -1014,7 +1158,7 @@ def fixed_tendon_limit(self) -> torch.Tensor: @property def default_fixed_tendon_limit(self) -> torch.Tensor: """Deprecated property. Please use :attr:`default_fixed_tendon_pos_limits` instead.""" - omni.log.warn( + logger.warning( "The `default_fixed_tendon_limit` property will be deprecated in a future release. Please use" " `default_fixed_tendon_pos_limits` instead." ) diff --git a/source/isaaclab/isaaclab/assets/asset_base.py b/source/isaaclab/isaaclab/assets/asset_base.py index be544504c4cc..158b69933510 100644 --- a/source/isaaclab/isaaclab/assets/asset_base.py +++ b/source/isaaclab/isaaclab/assets/asset_base.py @@ -1,10 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +import builtins import inspect import re import weakref @@ -12,10 +13,14 @@ from collections.abc import Sequence from typing import TYPE_CHECKING, Any +import torch + import omni.kit.app import omni.timeline +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager import isaaclab.sim as sim_utils +from isaaclab.sim.utils.stage import get_current_stage if TYPE_CHECKING: from .asset_base_cfg import AssetBaseCfg @@ -65,6 +70,8 @@ def __init__(self, cfg: AssetBaseCfg): self.cfg = cfg.copy() # flag for whether the asset is initialized self._is_initialized = False + # get stage handle + self.stage = get_current_stage() # check if base asset path is valid # note: currently the spawner does not work if there is a regex pattern in the leaf @@ -85,20 +92,9 @@ def __init__(self, cfg: AssetBaseCfg): 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, - ) + # register various callback functions + self._register_callbacks() + # 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 @@ -106,17 +102,8 @@ def __init__(self, cfg: AssetBaseCfg): def __del__(self): """Unsubscribe from the callbacks.""" - # clear physics events handles - 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 + # clear events handles + self._clear_callbacks() """ Properties @@ -161,6 +148,36 @@ def has_debug_vis_implementation(self) -> bool: 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: + sim_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. @@ -247,6 +264,43 @@ def _debug_vis_callback(self, event): Internal simulation callbacks. """ + def _register_callbacks(self): + """Registers the timeline and prim deletion callbacks.""" + + # register simulator callbacks (with weakref safety to avoid crashes on deletion) + def safe_callback(callback_name, event, obj_ref): + """Safely invoke a callback on a weakly-referenced object, ignoring ReferenceError if deleted.""" + try: + obj = obj_ref + getattr(obj, callback_name)(event) + except ReferenceError: + # Object has been deleted; ignore. + pass + + # note: use weakref on callbacks to ensure that this object can be deleted when its destructor is called. + # add callbacks for stage play/stop + obj_ref = weakref.proxy(self) + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + + # the order is set to 10 which is arbitrary but should be lower priority than the default order of 0 + # register timeline PLAY event callback (lower priority with order=10) + self._initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj_ref=obj_ref: safe_callback("_initialize_callback", event, obj_ref), + order=10, + ) + # register timeline STOP event callback (lower priority with order=10) + self._invalidate_initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event, obj_ref=obj_ref: safe_callback("_invalidate_initialize_callback", event, obj_ref), + order=10, + ) + # register prim deletion callback + self._prim_deletion_callback_id = SimulationManager.register_callback( + lambda event, obj_ref=obj_ref: safe_callback("_on_prim_deletion", event, obj_ref), + event=IsaacEvents.PRIM_DELETION, + ) + def _initialize_callback(self, event): """Initializes the scene elements. @@ -256,16 +310,54 @@ def _initialize_callback(self, event): """ if not self._is_initialized: # obtain simulation related information - sim = sim_utils.SimulationContext.instance() - if sim is None: - raise RuntimeError("SimulationContext is not initialized! Please initialize SimulationContext first.") - self._backend = sim.backend - self._device = sim.device + self._backend = SimulationManager.get_backend() + self._device = SimulationManager.get_physics_sim_device() # initialize the asset - self._initialize_impl() + 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/source/isaaclab/isaaclab/assets/asset_base_cfg.py b/source/isaaclab/isaaclab/assets/asset_base_cfg.py index 4d305d32b9f9..a9a8a1d471a8 100644 --- a/source/isaaclab/isaaclab/assets/asset_base_cfg.py +++ b/source/isaaclab/isaaclab/assets/asset_base_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/assets/deformable_object/__init__.py b/source/isaaclab/isaaclab/assets/deformable_object/__init__.py index dcad4e086491..503a238935bd 100644 --- a/source/isaaclab/isaaclab/assets/deformable_object/__init__.py +++ b/source/isaaclab/isaaclab/assets/deformable_object/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py index 2e5969c8a453..12ae6c51ad8f 100644 --- a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py @@ -1,16 +1,18 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch + 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 @@ -23,6 +25,9 @@ if TYPE_CHECKING: from .deformable_object_cfg import DeformableObjectCfg +# import logger +logger = logging.getLogger(__name__) + class DeformableObject(AssetBase): """A deformable object asset class. @@ -207,7 +212,8 @@ def write_nodal_kinematic_target_to_sim(self, targets: torch.Tensor, env_ids: Se """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. + 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. @@ -261,9 +267,8 @@ def transform_nodal_pos( """ def _initialize_impl(self): - # create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") + # 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: @@ -272,7 +277,9 @@ def _initialize_impl(self): # find deformable root prims root_prims = sim_utils.get_all_matching_child_prims( - template_prim_path, predicate=lambda prim: prim.HasAPI(PhysxSchema.PhysxDeformableBodyAPI) + template_prim_path, + predicate=lambda prim: prim.HasAPI(PhysxSchema.PhysxDeformableBodyAPI), + traverse_instance_prims=False, ) if len(root_prims) == 0: raise RuntimeError( @@ -305,7 +312,7 @@ def _initialize_impl(self): material_prim = mat_prim break if material_prim is None: - omni.log.info( + logger.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" @@ -341,14 +348,14 @@ def _initialize_impl(self): 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}") + logger.info(f"Deformable body initialized at: {root_prim_path_expr}") + logger.info(f"Number of instances: {self.num_instances}") + logger.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}") + logger.info(f"Deformable material initialized at: {material_prim_path_expr}") + logger.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.") + logger.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) @@ -358,6 +365,11 @@ def _initialize_impl(self): # update the deformable body data self.update(0.0) + # Initialize debug visualization handle + if self._debug_vis_handle is None: + # set initial state of debug visualization + self.set_debug_vis(self.cfg.debug_vis) + def _create_buffers(self): """Create buffers for storing data.""" # constants @@ -408,6 +420,4 @@ 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._physics_sim_view = None self._root_physx_view = None diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_cfg.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_cfg.py index 70defbb70015..9ef06d1b54c3 100644 --- a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_cfg.py +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_data.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_data.py index 9385bc846522..bb7714383e06 100644 --- a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_data.py +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_data.py @@ -1,11 +1,12 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch import weakref +import torch + import omni.physics.tensors.impl.api as physx import isaaclab.utils.math as math_utils @@ -122,10 +123,7 @@ def nodal_state_w(self): Shape is (num_instances, max_sim_vertices_per_body, 6). """ if self._nodal_state_w.timestamp < self._sim_timestamp: - nodal_positions = self.nodal_pos_w - nodal_velocities = self.nodal_vel_w - # set the buffer data and timestamp - self._nodal_state_w.data = torch.cat((nodal_positions, nodal_velocities), dim=-1) + 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 @@ -221,8 +219,8 @@ def collision_element_stress_w(self): @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). + """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. """ diff --git a/source/isaaclab/isaaclab/assets/rigid_object/__init__.py b/source/isaaclab/isaaclab/assets/rigid_object/__init__.py index ca532d5aa567..1598c060f1a4 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/__init__.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py index f873cd253087..6d0ec98f4314 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py @@ -1,21 +1,25 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch +import warp as wp + 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 isaaclab.utils.wrench_composer import WrenchComposer from ..asset_base import AssetBase from .rigid_object_data import RigidObjectData @@ -23,6 +27,9 @@ if TYPE_CHECKING: from .rigid_object_cfg import RigidObjectCfg +# import logger +logger = logging.getLogger(__name__) + class RigidObject(AssetBase): """A rigid object asset class. @@ -91,6 +98,27 @@ def root_physx_view(self) -> physx.RigidBodyView: """ return self._root_physx_view + @property + def instantaneous_wrench_composer(self) -> WrenchComposer: + """Instantaneous wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are only valid for the current simulation step. At the end of the simulation step, the wrenches set + to this object are discarded. This is useful to apply forces that change all the time, things like drag forces + for instance. + """ + return self._instantaneous_wrench_composer + + @property + def permanent_wrench_composer(self) -> WrenchComposer: + """Permanent wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are persistent and are applied to the simulation at every step. This is useful to apply forces that + are constant over a period of time, things like the thrust of a motor for instance. + """ + return self._permanent_wrench_composer + """ Operations. """ @@ -100,8 +128,8 @@ def reset(self, env_ids: Sequence[int] | None = None): 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 + self._instantaneous_wrench_composer.reset(env_ids) + self._permanent_wrench_composer.reset(env_ids) def write_data_to_sim(self): """Write external wrench to the simulation. @@ -111,14 +139,33 @@ def write_data_to_sim(self): 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, - ) + if self._instantaneous_wrench_composer.active or self._permanent_wrench_composer.active: + if self._instantaneous_wrench_composer.active: + # Compose instantaneous wrench with permanent wrench + self._instantaneous_wrench_composer.add_forces_and_torques( + forces=self._permanent_wrench_composer.composed_force, + torques=self._permanent_wrench_composer.composed_torque, + body_ids=self._ALL_BODY_INDICES_WP, + env_ids=self._ALL_INDICES_WP, + ) + # Apply both instantaneous and permanent wrench to the simulation + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._instantaneous_wrench_composer.composed_force_as_torch.view(-1, 3), + torque_data=self._instantaneous_wrench_composer.composed_torque_as_torch.view(-1, 3), + position_data=None, + indices=self._ALL_INDICES, + is_global=False, + ) + else: + # Apply permanent wrench to the simulation + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._permanent_wrench_composer.composed_force_as_torch.view(-1, 3), + torque_data=self._permanent_wrench_composer.composed_torque_as_torch.view(-1, 3), + position_data=None, + indices=self._ALL_INDICES, + is_global=False, + ) + self._instantaneous_wrench_composer.reset() def update(self, dt: float): self._data.update(dt) @@ -156,10 +203,8 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """ - - # set into simulation - self.write_root_pose_to_sim(root_state[:, :7], env_ids=env_ids) - self.write_root_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + 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. @@ -171,7 +216,6 @@ def write_root_com_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequenc root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """ - # set into simulation 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) @@ -185,7 +229,6 @@ def write_root_link_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequen root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """ - # set into simulation 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) @@ -195,22 +238,10 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] 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). + 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_state_w[env_ids, :7] = root_pose.clone() - # convert root quaternion from wxyz to xyzw - root_poses_xyzw = self._data.root_state_w[:, :7].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) + 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. @@ -218,7 +249,7 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence 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). + 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 @@ -226,12 +257,26 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence 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_state_w[env_ids, :7] = root_pose.clone() - self._data.root_state_w[env_ids, :7] = self._data.root_link_state_w[env_ids, :7] + 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] + if self._data._root_com_state_w.data is not None: + expected_com_pos, expected_com_quat = math_utils.combine_frame_transforms( + self._data.root_link_pose_w[env_ids, :3], + self._data.root_link_pose_w[env_ids, 3:7], + self.data.body_com_pos_b[env_ids, 0, :], + self.data.body_com_quat_b[env_ids, 0, :], + ) + self._data.root_com_state_w[env_ids, :3] = expected_com_pos + self._data.root_com_state_w[env_ids, 3:7] = expected_com_quat # convert root quaternion from wxyz to xyzw - root_poses_xyzw = self._data.root_link_state_w[:, :7].clone() + 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) @@ -252,18 +297,26 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[ else: local_env_ids = env_ids - com_pos = self.data.com_pos_b[local_env_ids, 0, :] - com_quat = self.data.com_quat_b[local_env_ids, 0, :] - + # 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_rotate(math_utils.quat_inv(com_quat), -com_pos), - math_utils.quat_inv(com_quat), + 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) - self.write_root_link_pose_to_sim(root_pose=root_link_pose, env_ids=env_ids) + + # 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. @@ -275,17 +328,7 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque 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_state_w[env_ids, 7:] = root_velocity.clone() - self._data.body_acc_w[env_ids] = 0.0 - # set into simulation - self.root_physx_view.set_velocities(self._data.root_state_w[:, 7:], indices=physx_env_ids) + 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. @@ -297,19 +340,26 @@ def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: S 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_state_w[env_ids, 7:] = root_velocity.clone() - self._data.root_state_w[env_ids, 7:] = self._data.root_com_state_w[env_ids, 7:] - self._data.body_acc_w[env_ids] = 0.0 + 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] + if self._data._root_link_state_w.data is not None: + self._data.root_link_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_state_w[:, 7:], indices=physx_env_ids) + 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. @@ -327,15 +377,23 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, 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() - quat = self.data.root_link_state_w[local_env_ids, 3:7] - com_pos_b = self.data.com_pos_b[local_env_ids, 0, :] - # transform given velocity to center of mass root_com_velocity[:, :3] += torch.linalg.cross( - root_com_velocity[:, 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1 + root_com_velocity[:, 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1 ) - # write center of mass velocity to sim - self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=env_ids) + + # write transformed velocity in CoM frame to sim + self.write_root_com_velocity_to_sim(root_com_velocity, env_ids=env_ids) """ Operations - Setters. @@ -345,14 +403,17 @@ def set_external_force_and_torque( self, forces: torch.Tensor, torques: torch.Tensor, + positions: torch.Tensor | None = None, body_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None, + is_global: bool = False, ): """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. + into buffers which are then applied to the simulation at every step. Optionally, set the position to apply the + external wrench at (in the local link frame of the bodies). .. caution:: If the function is called with empty forces and torques, then this function disables the application @@ -371,38 +432,48 @@ def set_external_force_and_torque( 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). + positions: External wrench positions in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). + Defaults to None. 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). + is_global: Whether to apply the external wrench in the global frame. Defaults to False. If set to False, + the external wrench is applied in the link frame of the bodies. """ - 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 + logger.warning( + "The function 'set_external_force_and_torque' will be deprecated in a future release. Please" + " use 'permanent_wrench_composer.set_forces_and_torques' instead." + ) + if forces is None and torques is None: + logger.warning("No forces or torques provided. No permanent external wrench will be applied.") # resolve all indices # -- env_ids if env_ids is None: - env_ids = slice(None) + env_ids = self._ALL_INDICES_WP + elif not isinstance(env_ids, torch.Tensor): + env_ids = wp.array(env_ids, dtype=wp.int32, device=self.device) + else: + env_ids = wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) # -- 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 + body_ids = self._ALL_BODY_INDICES_WP + + # Write to wrench composer + self._permanent_wrench_composer.set_forces_and_torques( + forces=wp.from_torch(forces, dtype=wp.vec3f) if forces is not None else None, + torques=wp.from_torch(torques, dtype=wp.vec3f) if torques is not None else None, + positions=wp.from_torch(positions, dtype=wp.vec3f) if positions is not None else None, + body_ids=body_ids, + env_ids=env_ids, + is_global=is_global, + ) """ Internal helper. """ def _initialize_impl(self): - # create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") + # 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: @@ -411,7 +482,9 @@ def _initialize_impl(self): # find rigid root prims root_prims = sim_utils.get_all_matching_child_prims( - template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.RigidBodyAPI) + template_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.RigidBodyAPI), + traverse_instance_prims=False, ) if len(root_prims) == 0: raise RuntimeError( @@ -426,7 +499,9 @@ def _initialize_impl(self): ) articulation_prims = sim_utils.get_all_matching_child_prims( - template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI) + template_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), + traverse_instance_prims=False, ) if len(articulation_prims) != 0: if articulation_prims[0].GetAttribute("physxArticulation:articulationEnabled").Get(): @@ -448,10 +523,10 @@ def _initialize_impl(self): 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}") + logger.info(f"Rigid body initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.") + logger.info(f"Number of instances: {self.num_instances}") + logger.info(f"Number of bodies: {self.num_bodies}") + logger.info(f"Body names: {self.body_names}") # container for data access self._data = RigidObjectData(self.root_physx_view, self.device) @@ -467,11 +542,14 @@ def _create_buffers(self): """Create buffers for storing data.""" # constants self._ALL_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) + self._ALL_INDICES_WP = wp.from_torch(self._ALL_INDICES.to(torch.int32), dtype=wp.int32) + self._ALL_BODY_INDICES_WP = wp.from_torch( + torch.arange(self.num_bodies, dtype=torch.int32, device=self.device), dtype=wp.int32 + ) - # 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) + # external wrench composer + self._instantaneous_wrench_composer = WrenchComposer(self) + self._permanent_wrench_composer = WrenchComposer(self) # set information about rigid body into data self._data.body_names = self.body_names @@ -501,5 +579,4 @@ def _invalidate_initialize_callback(self, event): # call parent super()._invalidate_initialize_callback(event) # set all existing views to None to invalidate them - self._physics_sim_view = None self._root_physx_view = None diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_cfg.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_cfg.py index f7115976f05d..8340aa45f763 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_cfg.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause 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 b51f2f30b619..d1a94f1eac76 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py @@ -1,14 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch import weakref +import torch + import omni.physics.tensors.impl.api as physx import isaaclab.utils.math as math_utils +from isaaclab.sim.utils.stage import get_current_stage_id from isaaclab.utils.buffers import TimestampedBuffer @@ -51,7 +53,8 @@ def __init__(self, root_physx_view: physx.RigidBodyView, device: str): self._sim_timestamp = 0.0 # Obtain global physics sim view - physics_sim_view = physx.create_simulation_view("torch") + stage_id = get_current_stage_id() + physics_sim_view = physx.create_simulation_view("torch", stage_id) physics_sim_view.set_subspace_roots("/") gravity = physics_sim_view.get_gravity() # Convert to direction vector @@ -63,10 +66,19 @@ def __init__(self, root_physx_view: physx.RigidBodyView, device: str): 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() - self._body_acc_w = TimestampedBuffer() def update(self, dt: float): """Updates the data for the rigid object. @@ -101,197 +113,287 @@ 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. """ ## - # Properties. + # Root state properties. ## @property - def root_state_w(self): - """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). + def root_link_pose_w(self) -> torch.Tensor: + """Root link pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, 7). - 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. + 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_state_w.timestamp < self._sim_timestamp: + 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") - velocity = self._root_physx_view.get_velocities() # set the buffer data and timestamp - self._root_state_w.data = torch.cat((pose, velocity), dim=-1) + 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): + 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. + world. The orientation is provided in (w, x, y, z) format. """ if self._root_link_state_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") - velocity = self._root_physx_view.get_velocities().clone() - - # adjust linear velocity to link from center of mass - velocity[:, :3] += torch.linalg.cross( - velocity[:, 3:], math_utils.quat_rotate(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1 - ) - # set the buffer data and timestamp - self._root_link_state_w.data = torch.cat((pose, velocity), dim=-1) + 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). + 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: - # read data from simulation (pose is of link) - pose = self._root_physx_view.get_transforms().clone() - pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz") - velocity = self._root_physx_view.get_velocities() - - # adjust pose to center of mass - pos, quat = math_utils.combine_frame_transforms( - pose[:, :3], pose[:, 3:7], self.com_pos_b[:, 0, :], self.com_quat_b[:, 0, :] - ) - pose = torch.cat((pos, quat), dim=-1) - # set the buffer data and timestamp - self._root_com_state_w.data = torch.cat((pos, quat, velocity), dim=-1) + 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): - """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame. Shape is (num_instances, 1, 13). + 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): - """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame. + 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): - """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame. + 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. + 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_acc_w(self): - """Acceleration of all bodies. Shape is (num_instances, 1, 6). + 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. + This quantity is the acceleration of the rigid bodies' center of mass frame relative to the world. """ - if self._body_acc_w.timestamp < self._sim_timestamp: - # note: we use finite differencing to compute acceleration - self._body_acc_w.data = self._root_physx_view.get_accelerations().unsqueeze(1) - self._body_acc_w.timestamp = self._sim_timestamp - return self._body_acc_w.data + 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 - @property - def projected_gravity_b(self): - """Projection of the gravity direction on base frame. Shape is (num_instances, 3).""" - return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W) + return self._body_com_acc_w.data @property - def heading_w(self): - """Yaw heading of the base frame (in radians). Shape is (num_instances,). + 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). - 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)`. + 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. """ - forward_w = math_utils.quat_apply(self.root_link_quat_w, self.FORWARD_VEC_B) - return torch.atan2(forward_w[:, 1], forward_w[:, 0]) + 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. + # Derived Properties. ## @property - def root_pos_w(self) -> torch.Tensor: - """Root position in simulation world frame. Shape is (num_instances, 3). - - This quantity is the position of the actor frame of the root rigid body. - """ - return self.root_state_w[:, :3] - - @property - def root_quat_w(self) -> torch.Tensor: - """Root 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_state_w[:, 3:7] + 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 root_vel_w(self) -> torch.Tensor: - """Root velocity in simulation world frame. Shape is (num_instances, 6). + def heading_w(self) -> torch.Tensor: + """Yaw heading of the base frame (in radians). Shape is (num_instances,). - This quantity contains the linear and angular velocities of the root rigid body's center of mass frame. + 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)`. """ - return self.root_state_w[:, 7:13] + 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_lin_vel_w(self) -> torch.Tensor: - """Root linear velocity in simulation world frame. Shape is (num_instances, 3). + 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 root rigid body's center of mass frame relative to the world. + 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 self.root_state_w[:, 7:10] + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_lin_vel_w) @property - def root_ang_vel_w(self) -> torch.Tensor: - """Root angular velocity in simulation world frame. Shape is (num_instances, 3). + 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 root rigid body's center of mass frame. + 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 self.root_state_w[:, 10:13] + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) @property - def root_lin_vel_b(self) -> torch.Tensor: - """Root linear velocity in base frame. Shape is (num_instances, 3). + 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_rotate_inverse(self.root_link_quat_w, self.root_lin_vel_w) + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_lin_vel_w) @property - def root_ang_vel_b(self) -> torch.Tensor: - """Root angular velocity in base world frame. Shape is (num_instances, 3). + 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_rotate_inverse(self.root_link_quat_w, self.root_ang_vel_w) + 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: @@ -299,11 +401,7 @@ def root_link_pos_w(self) -> torch.Tensor: This quantity is the position of the actor frame of the root rigid body relative to the world. """ - if self._root_link_state_w.timestamp < self._sim_timestamp: - # read data from simulation - pose = self._root_physx_view.get_transforms() - return pose[:, :3] - return self.root_link_state_w[:, :3] + return self.root_link_pose_w[:, :3] @property def root_link_quat_w(self) -> torch.Tensor: @@ -311,21 +409,7 @@ def root_link_quat_w(self) -> torch.Tensor: This quantity is the orientation of the actor frame of the root rigid body. """ - if self._root_link_state_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") - return pose[:, 3:7] - return self.root_link_state_w[:, 3:7] - - @property - def root_link_vel_w(self) -> torch.Tensor: - """Root link velocity 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. - """ - return self.root_link_state_w[:, 7:13] + return self.root_link_pose_w[:, 3:7] @property def root_link_lin_vel_w(self) -> torch.Tensor: @@ -333,7 +417,7 @@ def root_link_lin_vel_w(self) -> torch.Tensor: This quantity is the linear velocity of the root rigid body's actor frame relative to the world. """ - return self.root_link_state_w[:, 7:10] + return self.root_link_vel_w[:, :3] @property def root_link_ang_vel_w(self) -> torch.Tensor: @@ -341,25 +425,7 @@ def root_link_ang_vel_w(self) -> torch.Tensor: This quantity is the angular velocity of the actor frame of the root rigid body relative to the world. """ - return self.root_link_state_w[:, 10:13] - - @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_rotate_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_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) + return self.root_link_vel_w[:, 3:6] @property def root_com_pos_w(self) -> torch.Tensor: @@ -367,7 +433,7 @@ def root_com_pos_w(self) -> torch.Tensor: This quantity is the position of the actor frame of the root rigid body relative to the world. """ - return self.root_com_state_w[:, :3] + return self.root_com_pose_w[:, :3] @property def root_com_quat_w(self) -> torch.Tensor: @@ -375,19 +441,7 @@ def root_com_quat_w(self) -> torch.Tensor: This quantity is the orientation of the actor frame of the root rigid body relative to the world. """ - return self.root_com_state_w[:, 3:7] - - @property - def root_com_vel_w(self) -> torch.Tensor: - """Root center of mass velocity 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_state_w.timestamp < self._sim_timestamp: - # read data from simulation - velocity = self._root_physx_view.get_velocities() - return velocity - return self.root_com_state_w[:, 7:13] + return self.root_com_pose_w[:, 3:7] @property def root_com_lin_vel_w(self) -> torch.Tensor: @@ -395,11 +449,7 @@ def root_com_lin_vel_w(self) -> torch.Tensor: This quantity is the linear velocity of the root rigid body's center of mass frame relative to the world. """ - if self._root_com_state_w.timestamp < self._sim_timestamp: - # read data from simulation - velocity = self._root_physx_view.get_velocities() - return velocity[:, 0:3] - return self.root_com_state_w[:, 7:10] + return self.root_com_vel_w[:, :3] @property def root_com_ang_vel_w(self) -> torch.Tensor: @@ -407,188 +457,201 @@ def root_com_ang_vel_w(self) -> torch.Tensor: This quantity is the angular velocity of the root rigid body's center of mass frame relative to the world. """ - if self._root_com_state_w.timestamp < self._sim_timestamp: - # read data from simulation - velocity = self._root_physx_view.get_velocities() - return velocity[:, 3:6] - return self.root_com_state_w[:, 10:13] + return self.root_com_vel_w[:, 3:6] @property - def root_com_lin_vel_b(self) -> torch.Tensor: - """Root center of mass linear velocity in base frame. Shape is (num_instances, 3). + 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 linear velocity of the root rigid body's center of mass frame with respect to the - rigid body's actor frame. + This quantity is the position of the rigid bodies' actor frame relative to the world. """ - return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_lin_vel_w) + return self.body_link_pose_w[..., :3] @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). + 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 angular velocity of the root rigid body's center of mass frame with respect to the - rigid body's actor frame. + This quantity is the orientation of the rigid bodies' actor frame relative to the world. """ - return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_ang_vel_w) + return self.body_link_pose_w[..., 3:7] @property - def body_pos_w(self) -> torch.Tensor: - """Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + 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 position of the rigid bodies' actor frame. + This quantity is the linear velocity of the rigid bodies' center of mass frame relative to the world. """ - return self.body_state_w[..., :3] + return self.body_link_vel_w[..., :3] @property - def body_quat_w(self) -> torch.Tensor: - """Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, 1, 4). + 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 orientation of the rigid bodies' actor frame. + This quantity is the angular velocity of the rigid bodies' center of mass frame relative to the world. """ - return self.body_state_w[..., 3:7] + return self.body_link_vel_w[..., 3:6] @property - def body_vel_w(self) -> torch.Tensor: - """Velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 6). + def body_com_pos_w(self) -> torch.Tensor: + """Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). - This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame. + This quantity is the position of the rigid bodies' actor frame. """ - return self.body_state_w[..., 7:13] + return self.body_com_pose_w[..., :3] @property - def body_lin_vel_w(self) -> torch.Tensor: + 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_state_w[..., 7:10] + return self.body_com_vel_w[..., :3] @property - def body_ang_vel_w(self) -> torch.Tensor: + 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_state_w[..., 10:13] + return self.body_com_vel_w[..., 3:6] @property - def body_lin_acc_w(self) -> torch.Tensor: + 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_acc_w[..., 0:3] + return self.body_com_acc_w[..., :3] @property - def body_ang_acc_w(self) -> torch.Tensor: + 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_acc_w[..., 3:6] - - # - # Link body properties - # + return self.body_com_acc_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). + 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 position of the rigid bodies' actor frame relative to the world. + This quantity is the center of mass location relative to its body'slink frame. """ - return self.body_link_state_w[..., :3] + return self.body_com_pose_b[..., :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). + 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 rigid bodies' actor frame relative to the world. + This quantity is the orientation of the principles axes of inertia relative to its body's link frame. """ - return self.body_link_state_w[..., 3:7] + return self.body_com_pose_b[..., 3:7] + + ## + # Properties for backwards compatibility. + ## @property - def body_link_vel_w(self) -> torch.Tensor: - """Velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 6). + def root_pose_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pose_w`.""" + return self.root_link_pose_w - This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame - relative to the world. - """ - return self.body_link_state_w[..., 7:13] + @property + def root_pos_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pos_w`.""" + return self.root_link_pos_w @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). + def root_quat_w(self) -> torch.Tensor: + """Same as :attr:`root_link_quat_w`.""" + return self.root_link_quat_w - This quantity is the linear velocity of the rigid bodies' center of mass frame relative to the world. - """ - return self.body_link_state_w[..., 7:10] + @property + def root_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_vel_w`.""" + return self.root_com_vel_w @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). + def root_lin_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_lin_vel_w`.""" + return self.root_com_lin_vel_w - This quantity is the angular velocity of the rigid bodies' center of mass frame relative to the world. - """ - return self.body_link_state_w[..., 10:13] + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_w`.""" + return self.root_com_ang_vel_w - # - # Center of mass body properties - # + @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 body_com_pos_w(self) -> torch.Tensor: - """Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + def root_ang_vel_b(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_b`.""" + return self.root_com_ang_vel_b - This quantity is the position of the rigid bodies' actor frame. - """ - return self.body_com_state_w[..., :3] + @property + def body_pose_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pose_w`.""" + return self.body_link_pose_w @property - def body_com_quat_w(self) -> torch.Tensor: - """Orientation (w, x, y, z) of the prinicple axies of inertia of all bodies in simulation world frame. + def body_pos_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pos_w`.""" + return self.body_link_pos_w - Shape is (num_instances, 1, 4). This quantity is the orientation of the rigid bodies' actor frame. - """ - return self.body_com_state_w[..., 3:7] + @property + def body_quat_w(self) -> torch.Tensor: + """Same as :attr:`body_link_quat_w`.""" + return self.body_link_quat_w @property - def body_com_vel_w(self) -> torch.Tensor: - """Velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 6). + def body_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_vel_w`.""" + return self.body_com_vel_w - This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame. - """ - return self.body_com_state_w[..., 7:13] + @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_com_lin_vel_w(self) -> torch.Tensor: - """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + def body_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_ang_vel_w`.""" + return self.body_com_ang_vel_w - This quantity is the linear velocity of the rigid bodies' center of mass frame. - """ - return self.body_com_state_w[..., 7:10] + @property + def body_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_acc_w`.""" + return self.body_com_acc_w @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). + def body_lin_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_lin_acc_w`.""" + return self.body_com_lin_acc_w - This quantity is the angular velocity of the rigid bodies' center of mass frame. - """ - return self.body_com_state_w[..., 10:13] + @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: - """Center of mass of all of the bodies in simulation world frame. Shape is (num_instances, 1, 3). - - This quantity is the center of mass location relative to its body frame. - """ - return self._root_physx_view.get_coms().to(self.device)[..., :3].view(-1, 1, 3) + """Same as :attr:`body_com_pos_b`.""" + return self.body_com_pos_b @property def com_quat_b(self) -> torch.Tensor: - """Orientation (w,x,y,z) of the prinicple axies of inertia of all of the bodies in simulation world frame. Shape is (num_instances, 1, 4). - - This quantity is the orientation of the principles axes of inertia relative to its body frame. - """ - quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7] - return math_utils.convert_quat(quat, to="wxyz").view(-1, 1, 4) + """Same as :attr:`body_com_quat_b`.""" + return self.body_com_quat_b diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/__init__.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/__init__.py index d6feb8238e19..edca995d62a7 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/__init__.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py index 94b74eeb8022..b458b15b4035 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py @@ -1,25 +1,26 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +import logging import re -import torch -import weakref from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.kit.app -import omni.log +import torch +import warp as wp + import omni.physics.tensors.impl.api as physx -import omni.timeline +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 isaaclab.utils.wrench_composer import WrenchComposer from ..asset_base import AssetBase from .rigid_object_collection_data import RigidObjectCollectionData @@ -27,6 +28,9 @@ if TYPE_CHECKING: from .rigid_object_collection_cfg import RigidObjectCollectionCfg +# import logger +logger = logging.getLogger(__name__) + class RigidObjectCollection(AssetBase): """A rigid object collection class. @@ -66,7 +70,7 @@ def __init__(self, cfg: RigidObjectCollectionCfg): 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 @@ -87,25 +91,12 @@ def __init__(self, cfg: RigidObjectCollectionCfg): 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, - ) - + # register various callback functions + self._register_callbacks() self._debug_vis_handle = None """ @@ -143,6 +134,27 @@ def root_physx_view(self) -> physx.RigidBodyView: """ return self._root_physx_view # type: ignore + @property + def instantaneous_wrench_composer(self) -> WrenchComposer: + """Instantaneous wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are only valid for the current simulation step. At the end of the simulation step, the wrenches set + to this object are discarded. This is useful to apply forces that change all the time, things like drag forces + for instance. + """ + return self._instantaneous_wrench_composer + + @property + def permanent_wrench_composer(self) -> WrenchComposer: + """Permanent wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are persistent and are applied to the simulation at every step. This is useful to apply forces that + are constant over a period of time, things like the thrust of a motor for instance. + """ + return self._permanent_wrench_composer + """ Operations. """ @@ -160,8 +172,8 @@ def reset(self, env_ids: torch.Tensor | None = None, object_ids: slice | torch.T 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 + self._instantaneous_wrench_composer.reset(env_ids) + self._permanent_wrench_composer.reset(env_ids) def write_data_to_sim(self): """Write external wrench to the simulation. @@ -171,14 +183,33 @@ def write_data_to_sim(self): 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, - ) + if self._instantaneous_wrench_composer.active or self._permanent_wrench_composer.active: + if self._instantaneous_wrench_composer.active: + # Compose instantaneous wrench with permanent wrench + self._instantaneous_wrench_composer.add_forces_and_torques( + forces=self._permanent_wrench_composer.composed_force, + torques=self._permanent_wrench_composer.composed_torque, + body_ids=self._ALL_OBJ_INDICES_WP, + env_ids=self._ALL_ENV_INDICES_WP, + ) + # Apply both instantaneous and permanent wrench to the simulation + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self.reshape_data_to_view(self._instantaneous_wrench_composer.composed_force_as_torch), + torque_data=self.reshape_data_to_view(self._instantaneous_wrench_composer.composed_torque_as_torch), + position_data=None, + indices=self._env_obj_ids_to_view_ids(self._ALL_ENV_INDICES, self._ALL_OBJ_INDICES), + is_global=False, + ) + else: + # Apply permanent wrench to the simulation + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self.reshape_data_to_view(self._permanent_wrench_composer.composed_force_as_torch), + torque_data=self.reshape_data_to_view(self._permanent_wrench_composer.composed_torque_as_torch), + position_data=None, + indices=self._env_obj_ids_to_view_ids(self._ALL_ENV_INDICES, self._ALL_OBJ_INDICES), + is_global=False, + ) + self._instantaneous_wrench_composer.reset() def update(self, dt: float): self._data.update(dt) @@ -225,10 +256,8 @@ def write_object_state_to_sim( env_ids: Environment indices. If None, then all indices are used. object_ids: Object indices. If None, then all indices are used. """ - - # set into simulation - self.write_object_pose_to_sim(object_state[..., :7], env_ids=env_ids, object_ids=object_ids) - self.write_object_velocity_to_sim(object_state[..., 7:], env_ids=env_ids, object_ids=object_ids) + 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, @@ -237,6 +266,7 @@ def write_object_com_state_to_sim( 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. @@ -245,7 +275,6 @@ def write_object_com_state_to_sim( env_ids: Environment indices. If None, then all indices are used. object_ids: Object indices. If None, then all indices are used. """ - # set into simulation 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) @@ -256,6 +285,7 @@ def write_object_link_state_to_sim( 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. @@ -264,7 +294,6 @@ def write_object_link_state_to_sim( env_ids: Environment indices. If None, then all indices are used. object_ids: Object indices. If None, then all indices are used. """ - # set into simulation 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) @@ -283,22 +312,7 @@ def write_object_pose_to_sim( 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_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() - # convert the quaternion from wxyz to xyzw - poses_xyzw = self._data.object_state_w[..., :7].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) + 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, @@ -315,7 +329,6 @@ def write_object_link_pose_to_sim( 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: @@ -323,13 +336,32 @@ def write_object_link_pose_to_sim( # -- 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_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() - self._data.object_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() + 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() + if self._data._object_com_state_w.data is not None: + # 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] + com_pos, com_quat = math_utils.combine_frame_transforms( + object_pose[..., :3], + object_pose[..., 3:7], + com_pos_b, + com_quat_b, + ) + self._data.object_com_state_w[env_ids[:, None], object_ids, :3] = com_pos + self._data.object_com_state_w[env_ids[:, None], object_ids, 3:7] = com_quat + # convert the quaternion from wxyz to xyzw - poses_xyzw = self._data.object_link_state_w[..., :7].clone() + 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) @@ -341,36 +373,43 @@ def write_object_com_pose_to_sim( 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: - local_env_ids = slice(env_ids) - else: - local_env_ids = env_ids + env_ids = self._ALL_ENV_INDICES + # -- object_ids if object_ids is None: - local_object_ids = slice(object_ids) - else: - local_object_ids = object_ids - - com_pos = self.data.com_pos_b[local_env_ids][:, local_object_ids, :] - com_quat = self.data.com_quat_b[local_env_ids][:, local_object_ids, :] + 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_rotate(math_utils.quat_inv(com_quat), -com_pos), - math_utils.quat_inv(com_quat), + 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_pose=object_link_pose, env_ids=env_ids, object_ids=object_ids) + 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, @@ -385,22 +424,7 @@ def write_object_velocity_to_sim( 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 - - self._data.object_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() - self._data.object_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_state_w[..., 7:]), indices=view_ids - ) + 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, @@ -423,15 +447,22 @@ def write_object_com_velocity_to_sim( if object_ids is None: object_ids = self._ALL_OBJ_INDICES - self._data.object_com_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() - self._data.object_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() - self._data.object_acc_w[env_ids[:, None], object_ids] = 0.0 + # 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() + 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() + # 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_state_w[..., 7:]), indices=view_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, @@ -440,34 +471,40 @@ def write_object_link_velocity_to_sim( 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: - local_env_ids = slice(env_ids) - else: - local_env_ids = env_ids + env_ids = self._ALL_ENV_INDICES + # -- object_ids if object_ids is None: - local_object_ids = slice(object_ids) - else: - local_object_ids = object_ids + 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() - quat = self.data.object_link_state_w[local_env_ids][:, local_object_ids, 3:7] - com_pos_b = self.data.com_pos_b[local_env_ids][:, local_object_ids, :] - # transform given velocity to center of mass object_com_velocity[..., :3] += torch.linalg.cross( - object_com_velocity[..., 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1 + 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_velocity=object_com_velocity, env_ids=env_ids, object_ids=object_ids - ) + self.write_object_com_velocity_to_sim(object_com_velocity, env_ids=env_ids, object_ids=object_ids) """ Operations - Setters. @@ -477,8 +514,10 @@ def set_external_force_and_torque( self, forces: torch.Tensor, torques: torch.Tensor, + positions: torch.Tensor | None = None, object_ids: slice | torch.Tensor | None = None, env_ids: torch.Tensor | None = None, + is_global: bool = False, ): """Set external force and torque to apply on the objects' bodies in their local frame. @@ -503,35 +542,86 @@ def set_external_force_and_torque( 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). + positions: External wrench positions 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). + is_global: Whether to apply the external wrench in the global frame. Defaults to False. If set to False, + the external wrench is applied in the link frame of the bodies. """ - 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 + logger.warning( + "The function 'set_external_force_and_torque' will be deprecated in a future release. Please" + " use 'permanent_wrench_composer.set_forces_and_torques' instead." + ) + + if forces is None and torques is None: + logger.warning("No forces or torques provided. No permanent external wrench will be applied.") # resolve all indices # -- env_ids if env_ids is None: - env_ids = self._ALL_ENV_INDICES + env_ids = self._ALL_ENV_INDICES_WP + elif not isinstance(env_ids, torch.Tensor): + env_ids = wp.array(env_ids, dtype=wp.int32, device=self.device) + else: + env_ids = wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) # -- 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 + object_ids = self._ALL_OBJ_INDICES_WP + elif isinstance(object_ids, slice): + object_ids = wp.from_torch( + torch.arange(self.num_objects, dtype=torch.int32, device=self.device)[object_ids], dtype=wp.int32 + ) + elif not isinstance(object_ids, torch.Tensor): + object_ids = wp.array(object_ids, dtype=wp.int32, device=self.device) + else: + object_ids = wp.from_torch(object_ids.to(torch.int32), dtype=wp.int32) + + # Write to wrench composer + self._permanent_wrench_composer.set_forces_and_torques( + forces=wp.from_torch(forces, dtype=wp.vec3f) if forces is not None else None, + torques=wp.from_torch(torques, dtype=wp.vec3f) if torques is not None else None, + positions=wp.from_torch(positions, dtype=wp.vec3f) if positions is not None else None, + body_ids=object_ids, + env_ids=env_ids, + is_global=is_global, + ) + + """ + 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): - # create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") + # clear object names list to prevent double counting on re-initialization + self._object_names_list.clear() + # 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) @@ -542,7 +632,9 @@ def _initialize_impl(self): # find rigid root prims root_prims = sim_utils.get_all_matching_child_prims( - template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.RigidBodyAPI) + template_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.RigidBodyAPI), + traverse_instance_prims=False, ) if len(root_prims) == 0: raise RuntimeError( @@ -558,7 +650,9 @@ def _initialize_impl(self): # 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) + template_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), + traverse_instance_prims=False, ) if len(articulation_prims) != 0: if articulation_prims[0].GetAttribute("physxArticulation:articulationEnabled").Get(): @@ -584,9 +678,9 @@ def _initialize_impl(self): 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}") + logger.info(f"Number of instances: {self.num_instances}") + logger.info(f"Number of distinct objects: {self.num_objects}") + logger.info(f"Object names: {self.object_names}") # container for data access self._data = RigidObjectCollectionData(self.root_physx_view, self.num_objects, self.device) @@ -603,11 +697,12 @@ def _create_buffers(self): # 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) + self._ALL_ENV_INDICES_WP = wp.from_torch(self._ALL_ENV_INDICES.to(torch.int32), dtype=wp.int32) + self._ALL_OBJ_INDICES_WP = wp.from_torch(self._ALL_OBJ_INDICES.to(torch.int32), dtype=wp.int32) - # 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) + # external wrench composer + self._instantaneous_wrench_composer = WrenchComposer(self) + self._permanent_wrench_composer = WrenchComposer(self) # set information about rigid body into data self._data.object_names = self.object_names @@ -636,28 +731,6 @@ def _process_cfg(self): default_object_states = torch.cat(default_object_states, dim=1) self._data.default_object_state = default_object_states - 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_size). - - Args: - data: The data coming from the :attr:`root_physx_view`. Shape is (num_instances*num_objects, data_size). - - Returns: - The reshaped data. Shape is (num_instances, num_objects, data_size). - """ - 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_size). - - Returns: - The reshaped data. Shape is (num_instances*num_objects, data_size). - """ - return torch.einsum("ijk -> jik", data).reshape(self.num_objects * self.num_instances, *data.shape[2:]) - def _env_obj_ids_to_view_ids( self, env_ids: torch.Tensor, object_ids: Sequence[int] | slice | torch.Tensor ) -> torch.Tensor: @@ -687,5 +760,24 @@ def _invalidate_initialize_callback(self, event): # call parent super()._invalidate_initialize_callback(event) # set all existing views to None to invalidate them - self._physics_sim_view = None 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/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_cfg.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_cfg.py index 3ef96c5bae89..c99b20044dd5 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_cfg.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause 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 450f5302975e..5156ef729e42 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 @@ -1,14 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch import weakref +import torch + import omni.physics.tensors.impl.api as physx import isaaclab.utils.math as math_utils +from isaaclab.sim.utils.stage import get_current_stage_id from isaaclab.utils.buffers import TimestampedBuffer @@ -54,7 +56,8 @@ def __init__(self, root_physx_view: physx.RigidBodyView, num_objects: int, devic self._sim_timestamp = 0.0 # Obtain global physics sim view - physics_sim_view = physx.create_simulation_view("torch") + stage_id = get_current_stage_id() + physics_sim_view = physx.create_simulation_view("torch", stage_id) physics_sim_view.set_subspace_roots("/") gravity = physics_sim_view.get_gravity() # Convert to direction vector @@ -68,10 +71,19 @@ def __init__(self, root_physx_view: physx.RigidBodyView, num_objects: int, devic ) # 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() - self._object_acc_w = TimestampedBuffer() def update(self, dt: float): """Updates the data for the rigid object collection. @@ -107,14 +119,84 @@ 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. """ ## - # Properties. + # 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_apply(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. @@ -123,15 +205,10 @@ def object_state_w(self): 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: - # 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") - velocity = self._reshape_view_to_data(self._root_physx_view.get_velocities()) - # set the buffer data and timestamp - self._object_state_w.data = torch.cat((pose, velocity), dim=-1) + 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 @@ -143,19 +220,9 @@ def object_link_state_w(self): world. """ if self._object_link_state_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") - velocity = self._reshape_view_to_data(self._root_physx_view.get_velocities()) - - # adjust linear velocity to link from center of mass - velocity[..., :3] += torch.linalg.cross( - velocity[..., 3:], math_utils.quat_rotate(pose[..., 3:7], -self.com_pos_b[..., :]), dim=-1 - ) - - # set the buffer data and timestamp - self._object_link_state_w.data = torch.cat((pose, velocity), dim=-1) + 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 @@ -164,41 +231,51 @@ def object_com_state_w(self): 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 is the orientation principle axes of inertia. + 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: - # 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") - velocity = self._reshape_view_to_data(self._root_physx_view.get_velocities()) - - # adjust pose to center of mass - pos, quat = math_utils.combine_frame_transforms( - pose[..., :3], pose[..., 3:7], self.com_pos_b[..., :], self.com_quat_b[..., :] - ) - - # set the buffer data and timestamp - self._object_com_state_w.data = torch.cat((pos, quat, velocity), dim=-1) + 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_acc_w(self): + 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_acc_w.timestamp < self._sim_timestamp: - # note: we use finite differencing to compute acceleration - self._object_acc_w.data = self._reshape_view_to_data(self._root_physx_view.get_accelerations().clone()) - self._object_acc_w.timestamp = self._sim_timestamp - return self._object_acc_w.data + 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_rotate_inverse(self.object_link_quat_w, self.GRAVITY_VEC_W) + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.GRAVITY_VEC_W) @property def heading_w(self): @@ -211,83 +288,45 @@ def heading_w(self): forward_w = math_utils.quat_apply(self.object_link_quat_w, self.FORWARD_VEC_B) return torch.atan2(forward_w[..., 1], forward_w[..., 0]) - ## - # Derived properties. - ## - - @property - def object_pos_w(self) -> torch.Tensor: - """Object 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_state_w[..., :3] - - @property - def object_quat_w(self) -> torch.Tensor: - """Object 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_state_w[..., 3:7] - @property - def object_vel_w(self) -> torch.Tensor: - """Object velocity in simulation world frame. Shape is (num_instances, num_objects, 6). - - This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame. - """ - return self.object_state_w[..., 7:13] - - @property - def object_lin_vel_w(self) -> torch.Tensor: - """Object 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_state_w[..., 7:10] - - @property - def object_ang_vel_w(self) -> torch.Tensor: - """Object angular velocity in simulation world frame. Shape is (num_instances, num_objects, 3). + 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 angular velocity of the rigid bodies' center of mass frame. + 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 self.object_state_w[..., 10:13] + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_link_lin_vel_w) @property - def object_lin_vel_b(self) -> torch.Tensor: - """Object linear velocity in base frame. Shape is (num_instances, num_objects, 3). + 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 linear velocity of the rigid bodies' center of mass frame with respect to the - rigid body's actor frame. + 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_rotate_inverse(self.object_quat_w, self.object_lin_vel_w) + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_link_ang_vel_w) @property - def object_ang_vel_b(self) -> torch.Tensor: - """Object angular velocity in base world frame. Shape is (num_instances, num_objects, 3). + 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 angular velocity of the rigid bodies' center of mass frame with respect to the - rigid body's actor frame. + 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_rotate_inverse(self.object_quat_w, self.object_ang_vel_w) + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_com_lin_vel_w) @property - def object_lin_acc_w(self) -> torch.Tensor: - """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_objects, 3). + 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 linear acceleration of the rigid bodies' center of mass frame. + 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 self.object_acc_w[..., 0:3] + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_com_ang_vel_w) - @property - def object_ang_acc_w(self) -> torch.Tensor: - """Angular acceleration of all bodies 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_acc_w[..., 3:6] + ## + # Sliced properties. + ## @property def object_link_pos_w(self) -> torch.Tensor: @@ -295,11 +334,7 @@ def object_link_pos_w(self) -> torch.Tensor: This quantity is the position of the actor frame of the rigid bodies. """ - if self._object_state_w.timestamp < self._sim_timestamp: - # read data from simulation - pose = self._reshape_view_to_data(self._root_physx_view.get_transforms().clone()) - return pose[..., :3] - return self.object_link_state_w[..., :3] + return self.object_link_pose_w[..., :3] @property def object_link_quat_w(self) -> torch.Tensor: @@ -307,20 +342,7 @@ def object_link_quat_w(self) -> torch.Tensor: This quantity is the orientation of the actor frame of the rigid bodies. """ - if self._object_state_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") - return pose[..., 3:7] - return self.object_link_state_w[..., 3:7] - - @property - def object_link_vel_w(self) -> torch.Tensor: - """Object link velocity in simulation world frame. Shape is (num_instances, num_objects, 6). - - This quantity contains the linear and angular velocities of the rigid bodies' actor frame. - """ - return self.object_link_state_w[..., 7:13] + return self.object_link_pose_w[..., 3:7] @property def object_link_lin_vel_w(self) -> torch.Tensor: @@ -328,7 +350,7 @@ def object_link_lin_vel_w(self) -> torch.Tensor: This quantity is the linear velocity of the rigid bodies' actor frame. """ - return self.object_link_state_w[..., 7:10] + return self.object_link_vel_w[..., :3] @property def object_link_ang_vel_w(self) -> torch.Tensor: @@ -336,25 +358,7 @@ def object_link_ang_vel_w(self) -> torch.Tensor: This quantity is the angular velocity of the rigid bodies' actor frame. """ - return self.object_link_state_w[..., 10:13] - - @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_rotate_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_rotate_inverse(self.object_link_quat_w, self.object_link_ang_vel_w) + return self.object_link_vel_w[..., 3:6] @property def object_com_pos_w(self) -> torch.Tensor: @@ -362,27 +366,16 @@ def object_com_pos_w(self) -> torch.Tensor: This quantity is the position of the center of mass frame of the rigid bodies. """ - return self.object_com_state_w[..., :3] + 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). + """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_state_w[..., 3:7] - - @property - def object_com_vel_w(self) -> torch.Tensor: - """Object center of mass velocity in simulation world frame. Shape is (num_instances, num_objects, 6). - - This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame. - """ - if self._object_state_w.timestamp < self._sim_timestamp: - # read data from simulation - velocity = self._reshape_view_to_data(self._root_physx_view.get_velocities()) - return velocity - return self.object_com_state_w[..., 7:13] + return self.object_com_pose_w[..., 3:7] @property def object_com_lin_vel_w(self) -> torch.Tensor: @@ -390,11 +383,7 @@ def object_com_lin_vel_w(self) -> torch.Tensor: This quantity is the linear velocity of the rigid bodies' center of mass frame. """ - if self._object_state_w.timestamp < self._sim_timestamp: - # read data from simulation - velocity = self._reshape_view_to_data(self._root_physx_view.get_velocities()) - return velocity[..., 0:3] - return self.object_com_state_w[..., 7:10] + return self.object_com_vel_w[..., :3] @property def object_com_ang_vel_w(self) -> torch.Tensor: @@ -402,48 +391,113 @@ def object_com_ang_vel_w(self) -> torch.Tensor: This quantity is the angular velocity of the rigid bodies' center of mass frame. """ - if self._object_state_w.timestamp < self._sim_timestamp: - # read data from simulation - velocity = self._reshape_view_to_data(self._root_physx_view.get_velocities()) - return velocity[..., 3:6] - return self.object_com_state_w[..., 10:13] + return self.object_com_vel_w[..., 3:6] @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). + 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 velocity of the center of mass frame of the root rigid body frame with respect to the - rigid body's actor frame. + This quantity is the linear acceleration of the rigid bodies' center of mass frame. """ - return math_utils.quat_rotate_inverse(self.object_link_quat_w, self.object_com_lin_vel_w) + return self.object_com_acc_w[..., :3] @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). + 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 velocity of the center of mass frame of the root rigid body frame with respect to the - rigid body's actor frame. + This quantity is the angular acceleration of the rigid bodies' center of mass frame. """ - return math_utils.quat_rotate_inverse(self.object_link_quat_w, self.object_com_ang_vel_w) + return self.object_com_acc_w[..., 3:6] @property - def com_pos_b(self) -> torch.Tensor: - """Center of mass of all of the bodies in simulation world frame. Shape is (num_instances, 1, 3). + 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 frame. + This quantity is the center of mass location relative to its body link frame. """ - pos = self._root_physx_view.get_coms().to(self.device)[..., :3] - return self._reshape_view_to_data(pos) + return self.object_com_pose_b[..., :3] @property - def com_quat_b(self) -> torch.Tensor: - """Orientation (w,x,y,z) of the prinicple axies of inertia of all of the bodies in simulation world frame. Shape is (num_instances, 1, 4). + 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 frame. + 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. """ - quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7].view(self.num_instances, self.num_objects, 4) - quat_wxyz = math_utils.convert_quat(quat, to="wxyz") - return self._reshape_view_to_data(quat_wxyz) + 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. diff --git a/source/isaaclab/isaaclab/assets/surface_gripper/__init__.py b/source/isaaclab/isaaclab/assets/surface_gripper/__init__.py new file mode 100644 index 000000000000..3786976617c3 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/surface_gripper/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for surface_gripper assets.""" + +from .surface_gripper import SurfaceGripper +from .surface_gripper_cfg import SurfaceGripperCfg diff --git a/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper.py b/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper.py new file mode 100644 index 000000000000..2742e9baeb4c --- /dev/null +++ b/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper.py @@ -0,0 +1,427 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +import warnings +from typing import TYPE_CHECKING + +import torch + +from isaacsim.core.utils.extensions import enable_extension + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBase +from isaaclab.utils.version import get_isaac_sim_version + +if TYPE_CHECKING: + from isaacsim.robot.surface_gripper import GripperView + + from .surface_gripper_cfg import SurfaceGripperCfg + +# import logger +logger = logging.getLogger(__name__) + + +class SurfaceGripper(AssetBase): + """A surface gripper actuator class. + + Surface grippers are actuators capable of grasping objects when in close proximity with them. + + Each surface gripper in the collection must be a `Isaac Sim SurfaceGripper` primitive. + On playing the simulation, the physics engine will automatically register the surface grippers into a + SurfaceGripperView object. This object can be accessed using the :attr:`gripper_view` attribute. + + To interact with the surface grippers, the user can use the :attr:`state` to get the current state of the grippers, + :attr:`command` to get the current command sent to the grippers, and :func:`update_gripper_properties` to update the + properties of the grippers at runtime. Finally, the :func:`set_grippers_command` function should be used to set the + desired command for the grippers. + + Note: + The :func:`set_grippers_command` function does not write to the simulation. The simulation automatically + calls :func:`write_data_to_sim` function to write the command to the simulation. Similarly, the update + function is called automatically for every simulation step, and does not need to be called by the user. + + Note: + The SurfaceGripper is only supported on CPU for now. Please set the simulation backend to run on CPU. + Use `--device cpu` to run the simulation on CPU. + """ + + def __init__(self, cfg: SurfaceGripperCfg): + """Initialize the surface gripper. + + Args: + cfg: A configuration instance. + """ + # copy the configuration + self._cfg = cfg.copy() + + # checks for Isaac Sim v5.0 to ensure that the surface gripper is supported + if get_isaac_sim_version().major < 5: + raise NotImplementedError( + "SurfaceGrippers are only supported by IsaacSim 5.0 and newer. Current version is" + f" '{get_isaac_sim_version()}'. Please update to IsaacSim 5.0 or newer to use this feature." + ) + + # flag for whether the sensor is initialized + self._is_initialized = False + self._debug_vis_handle = None + + # register various callback functions + self._register_callbacks() + + """ + Properties + """ + + @property + def data(self): + raise NotImplementedError("SurfaceGripper does have a data interface.") + + @property + def num_instances(self) -> int: + """Number of instances of the gripper. + + This is equal to the total number of grippers (the view can only contain one gripper per environment). + """ + return self._num_envs + + @property + def state(self) -> torch.Tensor: + """Returns the gripper state buffer. + + The gripper state is a list of integers: + - -1 --> Open + - 0 --> Closing + - 1 --> Closed + """ + return self._gripper_state + + @property + def command(self) -> torch.Tensor: + """Returns the gripper command buffer. + + The gripper command is a list of floats: + - [-1, -0.3] --> Open + - [-0.3, 0.3] --> Do nothing + - [0.3, 1] --> Close + """ + return self._gripper_command + + @property + def gripper_view(self) -> GripperView: + """Returns the gripper view object.""" + return self._gripper_view + + """ + Operations + """ + + def update_gripper_properties( + self, + max_grip_distance: torch.Tensor | None = None, + coaxial_force_limit: torch.Tensor | None = None, + shear_force_limit: torch.Tensor | None = None, + retry_interval: torch.Tensor | None = None, + indices: torch.Tensor | None = None, + ) -> None: + """Update the gripper properties. + + Args: + max_grip_distance: The maximum grip distance of the gripper. Should be a tensor of shape (num_envs,). + coaxial_force_limit: The coaxial force limit of the gripper. Should be a tensor of shape (num_envs,). + shear_force_limit: The shear force limit of the gripper. Should be a tensor of shape (num_envs,). + retry_interval: The retry interval of the gripper. Should be a tensor of shape (num_envs,). + indices: The indices of the grippers to update the properties for. Can be a tensor of any shape. + """ + + if indices is None: + indices = self._ALL_INDICES + + indices_as_list = indices.tolist() + + if max_grip_distance is not None: + self._max_grip_distance[indices] = max_grip_distance + if coaxial_force_limit is not None: + self._coaxial_force_limit[indices] = coaxial_force_limit + if shear_force_limit is not None: + self._shear_force_limit[indices] = shear_force_limit + if retry_interval is not None: + self._retry_interval[indices] = retry_interval + + self._gripper_view.set_surface_gripper_properties( + max_grip_distance=self._max_grip_distance.tolist(), + coaxial_force_limit=self._coaxial_force_limit.tolist(), + shear_force_limit=self._shear_force_limit.tolist(), + retry_interval=self._retry_interval.tolist(), + indices=indices_as_list, + ) + + def update(self, dt: float) -> None: + """Update the gripper state using the SurfaceGripperView. + + This function is called every simulation step. + The data fetched from the gripper view is a list of strings containing 3 possible states: + - "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 + - "Closing" --> 0.0 + - "Closed" --> 1.0 + + Note: + 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[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. + + The gripper command is a list of integers that needs to be converted to a list of strings: + - [-1, -0.3] --> Open + - ]-0.3, 0.3[ --> Do nothing + - [0.3, 1] --> Closed + + The Do nothing command is not applied, and is only used to indicate whether the gripper state has changed. + """ + # Remove the SurfaceGripper indices that have a commanded value of 2 + indices = ( + torch.argwhere(torch.logical_or(self._gripper_command < -0.3, self._gripper_command > 0.3)) + .to(torch.int32) + .tolist() + ) + # Write to the SurfaceGripperView if there are any indices to write to + if len(indices) > 0: + self._gripper_view.apply_gripper_action(self._gripper_command.tolist(), indices) + + def set_grippers_command(self, states: torch.Tensor, indices: torch.Tensor | None = None) -> None: + """Set the internal gripper command buffer. This function does not write to the simulation. + + Possible values for the gripper command are: + - [-1, -0.3] --> Open + - ]-0.3, 0.3[ --> Do nothing + - [0.3, 1] --> Close + + Args: + states: A tensor of integers representing the gripper command. Shape must match that of indices. + indices: A tensor of integers representing the indices of the grippers to set the command for. Defaults + to None, in which case all grippers are set. + """ + if indices is None: + indices = self._ALL_INDICES + + self._gripper_command[indices] = states + + def reset(self, indices: torch.Tensor | None = None) -> None: + """Reset the gripper command buffer. + + Args: + indices: A tensor of integers representing the indices of the grippers to reset the command for. Defaults + to None, in which case all grippers are reset. + """ + # Would normally set the buffer to 0, for now we won't do that + if indices is None: + indices = self._ALL_INDICES + + # Reset the selected grippers to an open status + self._gripper_command[indices] = -1.0 + self.write_data_to_sim() + # Sets the gripper last command to be 0.0 (do nothing) + self._gripper_command[indices] = 0 + # Force set the state to open. It will read open in the next update call. + self._gripper_state[indices] = -1.0 + + """ + Initialization. + """ + + def _initialize_impl(self) -> None: + """Initializes the gripper-related handles and internal buffers. + + Raises: + ValueError: If the simulation backend is not CPU. + RuntimeError: If the Simulation Context is not initialized or if gripper prims are not found. + + Note: + The SurfaceGripper is only supported on CPU for now. Please set the simulation backend to run on CPU. + Use `--device cpu` to run the simulation on CPU. + """ + + enable_extension("isaacsim.robot.surface_gripper") + from isaacsim.robot.surface_gripper import GripperView + + # Check that we are using the CPU backend. + if self._device != "cpu": + raise Exception( + "SurfaceGripper is only supported on CPU for now. Please set the simulation backend to run on CPU. Use" + " `--device cpu` to run the simulation on CPU." + ) + + # 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 surface gripper prims + gripper_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, + predicate=lambda prim: prim.GetTypeName() == "IsaacSurfaceGripper", + traverse_instance_prims=False, + ) + if len(gripper_prims) == 0: + raise RuntimeError( + f"Failed to find a surface gripper when resolving '{self._cfg.prim_path}'." + " Please ensure that the prim has type 'IsaacSurfaceGripper'." + ) + if len(gripper_prims) > 1: + raise RuntimeError( + f"Failed to find a single surface gripper when resolving '{self._cfg.prim_path}'." + f" Found multiple '{gripper_prims}' under '{template_prim_path}'." + " Please ensure that there is only one surface gripper in the prim path tree." + ) + + # resolve gripper prim back into regex expression + gripper_prim_path = gripper_prims[0].GetPath().pathString + gripper_prim_path_expr = self._cfg.prim_path + gripper_prim_path[len(template_prim_path) :] + + # Count number of environments + self._prim_expr = gripper_prim_path_expr + env_prim_path_expr = self._prim_expr.rsplit("/", 1)[0] + self._parent_prims = sim_utils.find_matching_prims(env_prim_path_expr) + self._num_envs = len(self._parent_prims) + + # Create buffers + self._create_buffers() + + # Process the configuration + self._process_cfg() + + # Initialize gripper view and set properties. Note we do not set the properties through the gripper view + # to avoid having to convert them to list of floats here. Instead, we do it in the update_gripper_properties + # function which does this conversion internally. + self._gripper_view = GripperView( + self._prim_expr, + ) + self.update_gripper_properties( + max_grip_distance=self._max_grip_distance.clone(), + coaxial_force_limit=self._coaxial_force_limit.clone(), + shear_force_limit=self._shear_force_limit.clone(), + retry_interval=self._retry_interval.clone(), + ) + + # log information about the surface gripper + logger.info(f"Surface gripper initialized at: {self._cfg.prim_path} with root '{gripper_prim_path_expr}'.") + logger.info(f"Number of instances: {self._num_envs}") + + # Reset grippers + self.reset() + + def _create_buffers(self) -> None: + """Create the buffers for storing the gripper state, command, and properties.""" + self._gripper_state = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32) + self._gripper_command = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32) + self._ALL_INDICES = torch.arange(self._num_envs, device=self._device, dtype=torch.long) + + self._max_grip_distance = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32) + self._coaxial_force_limit = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32) + self._shear_force_limit = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32) + self._retry_interval = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32) + + def _process_cfg(self) -> None: + """Process the configuration for the gripper properties.""" + # Get one of the grippers as defined in the default stage + gripper_prim = self._parent_prims[0] + try: + max_grip_distance = gripper_prim.GetAttribute("isaac:maxGripDistance").Get() + except Exception as e: + warnings.warn( + f"Failed to retrieve max_grip_distance from stage, defaulting to user provided cfg. Exception: {e}" + ) + max_grip_distance = None + + try: + coaxial_force_limit = gripper_prim.GetAttribute("isaac:coaxialForceLimit").Get() + except Exception as e: + warnings.warn( + f"Failed to retrieve coaxial_force_limit from stage, defaulting to user provided cfg. Exception: {e}" + ) + coaxial_force_limit = None + + try: + shear_force_limit = gripper_prim.GetAttribute("isaac:shearForceLimit").Get() + except Exception as e: + warnings.warn( + f"Failed to retrieve shear_force_limit from stage, defaulting to user provided cfg. Exception: {e}" + ) + shear_force_limit = None + + try: + retry_interval = gripper_prim.GetAttribute("isaac:retryInterval").Get() + except Exception as e: + warnings.warn( + f"Failed to retrieve retry_interval from stage defaulting to user provided cfg. Exception: {e}" + ) + retry_interval = None + + self._max_grip_distance = self.parse_gripper_parameter(self._cfg.max_grip_distance, max_grip_distance) + self._coaxial_force_limit = self.parse_gripper_parameter(self._cfg.coaxial_force_limit, coaxial_force_limit) + self._shear_force_limit = self.parse_gripper_parameter(self._cfg.shear_force_limit, shear_force_limit) + self._retry_interval = self.parse_gripper_parameter(self._cfg.retry_interval, retry_interval) + + """ + Helper functions. + """ + + def parse_gripper_parameter( + self, cfg_value: float | int | tuple | None, default_value: float | int | tuple | None, ndim: int = 0 + ) -> torch.Tensor: + """Parse the gripper parameter. + + Args: + cfg_value: The value to parse. Can be a float, int, tuple, or None. + default_value: The default value to use if cfg_value is None. Can be a float, int, tuple, or None. + ndim: The number of dimensions of the parameter. Defaults to 0. + """ + # Adjust the buffer size based on the number of dimensions + if ndim == 0: + param = torch.zeros(self._num_envs, device=self._device) + elif ndim == 3: + param = torch.zeros(self._num_envs, 3, device=self._device) + elif ndim == 4: + param = torch.zeros(self._num_envs, 4, device=self._device) + else: + raise ValueError(f"Invalid number of dimensions: {ndim}") + + # Parse the parameter + if cfg_value is not None: + if isinstance(cfg_value, (float, int)): + param[:] = float(cfg_value) + elif isinstance(cfg_value, tuple): + if len(cfg_value) == ndim: + param[:] = torch.tensor(cfg_value, dtype=torch.float, device=self._device) + else: + raise ValueError(f"Invalid number of values for parameter. Got: {len(cfg_value)}\nExpected: {ndim}") + else: + raise TypeError(f"Invalid type for parameter value: {type(cfg_value)}. " + "Expected float or int.") + elif default_value is not None: + if isinstance(default_value, (float, int)): + param[:] = float(default_value) + elif isinstance(default_value, tuple): + assert len(default_value) == ndim, f"Expected {ndim} values, got {len(default_value)}" + param[:] = torch.tensor(default_value, dtype=torch.float, device=self._device) + else: + raise TypeError( + f"Invalid type for default value: {type(default_value)}. " + "Expected float or Tensor." + ) + else: + raise ValueError("The parameter value is None and no default value is provided.") + + return param diff --git a/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper_cfg.py b/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper_cfg.py new file mode 100644 index 000000000000..6648e183e2fb --- /dev/null +++ b/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper_cfg.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from ..asset_base_cfg import AssetBaseCfg +from .surface_gripper import SurfaceGripper + + +@configclass +class SurfaceGripperCfg(AssetBaseCfg): + """Configuration parameters for a surface gripper actuator.""" + + prim_path: str = MISSING + """The expression to find the grippers in the stage.""" + + max_grip_distance: float | None = None + """The maximum grip distance of the gripper.""" + + coaxial_force_limit: float | None = None + """The coaxial force limit of the gripper.""" + + shear_force_limit: float | None = None + """The shear force limit of the gripper.""" + + retry_interval: float | None = None + """The amount of time the gripper will spend trying to grasp an object.""" + + class_type: type = SurfaceGripper diff --git a/source/isaaclab/isaaclab/assets/utils/__init__.py b/source/isaaclab/isaaclab/assets/utils/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/utils/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/controllers/__init__.py b/source/isaaclab/isaaclab/controllers/__init__.py index 65b5926eb410..3a055c508e8d 100644 --- a/source/isaaclab/isaaclab/controllers/__init__.py +++ b/source/isaaclab/isaaclab/controllers/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/controllers/config/__init__.py b/source/isaaclab/isaaclab/controllers/config/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab/isaaclab/controllers/config/__init__.py +++ b/source/isaaclab/isaaclab/controllers/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/controllers/config/rmp_flow.py b/source/isaaclab/isaaclab/controllers/config/rmp_flow.py index fc7fc483aa10..55c6d8e1fbaa 100644 --- a/source/isaaclab/isaaclab/controllers/config/rmp_flow.py +++ b/source/isaaclab/isaaclab/controllers/config/rmp_flow.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,11 +8,20 @@ from isaacsim.core.utils.extensions import get_extension_path_from_name from isaaclab.controllers.rmp_flow import RmpFlowControllerCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +# Directory on Nucleus Server for RMP-Flow assets (URDFs, collision models, etc.) +ISAACLAB_NUCLEUS_RMPFLOW_DIR = os.path.join(ISAACLAB_NUCLEUS_DIR, "Controllers", "RmpFlowAssets") # 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" -) +# We need to move it here for doc building purposes. +try: + _RMP_CONFIG_DIR = os.path.join( + get_extension_path_from_name("isaacsim.robot_motion.motion_generation"), + "motion_policy_configs", + ) +except Exception: + _RMP_CONFIG_DIR = "" # Path to current directory _CUR_DIR = os.path.dirname(os.path.realpath(__file__)) @@ -35,3 +44,57 @@ evaluations_per_frame=5, ) """Configuration of RMPFlow for UR10 arm (default from `isaacsim.robot_motion.motion_generation`).""" + +GALBOT_LEFT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join( + ISAACLAB_NUCLEUS_RMPFLOW_DIR, + "galbot_one_charlie", + "rmpflow", + "galbot_one_charlie_left_arm_rmpflow_config.yaml", + ), + urdf_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "galbot_one_charlie", "galbot_one_charlie.urdf"), + collision_file=os.path.join( + ISAACLAB_NUCLEUS_RMPFLOW_DIR, "galbot_one_charlie", "rmpflow", "galbot_one_charlie_left_arm_gripper.yaml" + ), + frame_name="left_gripper_tcp_link", + evaluations_per_frame=5, + ignore_robot_state_updates=True, +) + +GALBOT_RIGHT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join( + ISAACLAB_NUCLEUS_RMPFLOW_DIR, + "galbot_one_charlie", + "rmpflow", + "galbot_one_charlie_right_arm_rmpflow_config.yaml", + ), + urdf_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "galbot_one_charlie", "galbot_one_charlie.urdf"), + collision_file=os.path.join( + ISAACLAB_NUCLEUS_RMPFLOW_DIR, "galbot_one_charlie", "rmpflow", "galbot_one_charlie_right_arm_suction.yaml" + ), + frame_name="right_suction_cup_tcp_link", + evaluations_per_frame=5, + ignore_robot_state_updates=True, +) + +"""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/differential_ik.py b/source/isaaclab/isaaclab/controllers/differential_ik.py index 66de445c8963..8841dbe4fb5e 100644 --- a/source/isaaclab/isaaclab/controllers/differential_ik.py +++ b/source/isaaclab/isaaclab/controllers/differential_ik.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.utils.math import apply_delta_pose, compute_pose_error if TYPE_CHECKING: @@ -209,7 +210,7 @@ def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Ten # 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)) + S_inv = torch.where(min_singular_value < S, S_inv, torch.zeros_like(S_inv)) jacobian_pinv = ( torch.transpose(Vh, dim0=1, dim1=2)[:, :, :6] @ torch.diag_embed(S_inv) diff --git a/source/isaaclab/isaaclab/controllers/differential_ik_cfg.py b/source/isaaclab/isaaclab/controllers/differential_ik_cfg.py index ea27f1ff4fef..315a762752ca 100644 --- a/source/isaaclab/isaaclab/controllers/differential_ik_cfg.py +++ b/source/isaaclab/isaaclab/controllers/differential_ik_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/controllers/joint_impedance.py b/source/isaaclab/isaaclab/controllers/joint_impedance.py index d0af11cc4ae1..bd35089b81af 100644 --- a/source/isaaclab/isaaclab/controllers/joint_impedance.py +++ b/source/isaaclab/isaaclab/controllers/joint_impedance.py @@ -1,12 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch from collections.abc import Sequence from dataclasses import MISSING +import torch + from isaaclab.utils import configclass diff --git a/source/isaaclab/isaaclab/controllers/operational_space.py b/source/isaaclab/isaaclab/controllers/operational_space.py index bcdf939ad69c..2505768e0581 100644 --- a/source/isaaclab/isaaclab/controllers/operational_space.py +++ b/source/isaaclab/isaaclab/controllers/operational_space.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.utils.math import ( apply_delta_pose, combine_frame_transforms, @@ -509,7 +510,6 @@ def compute( # 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.") diff --git a/source/isaaclab/isaaclab/controllers/operational_space_cfg.py b/source/isaaclab/isaaclab/controllers/operational_space_cfg.py index 99d61d983f50..d2fc3575bd73 100644 --- a/source/isaaclab/isaaclab/controllers/operational_space_cfg.py +++ b/source/isaaclab/isaaclab/controllers/operational_space_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py b/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py new file mode 100644 index 000000000000..17ed7a67b077 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pink IK controller package for IsaacLab. + +This package provides integration between Pink inverse kinematics solver and IsaacLab. +""" + +from .null_space_posture_task import NullSpacePostureTask +from .pink_ik import PinkIKController +from .pink_ik_cfg import PinkIKControllerCfg 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 000000000000..ff8c6b9b03d8 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/local_frame_task.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Sequence + +import numpy as np +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/null_space_posture_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py new file mode 100644 index 000000000000..8ab6ddcc2dc0 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py @@ -0,0 +1,275 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import pinocchio as pin +import scipy.linalg.blas as blas +import scipy.linalg.lapack as lapack +from pink.configuration import Configuration +from pink.tasks import Task + + +class NullSpacePostureTask(Task): + r"""Pink-based task that adds a posture objective that is in the null space projection of other tasks. + + This task implements posture control in the null space of higher priority tasks + (typically end-effector pose tasks) within the Pink inverse kinematics framework. + + **Mathematical Formulation:** + + For details on Pink Inverse Kinematics optimization formulation visit: https://github.com/stephane-caron/pink + + **Null Space Posture Task Implementation:** + + This task consists of two components: + + 1. **Error Function**: The posture error is computed as: + + .. math:: + + \mathbf{e}(\mathbf{q}) = \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) + + where: + - :math:`\mathbf{q}^*` is the target joint configuration + - :math:`\mathbf{q}` is the current joint configuration + - :math:`\mathbf{M}` is a joint selection mask matrix + + 2. **Jacobian Matrix**: The task Jacobian is the null space projector: + + .. math:: + + \mathbf{J}_{\text{posture}}(\mathbf{q}) = \mathbf{N}(\mathbf{q}) = + \mathbf{I} -\mathbf{J}_{\text{primary}}^+ \mathbf{J}_{\text{primary}} + + where: + - :math:`\mathbf{J}_{\text{primary}}` is the combined Jacobian of all higher priority tasks + - :math:`\mathbf{J}_{\text{primary}}^+` is the pseudoinverse of the primary task Jacobian + - :math:`\mathbf{N}(\mathbf{q})` is the null space projector matrix + + For example, if there are two frame tasks (e.g., controlling the pose of two end-effectors), the combined Jacobian + :math:`\mathbf{J}_{\text{primary}}` is constructed by stacking the individual Jacobians for each frame vertically: + + .. math:: + + \mathbf{J}_{\text{primary}} = + \begin{bmatrix} + \mathbf{J}_1(\mathbf{q}) \\ + \mathbf{J}_2(\mathbf{q}) + \end{bmatrix} + + where :math:`\mathbf{J}_1(\mathbf{q})` and :math:`\mathbf{J}_2(\mathbf{q})` are the Jacobians for the + first and second frame tasks, respectively. + + The null space projector ensures that joint velocities in the null space produce zero velocity + for the primary tasks: :math:`\mathbf{J}_{\text{primary}} \cdot \dot{\mathbf{q}}_{\text{null}} = \mathbf{0}`. + + **Task Integration:** + + When integrated into the Pink framework, this task contributes to the optimization as: + + .. math:: + + \left\| + \mathbf{N}(\mathbf{q}) \mathbf{v} + \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) + \right\|_{W_{\text{posture}}}^2 + + This formulation allows the robot to maintain a desired posture while respecting the constraints + imposed by higher priority tasks (e.g., end-effector positioning). + + """ + + # Regularization factor for pseudoinverse computation to ensure numerical stability + PSEUDOINVERSE_DAMPING_FACTOR: float = 1e-9 + + def __init__( + self, + cost: float, + lm_damping: float = 0.0, + gain: float = 1.0, + controlled_frames: list[str] | None = None, + controlled_joints: list[str] | None = None, + ) -> None: + r"""Initialize the null space posture task. + + This task maintains a desired joint posture in the null space of higher-priority + frame tasks. Joint selection allows excluding specific joints (e.g., wrist joints + in humanoid manipulation) to prevent large rotational ranges from overwhelming + errors in critical joints like shoulders and waist. + + Args: + cost: Task weighting factor in the optimization objective. + Units: :math:`[\text{cost}] / [\text{rad}]`. + lm_damping: Levenberg-Marquardt regularization scale (unitless). Defaults to 0.0. + gain: Task gain :math:`\alpha \in [0, 1]` for low-pass filtering. + Defaults to 1.0 (no filtering). + controlled_frames: Frame names whose Jacobians define the primary tasks for + null space projection. If None or empty, no projection is applied. + controlled_joints: Joint names to control in the posture task. If None or + empty, all actuated joints are controlled. + """ + super().__init__(cost=cost, gain=gain, lm_damping=lm_damping) + self.target_q: np.ndarray | None = None + self.controlled_frames: list[str] = controlled_frames or [] + self.controlled_joints: list[str] = controlled_joints or [] + self._joint_mask: np.ndarray | None = None + self._frame_names: list[str] | None = None + + def __repr__(self) -> str: + """Human-readable representation of the task.""" + return ( + f"NullSpacePostureTask(cost={self.cost}, gain={self.gain}, lm_damping={self.lm_damping}," + f" controlled_frames={self.controlled_frames}, controlled_joints={self.controlled_joints})" + ) + + def _build_joint_mapping(self, configuration: Configuration) -> None: + """Build joint mask and cache frequently used values. + + Creates a binary mask that selects which joints should be controlled + in the posture task. + + Args: + configuration: Robot configuration containing the model and joint information. + """ + # Create joint mask for full configuration size + self._joint_mask = np.zeros(configuration.model.nq) + + # Create dictionary for joint names to indices (exclude root joint) + joint_names = configuration.model.names.tolist()[1:] + + # Build joint mask efficiently + for i, joint_name in enumerate(joint_names): + if joint_name in self.controlled_joints: + self._joint_mask[i] = 1.0 + + # Cache frame names for performance + self._frame_names = list(self.controlled_frames) + + def set_target(self, target_q: np.ndarray) -> None: + """Set target posture configuration. + + Args: + target_q: Target vector in the configuration space. If the model + has a floating base, then this vector should include + floating-base coordinates (although they have no effect on the + posture task since only actuated joints are controlled). + """ + self.target_q = target_q.copy() + + def set_target_from_configuration(self, configuration: Configuration) -> None: + """Set target posture from a robot configuration. + + Args: + configuration: Robot configuration whose joint angles will be used + as the target posture. + """ + self.set_target(configuration.q) + + def compute_error(self, configuration: Configuration) -> np.ndarray: + r"""Compute posture task error. + + The error computation follows: + + .. math:: + + \mathbf{e}(\mathbf{q}) = \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) + + where :math:`\mathbf{M}` is the joint selection mask and :math:`\mathbf{q}^* - \mathbf{q}` + is computed using Pinocchio's difference function to handle joint angle wrapping. + + Args: + configuration: Robot configuration :math:`\mathbf{q}`. + + Returns: + Posture task error :math:`\mathbf{e}(\mathbf{q})` with the same dimension + as the configuration vector, but with zeros for non-controlled joints. + + Raises: + ValueError: If no posture target has been set. + """ + if self.target_q is None: + raise ValueError("No posture target has been set. Call set_target() first.") + + # Initialize joint mapping if needed + if self._joint_mask is None: + self._build_joint_mapping(configuration) + + # Compute configuration difference using Pinocchio's difference function + # This handles joint angle wrapping correctly + err = pin.difference( + configuration.model, + self.target_q, + configuration.q, + ) + + # Apply pre-computed joint mask to select only controlled joints + return self._joint_mask * err + + def compute_jacobian(self, configuration: Configuration) -> np.ndarray: + r"""Compute the null space projector Jacobian. + + The null space projector is defined as: + + .. math:: + + \mathbf{N}(\mathbf{q}) = \mathbf{I} - \mathbf{J}_{\text{primary}}^+ \mathbf{J}_{\text{primary}} + + where: + - :math:`\mathbf{J}_{\text{primary}}` is the combined Jacobian of all controlled frames + - :math:`\mathbf{J}_{\text{primary}}^+` is the pseudoinverse of the primary task Jacobian + - :math:`\mathbf{I}` is the identity matrix + + The null space projector ensures that joint velocities in the null space produce + zero velocity for the primary tasks: + :math:`\mathbf{J}_{\text{primary}} \cdot \dot{\mathbf{q}}_{\text{null}} = \mathbf{0}`. + + If no controlled frames are specified, returns the identity matrix. + + Args: + configuration: Robot configuration :math:`\mathbf{q}`. + + Returns: + Null space projector matrix :math:`\mathbf{N}(\mathbf{q})` with dimensions + :math:`n_q \times n_q` where :math:`n_q` is the number of configuration variables. + """ + # Initialize joint mapping if needed + if self._frame_names is None: + self._build_joint_mapping(configuration) + + # If no frame tasks are defined, return identity matrix (no null space projection) + if not self._frame_names: + return np.eye(configuration.model.nq) + + # Get Jacobians for all frame tasks and combine them + J_frame_tasks = [configuration.get_frame_jacobian(frame_name) for frame_name in self._frame_names] + J_combined = np.concatenate(J_frame_tasks, axis=0) + + # Compute null space projector: N = I - J^+ * J + # Use fast pseudoinverse computation with direct LAPACK/BLAS calls + m, n = J_combined.shape + + # Wide matrix (typical for robotics): use left pseudoinverse + # J^+ = J^T @ inv(J @ J^T + ฮปยฒI) + # This is faster because we invert an mร—m matrix instead of nร—n + + # Compute J @ J^T using BLAS (faster than numpy) + JJT = blas.dgemm(1.0, J_combined, J_combined.T) + np.fill_diagonal(JJT, JJT.diagonal() + self.PSEUDOINVERSE_DAMPING_FACTOR**2) + + # Use LAPACK's Cholesky factorization (dpotrf = Positive definite TRiangular Factorization) + L, info = lapack.dpotrf(JJT, lower=1, clean=False, overwrite_a=True) + + if info != 0: + # Fallback if not positive definite: use numpy's pseudoinverse + J_pinv = np.linalg.pinv(J_combined) + return np.eye(n) - J_pinv @ J_combined + + # Solve (J @ J^T + ฮปยฒI) @ X = J using LAPACK's triangular solver (dpotrs) + # This directly solves the system without computing the full inverse + X, _ = lapack.dpotrs(L, J_combined, lower=1) + + # Compute null space projector: N = I - J^T @ X + N_combined = np.eye(n) - J_combined.T @ X + + return N_combined diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py new file mode 100644 index 000000000000..788a5da67053 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -0,0 +1,255 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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. + +Reference: + Pink IK Solver: https://github.com/stephane-caron/pink +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +import torch +from pink import solve_ik + +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 + + +class PinkIKController: + """Integration of Pink IK controller with Isaac Lab. + + The Pink IK controller solves differential inverse kinematics through weighted tasks. Each task is defined + by a residual function e(q) that is driven to zero (e.g., e(q) = p_target - p_ee(q) for end-effector positioning). + The controller computes joint velocities v satisfying J_e(q)v = -ฮฑe(q), where J_e(q) is the task Jacobian. + Multiple tasks are resolved through weighted optimization, formulating a quadratic program that minimizes + weighted task errors while respecting joint velocity limits. + + It supports user defined tasks, and we have provided a NullSpacePostureTask for maintaining desired + joint configurations. + + Reference: + Pink IK Solver: https://github.com/stephane-caron/pink + """ + + def __init__( + self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str, controlled_joint_indices: list[int] + ): + """Initialize the Pink IK Controller. + + Args: + cfg: The configuration for the Pink IK controller containing task definitions, solver parameters, + and joint configurations. + 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: + ValueError: When joint_names or all_joint_names are not provided in the configuration. + """ + 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, + # where the joint_pos keys may be regex patterns and the values are the initial positions. + # We want to assign to each Pink joint name the value from the first matching regex key in joint_pos. + pink_joint_names = self.pink_configuration.model.names.tolist()[1:] + joint_pos_dict = robot_cfg.init_state.joint_pos + + # Use resolve_matching_names_values to match Pink joint names to joint_pos values + indices, _, values = resolve_matching_names_values( + joint_pos_dict, pink_joint_names, preserve_order=False, strict=False + ) + 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: + # If task is a NullSpacePostureTask, set the target to the initial joint positions + if isinstance(task, NullSpacePostureTask): + task.set_target(self.init_joint_positions) + continue + task.set_target_from_configuration(self.pink_configuration) + for task in cfg.fixed_input_tasks: + task.set_target_from_configuration(self.pink_configuration) + + # Create joint ordering mappings + self._setup_joint_ordering_mappings() + + 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) + ) + + 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( + [isaac_lab_joint_names.index(pink_joint) for pink_joint in pink_joint_names] + ) + self.pink_to_isaac_lab_ordering = np.array( + [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 + + 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. + + This method updates the target joint positions for null space posture tasks based on the current + joint configuration. This is useful for maintaining desired joint configurations when the primary + task allows redundancy. + + Args: + curr_joint_pos: The current joint positions of shape (num_joints,). + """ + for task in self.cfg.variable_input_tasks: + if isinstance(task, NullSpacePostureTask): + task.set_target(curr_joint_pos) + + def compute( + self, + curr_joint_pos: np.ndarray, + dt: float, + ) -> torch.Tensor: + """Compute the target joint positions based on current state and tasks. + + Performs inverse kinematics using the Pink solver to compute target joint positions that satisfy + the defined tasks. The solver uses quadratic programming to find optimal joint velocities that + minimize task errors while respecting constraints. + + Args: + curr_joint_pos: The current joint positions of shape (num_joints,). + dt: The time step for computing joint position changes in seconds. + + Returns: + 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) + + # Solve IK using Pink's solver + try: + velocity = solve_ik( + self.pink_configuration, + self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks, + dt, + solver="daqp", + safety_break=self.cfg.fail_on_joint_limit_violation, + ) + joint_angle_changes = velocity * dt + except (AssertionError, Exception) as e: + # Print warning and return the current joint positions as the target + if self.cfg.show_ik_warnings: + print( + "Warning: IK quadratic solver could not find a solution! Did not update the target joint" + f" positions.\nError: {e}" + ) + + if self.cfg.xr_enabled: + from isaaclab.ui.xr_widgets import XRVisualization + + XRVisualization.push_event("ik_error", {"error": e}) + 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( + joint_angle_changes[self.pink_to_isaac_lab_controlled_ordering], + device=self.device, + 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_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 new file mode 100644 index 000000000000..a66c4aec6658 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py @@ -0,0 +1,89 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for 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 environment 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 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. + """ + + 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.""" + + fail_on_joint_limit_violation: bool = True + """Whether to fail on joint limit violation. + + If True, the Pink IK solver will fail and raise an error if any joint limit is violated during optimization. + The PinkIKController will handle the error by setting the last joint positions. + + If False, the solver will ignore joint limit violations and return the closest solution found. + """ + + xr_enabled: bool = False + """If True, the Pink IK controller will send information to the XRVisualization.""" 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 000000000000..cd935390f0aa --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py @@ -0,0 +1,188 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import 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. + + + 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. + + Args: + urdf_path: Path to the robot URDF file. + mesh_path: Path to the mesh files for the robot. + controlled_joint_names: List of joint names to be actively controlled. + copy_data: If True, work on an internal copy of the input data. Defaults to True. + forward_kinematics: If True, compute forward kinematics from the configuration vector. Defaults to True. + """ + 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/rmp_flow.py b/source/isaaclab/isaaclab/controllers/rmp_flow.py index 01352d2a9762..f2766d702418 100644 --- a/source/isaaclab/isaaclab/controllers/rmp_flow.py +++ b/source/isaaclab/isaaclab/controllers/rmp_flow.py @@ -1,18 +1,26 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import 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.articulations import Articulation +import torch + +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 +import isaaclab.sim as sim_utils from isaaclab.utils import configclass +from isaaclab.utils.assets import retrieve_file_path @configclass @@ -71,9 +79,9 @@ def initialize(self, prim_paths_expr: str): prim_paths_expr: The expression to find the articulation prim paths. """ # obtain the simulation time - physics_dt = SimulationContext.instance().get_physics_dt() + physics_dt = sim_utils.SimulationContext.instance().get_physics_dt() # find all prims - self._prim_paths = prim_utils.find_matching_prim_paths(prim_paths_expr) + self._prim_paths = sim_utils.find_matching_prim_paths(prim_paths_expr) self.num_robots = len(self._prim_paths) # resolve controller if self.cfg.name == "rmp_flow": @@ -86,13 +94,19 @@ def initialize(self, prim_paths_expr: str): self.articulation_policies = list() for prim_path in self._prim_paths: # add robot reference - robot = Articulation(prim_path) + robot = SingleArticulation(prim_path) robot.initialize() + # download files if they are not local + + local_urdf_file = retrieve_file_path(self.cfg.urdf_file, force_download=True) + local_collision_file = retrieve_file_path(self.cfg.collision_file, force_download=True) + local_config_file = retrieve_file_path(self.cfg.config_file, force_download=True) + # 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, + robot_description_path=local_collision_file, + urdf_path=local_urdf_file, + rmpflow_config_path=local_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, diff --git a/source/isaaclab/isaaclab/controllers/utils.py b/source/isaaclab/isaaclab/controllers/utils.py new file mode 100644 index 000000000000..7e72912fdfda --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/utils.py @@ -0,0 +1,138 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Helper functions for Isaac Lab controllers. + +This module provides utility functions to help with controller implementations. +""" + +import logging +import os +import re + +from isaacsim.core.utils.extensions import enable_extension + +enable_extension("isaacsim.asset.exporter.urdf") + +from nvidia.srl.from_usd.to_urdf import UsdToUrdf + +# import logger +logger = logging.getLogger(__name__) + + +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": logging.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: + logger.warning(f"Replacing {joint} with fixed joint") + logger.warning(old_str) + logger.warning(new_str) + if old_str not in content: + logger.warning(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) + + +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: + logger.warning(f"Replacing {joint} with fixed joint") + logger.warning(old_str) + logger.warning(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/__init__.py b/source/isaaclab/isaaclab/devices/__init__.py index 2b102e2ade5f..b2605d39ca16 100644 --- a/source/isaaclab/isaaclab/devices/__init__.py +++ b/source/isaaclab/isaaclab/devices/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,6 +11,7 @@ * **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. +* **Haply**: Haptic device (Inverse3 + VerseGrip) with position, orientation tracking and force feedback. 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 @@ -19,8 +20,11 @@ the peripheral device. """ -from .device_base import DeviceBase -from .gamepad import Se2Gamepad, Se3Gamepad -from .keyboard import Se2Keyboard, Se3Keyboard -from .openxr import Se3HandTracking -from .spacemouse import Se2SpaceMouse, Se3SpaceMouse +from .device_base import DeviceBase, DeviceCfg, DevicesCfg +from .gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg +from .haply import HaplyDevice, HaplyDeviceCfg +from .keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg +from .openxr import ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg +from .retargeter_base import RetargeterBase, RetargeterCfg +from .spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from .teleop_device_factory import create_teleop_device diff --git a/source/isaaclab/isaaclab/devices/device_base.py b/source/isaaclab/isaaclab/devices/device_base.py index 672f0ac8979c..a434bcc73cfd 100644 --- a/source/isaaclab/isaaclab/devices/device_base.py +++ b/source/isaaclab/isaaclab/devices/device_base.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,18 +7,66 @@ from abc import ABC, abstractmethod from collections.abc import Callable +from dataclasses import dataclass, field +from enum import Enum from typing import Any +import torch + +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +@dataclass +class DeviceCfg: + """Configuration for teleoperation devices.""" + + # Whether teleoperation should start active by default + teleoperation_active_default: bool = True + # Torch device string to place output tensors on + sim_device: str = "cpu" + # Retargeters that transform device data into robot commands + retargeters: list[RetargeterCfg] = field(default_factory=list) + # Concrete device class to construct for this config. Set by each device module. + class_type: type["DeviceBase"] | None = None + + +@dataclass +class DevicesCfg: + """Configuration for all supported teleoperation devices.""" + + devices: dict[str, DeviceCfg] = field(default_factory=dict) + class DeviceBase(ABC): - """An interface class for teleoperation devices.""" + """An interface class for teleoperation devices. + + Derived classes have two implementation options: - def __init__(self): - """Initialize the teleoperation interface.""" - pass + 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 [] + # Aggregate required features across all retargeters + self._required_features = set() + for retargeter in self._retargeters: + self._required_features.update(retargeter.get_requirements()) def __str__(self) -> str: - """Returns: A string containing the information of joystick.""" + """Returns: A string identifier for the device.""" return f"{self.__class__.__name__}" """ @@ -41,11 +89,72 @@ def add_callback(self, key: Any, func: Callable): """ raise NotImplementedError - @abstractmethod - def advance(self) -> Any: - """Provides the joystick event state. + 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: - The processed output form the joystick. + 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 + raise NotImplementedError("Derived class must implement _get_raw_data() or override advance()") + + def advance(self) -> torch.Tensor: + """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: + When no retargeters are configured, returns raw device data in its native format. + When retargeters are configured, returns a torch.Tensor containing the concatenated + outputs from all retargeters. + """ + 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 + # Concatenate retargeted outputs into a single tensor + return torch.cat([retargeter.retarget(raw_data) for retargeter in self._retargeters], dim=-1) + + # ----------------------------- + # Shared data layout helpers (for retargeters across devices) + # ----------------------------- + class TrackingTarget(Enum): + """Standard tracking targets shared across devices.""" + + HAND_LEFT = 0 + HAND_RIGHT = 1 + HEAD = 2 + CONTROLLER_LEFT = 3 + CONTROLLER_RIGHT = 4 + + class MotionControllerDataRowIndex(Enum): + """Rows in the motion-controller 2x7 array.""" + + POSE = 0 + INPUTS = 1 + + class MotionControllerInputIndex(Enum): + """Indices in the motion-controller input row.""" + + THUMBSTICK_X = 0 + THUMBSTICK_Y = 1 + TRIGGER = 2 + SQUEEZE = 3 + BUTTON_0 = 4 + BUTTON_1 = 5 + PADDING = 6 diff --git a/source/isaaclab/isaaclab/devices/gamepad/__init__.py b/source/isaaclab/isaaclab/devices/gamepad/__init__.py index 4add06efab6e..8f8ec66aa4ec 100644 --- a/source/isaaclab/isaaclab/devices/gamepad/__init__.py +++ b/source/isaaclab/isaaclab/devices/gamepad/__init__.py @@ -1,9 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Gamepad device for SE(2) and SE(3) control.""" -from .se2_gamepad import Se2Gamepad -from .se3_gamepad import Se3Gamepad +from .se2_gamepad import Se2Gamepad, Se2GamepadCfg +from .se3_gamepad import Se3Gamepad, Se3GamepadCfg diff --git a/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py b/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py index 828f9ce0e5fe..5954c3c6918e 100644 --- a/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py +++ b/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py @@ -1,18 +1,24 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Gamepad controller for SE(2) control.""" -import numpy as np +from __future__ import annotations + import weakref from collections.abc import Callable +from dataclasses import dataclass + +import numpy as np +import torch import carb +import carb.input import omni -from ..device_base import DeviceBase +from ..device_base import DeviceBase, DeviceCfg class Se2Gamepad(DeviceBase): @@ -41,10 +47,7 @@ class Se2Gamepad(DeviceBase): 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, + cfg: Se2GamepadCfg, ): """Initialize the gamepad layer. @@ -59,10 +62,11 @@ def __init__( 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 + self.v_x_sensitivity = cfg.v_x_sensitivity + self.v_y_sensitivity = cfg.v_y_sensitivity + self.omega_z_sensitivity = cfg.omega_z_sensitivity + self.dead_zone = cfg.dead_zone + self._sim_device = cfg.sim_device # acquire omniverse interfaces self._appwindow = omni.appwindow.get_default_app_window() self._input = carb.input.acquire_input_interface() @@ -87,7 +91,7 @@ def __init__( def __del__(self): """Unsubscribe from gamepad events.""" - self._input.unsubscribe_from_gamepad_events(self._gamepad, self._gamepad_sub) + self._input.unsubscribe_to_gamepad_events(self._gamepad, self._gamepad_sub) self._gamepad_sub = None def __str__(self) -> str: @@ -120,13 +124,14 @@ def add_callback(self, key: carb.input.GamepadInput, func: Callable): """ self._additional_callbacks[key] = func - def advance(self) -> np.ndarray: + def advance(self) -> torch.Tensor: """Provides the result from gamepad event state. Returns: - A 3D array containing the linear (x,y) and angular velocity (z). + A tensor containing the linear (x,y) and angular velocity (z). """ - return self._resolve_command_buffer(self._base_command_raw) + numpy_result = self._resolve_command_buffer(self._base_command_raw) + return torch.tensor(numpy_result, dtype=torch.float32, device=self._sim_device) """ Internal helpers. @@ -197,3 +202,14 @@ def _resolve_command_buffer(self, raw_command: np.ndarray) -> np.ndarray: command[command_sign] *= -1 return command + + +@dataclass +class Se2GamepadCfg(DeviceCfg): + """Configuration for SE2 gamepad devices.""" + + v_x_sensitivity: float = 1.0 + v_y_sensitivity: float = 1.0 + omega_z_sensitivity: float = 1.0 + dead_zone: float = 0.01 + class_type: type[DeviceBase] = Se2Gamepad diff --git a/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py b/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py index 43615d4e86cf..2520de6247eb 100644 --- a/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py +++ b/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py @@ -1,19 +1,24 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Gamepad controller for SE(3) control.""" -import numpy as np +from __future__ import annotations + import weakref from collections.abc import Callable +from dataclasses import dataclass + +import numpy as np +import torch from scipy.spatial.transform import Rotation import carb import omni -from ..device_base import DeviceBase +from ..device_base import DeviceBase, DeviceCfg class Se3Gamepad(DeviceBase): @@ -47,22 +52,24 @@ class Se3Gamepad(DeviceBase): """ - def __init__(self, pos_sensitivity: float = 1.0, rot_sensitivity: float = 1.6, dead_zone: float = 0.01): + def __init__( + self, + cfg: Se3GamepadCfg, + ): """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. + cfg: Configuration object for gamepad settings. """ # 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 + self.pos_sensitivity = cfg.pos_sensitivity + self.rot_sensitivity = cfg.rot_sensitivity + self.dead_zone = cfg.dead_zone + self.gripper_term = cfg.gripper_term + self._sim_device = cfg.sim_device # acquire omniverse interfaces self._appwindow = omni.appwindow.get_default_app_window() self._input = carb.input.acquire_input_interface() @@ -88,7 +95,7 @@ def __init__(self, pos_sensitivity: float = 1.0, rot_sensitivity: float = 1.6, d def __del__(self): """Unsubscribe from gamepad events.""" - self._input.unsubscribe_from_gamepad_events(self._gamepad, self._gamepad_sub) + self._input.unsubscribe_to_gamepad_events(self._gamepad, self._gamepad_sub) self._gamepad_sub = None def __str__(self) -> str: @@ -127,11 +134,13 @@ def add_callback(self, key: carb.input.GamepadInput, func: Callable): """ self._additional_callbacks[key] = func - def advance(self) -> tuple[np.ndarray, bool]: + def advance(self) -> torch.Tensor: """Provides the result from gamepad event state. Returns: - A tuple containing the delta pose command and gripper commands. + torch.Tensor: A 7-element tensor containing: + - delta pose: First 6 elements as [x, y, z, rx, ry, rz] in meters and radians. + - gripper command: Last element as a binary value (+1.0 for open, -1.0 for close). """ # -- resolve position command delta_pos = self._resolve_command_buffer(self._delta_pose_raw[:, :3]) @@ -140,7 +149,12 @@ def advance(self) -> tuple[np.ndarray, bool]: # -- 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 + command = np.concatenate([delta_pos, rot_vec]) + if self.gripper_term: + gripper_value = -1.0 if self._close_gripper else 1.0 + command = np.append(command, gripper_value) + + return torch.tensor(command, dtype=torch.float32, device=self._sim_device) """ Internal helpers. @@ -242,3 +256,14 @@ def _resolve_command_buffer(self, raw_command: np.ndarray) -> np.ndarray: delta_command[delta_command_sign] *= -1 return delta_command + + +@dataclass +class Se3GamepadCfg(DeviceCfg): + """Configuration for SE3 gamepad devices.""" + + gripper_term: bool = True + dead_zone: float = 0.01 # For gamepad devices + pos_sensitivity: float = 1.0 + rot_sensitivity: float = 1.6 + class_type: type[DeviceBase] = Se3Gamepad diff --git a/source/isaaclab/isaaclab/devices/haply/__init__.py b/source/isaaclab/isaaclab/devices/haply/__init__.py new file mode 100644 index 000000000000..b86030f80e7a --- /dev/null +++ b/source/isaaclab/isaaclab/devices/haply/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Haply device interface for teleoperation.""" + +from .se3_haply import HaplyDevice, HaplyDeviceCfg + +__all__ = ["HaplyDevice", "HaplyDeviceCfg"] diff --git a/source/isaaclab/isaaclab/devices/haply/se3_haply.py b/source/isaaclab/isaaclab/devices/haply/se3_haply.py new file mode 100644 index 000000000000..9d9c06c92dc7 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/haply/se3_haply.py @@ -0,0 +1,395 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Haply device controller for SE3 control with force feedback.""" + +from __future__ import annotations + +import asyncio +import json +import threading +import time +from collections.abc import Callable +from dataclasses import dataclass + +import numpy as np +import torch + +try: + import websockets + + WEBSOCKETS_AVAILABLE = True +except ImportError: + WEBSOCKETS_AVAILABLE = False + +from ..device_base import DeviceBase, DeviceCfg +from ..retargeter_base import RetargeterBase + + +class HaplyDevice(DeviceBase): + """A Haply device controller for sending SE(3) commands with force feedback. + + This class provides an interface to Haply robotic devices (Inverse3 + VerseGrip) + for teleoperation. It communicates via WebSocket and supports: + + - Position tracking from Inverse3 device + - Orientation and button inputs from VerseGrip device + - Directional force feedback to Inverse3 + - Real-time data streaming at configurable rates + + The device provides raw data: + + * Position: 3D position (x, y, z) in meters from Inverse3 + * Orientation: Quaternion (x, y, z, w) from VerseGrip + * Buttons: Three buttons (a, b, c) from VerseGrip with state (pressed/not pressed) + + Note: All button logic (e.g., gripper control, reset, mode switching) should be + implemented in the application layer using the raw button states from advance(). + + Note: + Requires the Haply SDK to be running and accessible via WebSocket. + Install dependencies: pip install websockets + + """ + + def __init__(self, cfg: HaplyDeviceCfg, retargeters: list[RetargeterBase] | None = None): + """Initialize the Haply device interface. + + Args: + cfg: Configuration object for Haply device settings. + retargeters: Optional list of retargeting components that transform device data + into robot commands. If None or empty, the device outputs its native data format. + + Raises: + ImportError: If websockets module is not installed. + RuntimeError: If connection to Haply device fails. + """ + super().__init__(retargeters) + + if not WEBSOCKETS_AVAILABLE: + raise ImportError("websockets module is required for Haply device. Install with: pip install websockets") + + # Store configuration + self.websocket_uri = cfg.websocket_uri + self.pos_sensitivity = cfg.pos_sensitivity + self.data_rate = cfg.data_rate + self._sim_device = cfg.sim_device + self.limit_force = cfg.limit_force + + # Device status (True only when both Inverse3 and VerseGrip are connected) + self.connected = False + self._connected_lock = threading.Lock() + + # Device IDs (will be set after first message) + self.inverse3_device_id = None + self.verse_grip_device_id = None + + # Current data cache + self.cached_data = { + "position": np.zeros(3, dtype=np.float32), + "quaternion": np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), + "buttons": {"a": False, "b": False, "c": False}, + "inverse3_connected": False, + "versegrip_connected": False, + } + + self.data_lock = threading.Lock() + + # Force feedback + self.feedback_force = {"x": 0.0, "y": 0.0, "z": 0.0} + self.force_lock = threading.Lock() + + self._additional_callbacks = dict() + + # Button state tracking + self._prev_buttons = {"a": False, "b": False, "c": False} + + # Connection monitoring + self.consecutive_timeouts = 0 + self.max_consecutive_timeouts = 10 # ~10 seconds at 1s timeout + self.timeout_warning_issued = False + + # Start WebSocket connection + self.running = True + self._websocket_thread = None + self._start_websocket_thread() + + # Wait for both devices to connect + timeout = 5.0 + start_time = time.time() + while (time.time() - start_time) < timeout: + with self._connected_lock: + if self.connected: + break + time.sleep(0.1) + + with self._connected_lock: + if not self.connected: + raise RuntimeError(f"Failed to connect both Inverse3 and VerseGrip devices within {timeout}s. ") + + def __del__(self): + """Cleanup on deletion: shutdown WebSocket connection and background thread.""" + if not hasattr(self, "running") or not self.running: + return + + self.running = False + + # Reset force feedback before closing + if hasattr(self, "force_lock") and hasattr(self, "feedback_force"): + with self.force_lock: + self.feedback_force = {"x": 0.0, "y": 0.0, "z": 0.0} + + # Explicitly wait for WebSocket thread to finish + if hasattr(self, "_websocket_thread") and self._websocket_thread is not None: + if self._websocket_thread.is_alive(): + self._websocket_thread.join(timeout=2.0) + if self._websocket_thread.is_alive(): + self._websocket_thread.daemon = True + + def __str__(self) -> str: + """Returns: A string containing the information of the device.""" + msg = f"Haply Device Controller: {self.__class__.__name__}\n" + msg += f"\tWebSocket URI: {self.websocket_uri}\n" + msg += f"\tInverse3 ID: {self.inverse3_device_id}\n" + msg += f"\tVerseGrip ID: {self.verse_grip_device_id}\n" + msg += "\t----------------------------------------------\n" + msg += "\tOutput: [x, y, z, qx, qy, qz, qw, btn_a, btn_b, btn_c]\n" + msg += "\tInverse3: Provides position (x, y, z) and force feedback\n" + msg += "\tVerseGrip: Provides orientation (quaternion) and buttons (a, b, c)" + return msg + + def reset(self): + """Reset the device internal state.""" + with self.force_lock: + self.feedback_force = {"x": 0.0, "y": 0.0, "z": 0.0} + + # Reset button state tracking + self._prev_buttons = {"a": False, "b": False, "c": False} + + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind to button events. + + Args: + key: The button to check against. Valid values are "a", "b", "c". + func: The function to call when button is pressed. The callback function should not + take any arguments. + """ + if key not in ["a", "b", "c"]: + raise ValueError(f"Invalid button key: {key}. Valid keys are 'a', 'b', 'c'.") + self._additional_callbacks[key] = func + + def advance(self) -> torch.Tensor: + """Provides the result from Haply device state. + + Returns: + torch.Tensor: A tensor containing the raw device data: + - 10 elements: [x, y, z, qx, qy, qz, qw, button_a, button_b, button_c] + where (x, y, z) is position, (qx, qy, qz, qw) is quaternion orientation, + and buttons are 1.0 (pressed) or 0.0 (not pressed) + """ + with self.data_lock: + if not (self.cached_data["inverse3_connected"] and self.cached_data["versegrip_connected"]): + raise RuntimeError("Haply devices not connected. Both Inverse3 and VerseGrip must be connected.") + + # Safe copy within lock + position = self.cached_data["position"].copy() * self.pos_sensitivity + quaternion = self.cached_data["quaternion"].copy() + button_a = self.cached_data["buttons"].get("a", False) + button_b = self.cached_data["buttons"].get("b", False) + button_c = self.cached_data["buttons"].get("c", False) + + # Button callbacks execute OUTSIDE lock to prevent deadlock + for button_key, current_state in [("a", button_a), ("b", button_b), ("c", button_c)]: + prev_state = self._prev_buttons.get(button_key, False) + + if current_state and not prev_state: + if button_key in self._additional_callbacks: + self._additional_callbacks[button_key]() + + self._prev_buttons[button_key] = current_state + + button_states = np.array( + [ + 1.0 if button_a else 0.0, + 1.0 if button_b else 0.0, + 1.0 if button_c else 0.0, + ], + dtype=np.float32, + ) + + # Construct command tensor: [position(3), quaternion(4), buttons(3)] + command = np.concatenate([position, quaternion, button_states]) + + return torch.tensor(command, dtype=torch.float32, device=self._sim_device) + + def push_force(self, forces: torch.Tensor, position: torch.Tensor) -> None: + """Push force vector to Haply Inverse3 device. + + Overrides DeviceBase.push_force() to provide force feedback for Haply Inverse3. + Forces are clipped to [-limit_force, limit_force] range for safety. + + Args: + forces: Tensor of shape (N, 3) with forces [fx, fy, fz]. + position: Tensor of shape (N) with indices specifying which forces to use. + """ + # Check if forces is empty + if forces.shape[0] == 0: + raise ValueError("No forces provided") + + # Select forces using position indices + selected_forces = forces[position] if position.ndim > 0 else forces[position].unsqueeze(0) + force = selected_forces.sum(dim=0) + force = force.cpu().numpy() if force.is_cuda else force.numpy() + + fx = np.clip(force[0], -self.limit_force, self.limit_force) + fy = np.clip(force[1], -self.limit_force, self.limit_force) + fz = np.clip(force[2], -self.limit_force, self.limit_force) + + with self.force_lock: + self.feedback_force = {"x": float(fx), "y": float(fy), "z": float(fz)} + + def _start_websocket_thread(self): + """Start WebSocket connection thread.""" + + def websocket_thread(): + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + loop.run_until_complete(self._websocket_loop()) + + self._websocket_thread = threading.Thread(target=websocket_thread, daemon=False) + self._websocket_thread.start() + + async def _websocket_loop(self): + """WebSocket data reading and writing loop.""" + while self.running: + try: + async with websockets.connect(self.websocket_uri, ping_interval=None, ping_timeout=None) as ws: + first_message = True + + while self.running: + try: + response = await asyncio.wait_for(ws.recv(), timeout=1.0) + data = json.loads(response) + + self.consecutive_timeouts = 0 + if self.timeout_warning_issued: + self.timeout_warning_issued = False + + # Safe array access - no IndexError risk with ternary operator + inverse3_list = data.get("inverse3", []) + verse_grip_list = data.get("wireless_verse_grip", []) + inverse3_data = inverse3_list[0] if inverse3_list else {} + verse_grip_data = verse_grip_list[0] if verse_grip_list else {} + + if first_message: + first_message = False + if inverse3_data: + self.inverse3_device_id = inverse3_data.get("device_id") + if verse_grip_data: + self.verse_grip_device_id = verse_grip_data.get("device_id") + + with self.data_lock: + inverse3_connected = False + versegrip_connected = False + + if inverse3_data and "state" in inverse3_data: + cursor_pos = inverse3_data["state"].get("cursor_position", {}) + if cursor_pos: + self.cached_data["position"] = np.array( + [cursor_pos.get(k, 0.0) for k in ("x", "y", "z")], dtype=np.float32 + ) + inverse3_connected = True + + if verse_grip_data and "state" in verse_grip_data: + state = verse_grip_data["state"] + self.cached_data["buttons"] = { + k: state.get("buttons", {}).get(k, False) for k in ("a", "b", "c") + } + orientation = state.get("orientation", {}) + if orientation: + self.cached_data["quaternion"] = np.array( + [ + orientation.get(k, 1.0 if k == "w" else 0.0) + for k in ("x", "y", "z", "w") + ], + dtype=np.float32, + ) + versegrip_connected = True + + self.cached_data["inverse3_connected"] = inverse3_connected + self.cached_data["versegrip_connected"] = versegrip_connected + # Both devices required (AND logic): Inverse3 for position/force, + both_connected = inverse3_connected and versegrip_connected + + with self._connected_lock: + self.connected = both_connected + + # Send force feedback + if self.inverse3_device_id: + with self.force_lock: + current_force = self.feedback_force.copy() + + request_msg = { + "inverse3": [ + { + "device_id": self.inverse3_device_id, + "commands": {"set_cursor_force": {"values": current_force}}, + } + ] + } + await ws.send(json.dumps(request_msg)) + + await asyncio.sleep(1.0 / self.data_rate) + + except asyncio.TimeoutError: + self.consecutive_timeouts += 1 + + # Check if timeout + if ( + self.consecutive_timeouts >= self.max_consecutive_timeouts + and not self.timeout_warning_issued + ): + self.timeout_warning_issued = True + with self.data_lock: + self.cached_data["inverse3_connected"] = False + self.cached_data["versegrip_connected"] = False + with self._connected_lock: + self.connected = False + continue + except Exception as e: + print(f"[ERROR] Error in WebSocket receive loop: {e}") + break + + except Exception: + with self.data_lock: + self.cached_data["inverse3_connected"] = False + self.cached_data["versegrip_connected"] = False + with self._connected_lock: + self.connected = False + self.consecutive_timeouts = 0 + self.timeout_warning_issued = False + + if self.running: + await asyncio.sleep(2.0) + else: + break + + +@dataclass +class HaplyDeviceCfg(DeviceCfg): + """Configuration for Haply device. + + Attributes: + websocket_uri: WebSocket URI for Haply SDK connection + pos_sensitivity: Position sensitivity scaling factor + data_rate: Data exchange rate in Hz + limit_force: Maximum force magnitude in Newtons (safety limit) + """ + + websocket_uri: str = "ws://localhost:10001" + pos_sensitivity: float = 1.0 + data_rate: float = 200.0 + limit_force: float = 2.0 + class_type: type[DeviceBase] = HaplyDevice diff --git a/source/isaaclab/isaaclab/devices/keyboard/__init__.py b/source/isaaclab/isaaclab/devices/keyboard/__init__.py index 846aa2353a53..eff757a6d132 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/__init__.py +++ b/source/isaaclab/isaaclab/devices/keyboard/__init__.py @@ -1,9 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Keyboard device for SE(2) and SE(3) control.""" -from .se2_keyboard import Se2Keyboard -from .se3_keyboard import Se3Keyboard +from .se2_keyboard import Se2Keyboard, Se2KeyboardCfg +from .se3_keyboard import Se3Keyboard, Se3KeyboardCfg diff --git a/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py index 8fe467cd0608..beb19d1835d7 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py @@ -1,18 +1,23 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Keyboard controller for SE(2) control.""" -import numpy as np +from __future__ import annotations + import weakref from collections.abc import Callable +from dataclasses import dataclass + +import numpy as np +import torch import carb import omni -from ..device_base import DeviceBase +from ..device_base import DeviceBase, DeviceCfg class Se2Keyboard(DeviceBase): @@ -39,7 +44,7 @@ class Se2Keyboard(DeviceBase): """ - def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, omega_z_sensitivity: float = 1.0): + def __init__(self, cfg: Se2KeyboardCfg): """Initialize the keyboard layer. Args: @@ -48,9 +53,11 @@ def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, o 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 + self.v_x_sensitivity = cfg.v_x_sensitivity + self.v_y_sensitivity = cfg.v_y_sensitivity + self.omega_z_sensitivity = cfg.omega_z_sensitivity + self._sim_device = cfg.sim_device + # acquire omniverse interfaces self._appwindow = omni.appwindow.get_default_app_window() self._input = carb.input.acquire_input_interface() @@ -69,7 +76,7 @@ def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, o 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: @@ -107,13 +114,13 @@ def add_callback(self, key: str, func: Callable): """ self._additional_callbacks[key] = func - def advance(self) -> np.ndarray: + def advance(self) -> torch.Tensor: """Provides the result from keyboard event state. Returns: - 3D array containing the linear (x,y) and angular velocity (z). + Tensor containing the linear (x,y) and angular velocity (z). """ - return self._base_command + return torch.tensor(self._base_command, dtype=torch.float32, device=self._sim_device) """ Internal helpers. @@ -165,3 +172,13 @@ def _create_key_bindings(self): "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, } + + +@dataclass +class Se2KeyboardCfg(DeviceCfg): + """Configuration for SE2 keyboard devices.""" + + v_x_sensitivity: float = 0.8 + v_y_sensitivity: float = 0.4 + omega_z_sensitivity: float = 1.0 + class_type: type[DeviceBase] = Se2Keyboard diff --git a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py index 5ddd203b5140..db6b17d1702a 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py @@ -1,19 +1,24 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Keyboard controller for SE(3) control.""" -import numpy as np +from __future__ import annotations + import weakref from collections.abc import Callable +from dataclasses import dataclass + +import numpy as np +import torch from scipy.spatial.transform import Rotation import carb import omni -from ..device_base import DeviceBase +from ..device_base import DeviceBase, DeviceCfg class Se3Keyboard(DeviceBase): @@ -47,16 +52,17 @@ class Se3Keyboard(DeviceBase): """ - def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): + def __init__(self, cfg: Se3KeyboardCfg): """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. + cfg: Configuration object for keyboard settings. """ # store inputs - self.pos_sensitivity = pos_sensitivity - self.rot_sensitivity = rot_sensitivity + self.pos_sensitivity = cfg.pos_sensitivity + self.rot_sensitivity = cfg.rot_sensitivity + self.gripper_term = cfg.gripper_term + self._sim_device = cfg.sim_device # acquire omniverse interfaces self._appwindow = omni.appwindow.get_default_app_window() self._input = carb.input.acquire_input_interface() @@ -77,7 +83,7 @@ def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): 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: @@ -117,16 +123,23 @@ def add_callback(self, key: str, func: Callable): """ self._additional_callbacks[key] = func - def advance(self) -> tuple[np.ndarray, bool]: + def advance(self) -> torch.Tensor: """Provides the result from keyboard event state. Returns: - A tuple containing the delta pose command and gripper commands. + torch.Tensor: A 7-element tensor containing: + - delta pose: First 6 elements as [x, y, z, rx, ry, rz] in meters and radians. + - gripper command: Last element as a binary value (+1.0 for open, -1.0 for close). """ # 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 + command = np.concatenate([self._delta_pos, rot_vec]) + if self.gripper_term: + gripper_value = -1.0 if self._close_gripper else 1.0 + command = np.append(command, gripper_value) + + return torch.tensor(command, dtype=torch.float32, device=self._sim_device) """ Internal helpers. @@ -186,3 +199,14 @@ def _create_key_bindings(self): "C": np.asarray([0.0, 0.0, 1.0]) * self.rot_sensitivity, "V": np.asarray([0.0, 0.0, -1.0]) * self.rot_sensitivity, } + + +@dataclass +class Se3KeyboardCfg(DeviceCfg): + """Configuration for SE3 keyboard devices.""" + + gripper_term: bool = True + pos_sensitivity: float = 0.4 + rot_sensitivity: float = 0.8 + retargeters: None = None + class_type: type[DeviceBase] = Se3Keyboard diff --git a/source/isaaclab/isaaclab/devices/openxr/__init__.py b/source/isaaclab/isaaclab/devices/openxr/__init__.py index f47593619a05..030fdbdd00b5 100644 --- a/source/isaaclab/isaaclab/devices/openxr/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/__init__.py @@ -1,8 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Keyboard device for SE(2) and SE(3) control.""" -from .se3_handtracking import Se3HandTracking +from .manus_vive import ManusVive, ManusViveCfg +from .openxr_device import OpenXRDevice, OpenXRDeviceCfg +from .xr_cfg import XrAnchorRotationMode, XrCfg, remove_camera_configs diff --git a/source/isaaclab/isaaclab/devices/openxr/common.py b/source/isaaclab/isaaclab/devices/openxr/common.py new file mode 100644 index 000000000000..088641c2886a --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/common.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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/source/isaaclab/isaaclab/devices/openxr/manus_vive.py b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py new file mode 100644 index 000000000000..f8a18a098a8c --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py @@ -0,0 +1,245 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Manus and Vive for teleoperation and interaction. +""" + +from __future__ import annotations + +import contextlib +from collections.abc import Callable +from dataclasses import dataclass + +import numpy as np +from packaging import version + +import carb + +from isaaclab.devices.openxr.common import HAND_JOINT_NAMES +from isaaclab.devices.retargeter_base import RetargeterBase +from isaaclab.utils.version import get_isaac_sim_version + +from ..device_base import DeviceBase, DeviceCfg +from .xr_cfg import XrCfg + +# For testing purposes, we need to mock the XRCore +XRCore = None + +with contextlib.suppress(ModuleNotFoundError): + from omni.kit.xr.core import XRCore + +from isaacsim.core.prims import SingleXFormPrim + +from .manus_vive_utils import HAND_JOINT_MAP, ManusViveIntegration + + +class ManusVive(DeviceBase): + """Manus gloves and Vive trackers for teleoperation and interaction. + + This device tracks hand joints using Manus gloves and Vive trackers and makes them available as: + + 1. A dictionary of tracking data (when used without retargeters) + 2. Retargeted commands for robot control (when retargeters are provided) + + The user needs to install the Manus SDK and add `{path_to_manus_sdk}/manus_sdk/lib` to `LD_LIBRARY_PATH`. + Data are acquired by `ManusViveIntegration` from `isaaclab.devices.openxr.manus_vive_utils`, including + + * Vive tracker poses in scene frame, calibrated from AVP wrist poses. + * Hand joints calculated from Vive wrist joints and Manus hand joints (relative to wrist). + * Vive trackers are automatically mapped to the left and right wrist joints. + + Raw data format (_get_raw_data output): consistent with :class:`OpenXRDevice`. + Joint names are defined in `HAND_JOINT_MAP` from `isaaclab.devices.openxr.manus_vive_utils`. + + Teleop commands: consistent with :class:`OpenXRDevice`. + + The device tracks 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. + """ + + TELEOP_COMMAND_EVENT_TYPE = "teleop_command" + + def __init__(self, cfg: ManusViveCfg, retargeters: list[RetargeterBase] | None = None): + """Initialize the Manus+Vive device. + + Args: + cfg: Configuration object for Manus+Vive settings. + retargeters: List of retargeter instances to use for transforming raw tracking data. + """ + super().__init__(retargeters) + # Enforce minimum Isaac Sim version (>= 5.1) + isaac_sim_version = get_isaac_sim_version() + if isaac_sim_version < version.parse("5.1"): + raise RuntimeError(f"ManusVive requires Isaac Sim >= 5.1. Detected version: '{isaac_sim_version}'.") + self._xr_cfg = 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._manus_vive = ManusViveIntegration() + + # Initialize dictionaries instead of arrays + default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_joint_poses_left = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_headpose = default_pose.copy() + + 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: + """Provide details about the device configuration, tracking settings, + and available gesture commands. + + Returns: + Formatted string with device information. + """ + + msg = f"Manus+Vive 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: 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 + + def reset(self): + """Reset cached joint and head poses.""" + default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_joint_poses_left = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_headpose = default_pose.copy() + + def add_callback(self, key: str, func: Callable): + """Register a callback for a given key. + + Args: + key: The message key to bind ('START', 'STOP', 'RESET'). + func: The function to invoke when the message key is received. + """ + self._additional_callbacks[key] = func + + def _get_raw_data(self) -> dict: + """Get the latest tracking data from Manus and Vive. + + Returns: + Dictionary with TrackingTarget enum keys (HAND_LEFT, HAND_RIGHT, HEAD) containing: + - Left hand joint poses: Dictionary of 26 joints with position and orientation + - Right hand joint poses: Dictionary of 26 joints with position and orientation + - Head pose: Single 7-element array with 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. + """ + hand_tracking_data = self._manus_vive.get_all_device_data()["manus_gloves"] + result = {"left": self._previous_joint_poses_left, "right": self._previous_joint_poses_right} + for joint, pose in hand_tracking_data.items(): + hand, index = joint.split("_") + joint_name = HAND_JOINT_MAP[int(index)] + result[hand][joint_name] = np.array(pose["position"] + pose["orientation"], dtype=np.float32) + return { + DeviceBase.TrackingTarget.HAND_LEFT: result["left"], + DeviceBase.TrackingTarget.HAND_RIGHT: result["right"], + DeviceBase.TrackingTarget.HEAD: self._calculate_headpose(), + } + + def _calculate_headpose(self) -> np.ndarray: + """Calculate the head pose from OpenXR. + + Returns: + 7-element numpy.ndarray [x, y, z, qw, qx, qy, qz]. + """ + head_device = XRCore.get_singleton().get_input_device("/user/head") + 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 _on_teleop_command(self, event: carb.events.IEvent): + """Handle teleoperation command events. + + Args: + event: The XR message-bus event containing a 'message' payload. + """ + 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"]() + + +@dataclass +class ManusViveCfg(DeviceCfg): + """Configuration for Manus and Vive.""" + + xr_cfg: XrCfg | None = None + class_type: type[DeviceBase] = ManusVive diff --git a/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py b/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py new file mode 100644 index 000000000000..80c0b346b315 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py @@ -0,0 +1,514 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import contextlib +import logging +from time import time + +import numpy as np + +from isaacsim.core.utils.extensions import enable_extension + +# For testing purposes, we need to mock the XRCore +XRCore, XRPoseValidityFlags = None, None + +with contextlib.suppress(ModuleNotFoundError): + from omni.kit.xr.core import XRCore, XRPoseValidityFlags + +from pxr import Gf + +# import logger +logger = logging.getLogger(__name__) + +# Mapping from Manus joint index (0-24) to joint name. Palm (25) is calculated from middle metacarpal and proximal. +HAND_JOINT_MAP = { + # Wrist + 0: "wrist", + # Thumb + 1: "thumb_metacarpal", + 2: "thumb_proximal", + 3: "thumb_distal", + 4: "thumb_tip", + # Index + 5: "index_metacarpal", + 6: "index_proximal", + 7: "index_intermediate", + 8: "index_distal", + 9: "index_tip", + # Middle + 10: "middle_metacarpal", + 11: "middle_proximal", + 12: "middle_intermediate", + 13: "middle_distal", + 14: "middle_tip", + # Ring + 15: "ring_metacarpal", + 16: "ring_proximal", + 17: "ring_intermediate", + 18: "ring_distal", + 19: "ring_tip", + # Little + 20: "little_metacarpal", + 21: "little_proximal", + 22: "little_intermediate", + 23: "little_distal", + 24: "little_tip", + # Palm + 25: "palm", +} + + +class ManusViveIntegration: + def __init__(self): + enable_extension("isaacsim.xr.input_devices") + from isaacsim.xr.input_devices.impl.manus_vive_integration import get_manus_vive_integration + + _manus_vive_integration = get_manus_vive_integration() + self.manus = _manus_vive_integration.manus_tracker + self.vive_tracker = _manus_vive_integration.vive_tracker + self.device_status = _manus_vive_integration.device_status + self.default_pose = {"position": [0, 0, 0], "orientation": [1, 0, 0, 0]} + # 90-degree ccw rotation on Y-axis and 90-degree ccw rotation on Z-axis + self.rot_adjust = Gf.Matrix3d().SetRotate(Gf.Quatd(0.5, Gf.Vec3d(-0.5, 0.5, 0.5))) + self.scene_T_lighthouse_static = None + self._vive_left_id = None + self._vive_right_id = None + self._pairA_candidates = [] # Pair A: WM0->Left, WM1->Right + self._pairB_candidates = [] # Pair B: WM1->Left, WM0->Right + self._pairA_trans_errs = [] + self._pairA_rot_errs = [] + self._pairB_trans_errs = [] + self._pairB_rot_errs = [] + + def get_all_device_data(self) -> dict: + """Get all tracked device data in scene coordinates. + + Returns: + Manus glove joint data and Vive tracker data. + { + 'manus_gloves': { + '{left/right}_{joint_index}': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + ... + }, + 'vive_trackers': { + '{vive_tracker_id}': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + ... + } + } + """ + self.update_manus() + self.update_vive() + # Get raw data from trackers + manus_data = self.manus.get_data() + vive_data = self.vive_tracker.get_data() + vive_transformed = self._transform_vive_data(vive_data) + scene_T_wrist = self._get_scene_T_wrist_matrix(vive_transformed) + + return { + "manus_gloves": self._transform_manus_data(manus_data, scene_T_wrist), + "vive_trackers": vive_transformed, + } + + def get_device_status(self) -> dict: + """Get connection and data freshness status for Manus gloves and Vive trackers. + + Returns: + Dictionary containing connection flags and last-data timestamps. + Format: { + 'manus_gloves': {'connected': bool, 'last_data_time': float}, + 'vive_trackers': {'connected': bool, 'last_data_time': float}, + 'left_hand_connected': bool, + 'right_hand_connected': bool + } + """ + return self.device_status + + def update_manus(self): + """Update raw Manus glove data and status flags.""" + self.manus.update() + self.device_status["manus_gloves"]["last_data_time"] = time() + manus_data = self.manus.get_data() + self.device_status["left_hand_connected"] = "left_0" in manus_data + self.device_status["right_hand_connected"] = "right_0" in manus_data + + def update_vive(self): + """Update raw Vive tracker data, and initialize coordinate transformation if it is the first data update.""" + self.vive_tracker.update() + self.device_status["vive_trackers"]["last_data_time"] = time() + try: + # Initialize coordinate transformation from first Vive wrist position + if self.scene_T_lighthouse_static is None: + self._initialize_coordinate_transformation() + except Exception as e: + logger.error(f"Vive tracker update failed: {e}") + + def _initialize_coordinate_transformation(self): + """Initialize the scene to lighthouse coordinate transformation. + + The coordinate transformation is used to transform the wrist pose from lighthouse + coordinate system to isaac sim scene coordinate. It is computed from multiple + frames of AVP/OpenXR wrist pose and Vive wrist pose samples at the beginning of the session. + """ + min_frames = 6 + tolerance = 3.0 + vive_data = self.vive_tracker.get_data() + wm0_id, wm1_id = get_vive_wrist_ids(vive_data) + if wm0_id is None and wm1_id is None: + return + + try: + # Fetch OpenXR wrists + L, R, gloves = None, None, [] + if self.device_status["left_hand_connected"]: + gloves.append("left") + L = get_openxr_wrist_matrix("left") + if self.device_status["right_hand_connected"]: + gloves.append("right") + R = get_openxr_wrist_matrix("right") + + M0, M1, vives = None, None, [] + if wm0_id is not None: + vives.append(wm0_id) + M0 = pose_to_matrix(vive_data[wm0_id]) + if wm1_id is not None: + vives.append(wm1_id) + M1 = pose_to_matrix(vive_data[wm1_id]) + + TL0, TL1, TR0, TR1 = None, None, None, None + # Compute transforms for available pairs + if wm0_id is not None and L is not None: + TL0 = M0.GetInverse() * L + self._pairA_candidates.append(TL0) + if wm1_id is not None and L is not None: + TL1 = M1.GetInverse() * L + self._pairB_candidates.append(TL1) + if wm1_id is not None and R is not None: + TR1 = M1.GetInverse() * R + self._pairA_candidates.append(TR1) + if wm0_id is not None and R is not None: + TR0 = M0.GetInverse() * R + self._pairB_candidates.append(TR0) + + # Per-frame pairing error if both candidates present + if TL0 is not None and TR1 is not None and TL1 is not None and TR0 is not None: + eT, eR = compute_delta_errors(TL0, TR1) + self._pairA_trans_errs.append(eT) + self._pairA_rot_errs.append(eR) + eT, eR = compute_delta_errors(TL1, TR0) + self._pairB_trans_errs.append(eT) + self._pairB_rot_errs.append(eR) + + # Choose a mapping + choose_A = None + if len(self._pairA_candidates) == 0 and len(self._pairB_candidates) >= min_frames: + choose_A = False + elif len(self._pairB_candidates) == 0 and len(self._pairA_candidates) >= min_frames: + choose_A = True + elif len(self._pairA_trans_errs) >= min_frames and len(self._pairB_trans_errs) >= min_frames: + errA = get_pairing_error(self._pairA_trans_errs, self._pairA_rot_errs) + errB = get_pairing_error(self._pairB_trans_errs, self._pairB_rot_errs) + if errA < errB and errA < tolerance: + 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") + logger.info( + f"Pairing Vive trackers with wrists: error of pairing A: {errA}, error of pairing B: {errB}" + ) + if choose_A is None: + return + + if choose_A: + chosen_list = self._pairA_candidates + self._vive_left_id, self._vive_right_id = wm0_id, wm1_id + else: + chosen_list = self._pairB_candidates + self._vive_left_id, self._vive_right_id = wm1_id, wm0_id + + if len(chosen_list) >= min_frames: + cluster = select_mode_cluster(chosen_list) + 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 + print( + f"Wrist calibration computed. Resolved mapping: {self._vive_left_id}->Left," + f" {self._vive_right_id}->Right" + ) + + except Exception as e: + logger.error(f"Failed to initialize coordinate transformation: {e}") + + def _transform_vive_data(self, device_data: dict) -> dict: + """Transform Vive tracker poses to scene coordinates. + + Args: + device_data: raw vive tracker poses, with device id as keys. + + Returns: + Vive tracker poses in scene coordinates, with device id as keys. + """ + transformed_data = {} + for joint_name, joint_data in device_data.items(): + transformed_pose = self.default_pose + if self.scene_T_lighthouse_static is not None: + transformed_matrix = pose_to_matrix(joint_data) * self.scene_T_lighthouse_static + transformed_pose = matrix_to_pose(transformed_matrix) + transformed_data[joint_name] = transformed_pose + return transformed_data + + def _get_scene_T_wrist_matrix(self, vive_data: dict) -> dict: + """Compute scene-frame wrist transforms for left and right hands. + + Args: + vive_data: Vive tracker poses expressed in scene coordinates. + + Returns: + Dictionary with 'left' and 'right' keys mapping to 4x4 transforms. + """ + scene_T_wrist = {"left": Gf.Matrix4d().SetIdentity(), "right": Gf.Matrix4d().SetIdentity()} + # 10 cm offset on Y-axis for change in vive tracker position after flipping the palm + Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, -0.1, 0)) + if self._vive_left_id is not None: + scene_T_wrist["left"] = Rcorr * pose_to_matrix(vive_data[self._vive_left_id]) + if self._vive_right_id is not None: + scene_T_wrist["right"] = Rcorr * pose_to_matrix(vive_data[self._vive_right_id]) + return scene_T_wrist + + def _transform_manus_data(self, manus_data: dict, scene_T_wrist: dict) -> dict: + """Transform Manus glove joints from wrist-relative to scene coordinates. + + Args: + manus_data: Raw Manus joint pose dictionary, wrist-relative. + scene_T_wrist: Dictionary of scene transforms for left and right wrists. + + Returns: + Dictionary of Manus joint poses in scene coordinates. + """ + Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, 0, 0)).GetInverse() + transformed_data = {} + for joint_name, joint_data in manus_data.items(): + hand, _ = joint_name.split("_") + joint_mat = Rcorr * pose_to_matrix(joint_data) * scene_T_wrist[hand] + transformed_data[joint_name] = matrix_to_pose(joint_mat) + # Calculate palm with middle metacarpal and proximal + transformed_data["left_25"] = self._get_palm(transformed_data, "left") + transformed_data["right_25"] = self._get_palm(transformed_data, "right") + return transformed_data + + def _get_palm(self, transformed_data: dict, hand: str) -> dict: + """Compute palm pose from middle metacarpal and proximal joints. + + Args: + transformed_data: Manus joint poses in scene coordinates. + hand: The hand side, either 'left' or 'right'. + + Returns: + Pose dictionary with 'position' and 'orientation'. + """ + if f"{hand}_6" not in transformed_data or f"{hand}_7" not in transformed_data: + # Joint data not arrived yet + return self.default_pose + metacarpal = transformed_data[f"{hand}_6"] + proximal = transformed_data[f"{hand}_7"] + pos = (np.array(metacarpal["position"]) + np.array(proximal["position"])) / 2.0 + return {"position": [pos[0], pos[1], pos[2]], "orientation": metacarpal["orientation"]} + + +def compute_delta_errors(a: Gf.Matrix4d, b: Gf.Matrix4d) -> tuple[float, float]: + """Compute translation and rotation error between two transforms. + + Args: + a: The first transform. + b: The second transform. + + Returns: + Tuple containing (translation_error_m, rotation_error_deg). + """ + try: + delta = a * b.GetInverse() + t = delta.ExtractTranslation() + trans_err = float(np.linalg.norm([t[0], t[1], t[2]])) + q = delta.ExtractRotation().GetQuat() + w = float(max(min(q.GetReal(), 1.0), -1.0)) + ang = 2.0 * float(np.arccos(w)) + ang_deg = float(np.degrees(ang)) + if ang_deg > 180.0: + ang_deg = 360.0 - ang_deg + return trans_err, ang_deg + except Exception: + return float("inf"), float("inf") + + +def average_transforms(mats: list[Gf.Matrix4d]) -> Gf.Matrix4d: + """Average rigid transforms across translations and quaternions. + + Args: + mats: The list of 4x4 transforms to average. + + Returns: + Averaged 4x4 transform, or None if the list is empty. + """ + if not mats: + return None + ref_quat = mats[0].ExtractRotation().GetQuat() + ref = np.array([ref_quat.GetReal(), *ref_quat.GetImaginary()]) + acc_q = np.zeros(4, dtype=np.float64) + acc_t = np.zeros(3, dtype=np.float64) + for m in mats: + t = m.ExtractTranslation() + acc_t += np.array([t[0], t[1], t[2]], dtype=np.float64) + q = m.ExtractRotation().GetQuat() + qi = np.array([q.GetReal(), *q.GetImaginary()], dtype=np.float64) + if np.dot(qi, ref) < 0.0: + qi = -qi + acc_q += qi + mean_t = acc_t / float(len(mats)) + norm = np.linalg.norm(acc_q) + if norm <= 1e-12: + quat_avg = Gf.Quatd(1.0, Gf.Vec3d(0.0, 0.0, 0.0)) + else: + qn = acc_q / norm + quat_avg = Gf.Quatd(float(qn[0]), Gf.Vec3d(float(qn[1]), float(qn[2]), float(qn[3]))) + rot3 = Gf.Matrix3d().SetRotate(quat_avg) + trans = Gf.Vec3d(float(mean_t[0]), float(mean_t[1]), float(mean_t[2])) + return Gf.Matrix4d(rot3, trans) + + +def select_mode_cluster( + mats: list[Gf.Matrix4d], trans_thresh_m: float = 0.03, rot_thresh_deg: float = 10.0 +) -> list[Gf.Matrix4d]: + """Select the largest cluster of transforms under proximity thresholds. + + Args: + mats: The list of 4x4 transforms to cluster. + trans_thresh_m: The translation threshold in meters. + rot_thresh_deg: The rotation threshold in degrees. + + Returns: + The largest cluster (mode) of transforms. + """ + if not mats: + return [] + best_cluster: list[Gf.Matrix4d] = [] + for center in mats: + cluster: list[Gf.Matrix4d] = [] + for m in mats: + trans_err, rot_err = compute_delta_errors(m, center) + if trans_err <= trans_thresh_m and rot_err <= rot_thresh_deg: + cluster.append(m) + if len(cluster) > len(best_cluster): + best_cluster = cluster + return best_cluster + + +def get_openxr_wrist_matrix(hand: str) -> Gf.Matrix4d: + """Get the OpenXR wrist matrix if valid. + + Args: + hand: The hand side ('left' or 'right'). + + Returns: + 4x4 transform for the wrist if valid, otherwise None. + """ + hand = hand.lower() + try: + hand_device = XRCore.get_singleton().get_input_device(f"/user/hand/{hand}") + if hand_device is None: + return None + joints = hand_device.get_all_virtual_world_poses() + if "wrist" not in joints: + return None + joint = joints["wrist"] + required = XRPoseValidityFlags.POSITION_VALID | XRPoseValidityFlags.ORIENTATION_VALID + if (joint.validity_flags & required) != required: + return None + return joint.pose_matrix + except Exception as e: + logger.warning(f"OpenXR {hand} wrist fetch failed: {e}") + return None + + +def get_vive_wrist_ids(vive_data: dict) -> tuple[str, str]: + """Get the Vive wrist tracker IDs if available. + + Args: + vive_data: The raw Vive data dictionary. + + Returns: + (wm0_id, wm1_id) if available, otherwise None values. + """ + wm_ids = [k for k in vive_data.keys() if len(k) >= 2 and k[:2] == "WM"] + wm_ids.sort() + if len(wm_ids) >= 2: # Assumes the first two vive trackers are the wrist trackers + return wm_ids[0], wm_ids[1] + if len(wm_ids) == 1: + return wm_ids[0], None + return None, None + + +def pose_to_matrix(pose: dict) -> Gf.Matrix4d: + """Convert a pose dictionary to a 4x4 transform matrix. + + Args: + pose: The pose with 'position' and 'orientation' fields. + + Returns: + A 4x4 transform representing the pose. + """ + pos, ori = pose["position"], pose["orientation"] + quat = Gf.Quatd(ori[0], Gf.Vec3d(ori[1], ori[2], ori[3])) + rot = Gf.Matrix3d().SetRotate(quat) + trans = Gf.Vec3d(pos[0], pos[1], pos[2]) + return Gf.Matrix4d(rot, trans) + + +def matrix_to_pose(matrix: Gf.Matrix4d) -> dict: + """Convert a 4x4 transform matrix to a pose dictionary. + + Args: + matrix: The 4x4 transform matrix to convert. + + Returns: + Pose dictionary with 'position' and 'orientation'. + """ + pos = matrix.ExtractTranslation() + rot = matrix.ExtractRotation() + quat = rot.GetQuat() + return { + "position": [pos[0], pos[1], pos[2]], + "orientation": [quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]], + } + + +def get_pairing_error(trans_errs: list, rot_errs: list) -> float: + """Compute a scalar pairing error from translation and rotation errors. + + Args: + trans_errs: The list of translation errors across samples. + rot_errs: The list of rotation errors across samples. + + Returns: + The weighted sum of medians of translation and rotation errors. + """ + + def _median(values: list) -> float: + try: + return float(np.median(np.asarray(values, dtype=np.float64))) + except Exception: + return float("inf") + + return _median(trans_errs) + 0.01 * _median(rot_errs) diff --git a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py new file mode 100644 index 000000000000..49f423fe8a01 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py @@ -0,0 +1,511 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""OpenXR-powered device for teleoperation and interaction.""" + +from __future__ import annotations + +import contextlib +import logging +from collections.abc import Callable +from dataclasses import dataclass +from typing import Any + +import numpy as np + +import carb + +# import logger +logger = logging.getLogger(__name__) + +from isaaclab.devices.openxr.common import HAND_JOINT_NAMES +from isaaclab.devices.retargeter_base import RetargeterBase + +from ..device_base import DeviceBase, DeviceCfg +from .xr_anchor_utils import XrAnchorSynchronizer +from .xr_cfg import XrCfg + +# For testing purposes, we need to mock the XRCore, XRPoseValidityFlags classes +XRCore = None +XRPoseValidityFlags = None +XRCoreEventType = None + +with contextlib.suppress(ModuleNotFoundError): + from omni.kit.xr.core import XRCore, XRCoreEventType, XRPoseValidityFlags + +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 tracking data (when used without retargeters) + 2. Retargeted commands for robot control (when retargeters are provided) + + Raw data format (_get_raw_data output): + + * A dictionary with keys matching TrackingTarget enum values (HAND_LEFT, HAND_RIGHT, HEAD) + * Each hand tracking entry contains a dictionary of joint poses + * Each joint pose is a 7D vector (x, y, z, qw, qx, qy, qz) in meters and quaternion units + * Joint names are defined in HAND_JOINT_NAMES from 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 tracks 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. + """ + + TELEOP_COMMAND_EVENT_TYPE = "teleop_command" + + def __init__( + self, + cfg: OpenXRDeviceCfg, + retargeters: list[RetargeterBase] | None = None, + ): + """Initialize the OpenXR device. + + Args: + cfg: Configuration object for OpenXR settings. + retargeters: List of retargeter instances to use for transforming raw tracking data. + """ + super().__init__(retargeters) + self._xr_cfg = cfg.xr_cfg or XrCfg() + self._additional_callbacks = dict() + self._xr_core = XRCore.get_singleton() if XRCore is not None else None + self._vc_subscription = ( + self._xr_core.get_message_bus().create_subscription_to_pop_by_type( + carb.events.type_from_string(self.TELEOP_COMMAND_EVENT_TYPE), self._on_teleop_command + ) + if self._xr_core is not None + else None + ) + + # Initialize dictionaries instead of arrays + default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_joint_poses_left = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_headpose = default_pose.copy() + + if self._xr_cfg.anchor_prim_path is not None: + anchor_path = self._xr_cfg.anchor_prim_path + if anchor_path.endswith("/"): + anchor_path = anchor_path[:-1] + self._xr_anchor_headset_path = f"{anchor_path}/XRAnchor" + else: + self._xr_anchor_headset_path = "/World/XRAnchor" + + _ = SingleXFormPrim( + self._xr_anchor_headset_path, position=self._xr_cfg.anchor_pos, orientation=self._xr_cfg.anchor_rot + ) + + if hasattr(carb, "settings"): + 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", self._xr_anchor_headset_path) + + # Button binding support + self.__button_subscriptions: dict[str, dict] = {} + + # Optional anchor synchronizer + self._anchor_sync: XrAnchorSynchronizer | None = None + if self._xr_core is not None and self._xr_cfg.anchor_prim_path is not None: + try: + self._anchor_sync = XrAnchorSynchronizer( + xr_core=self._xr_core, + xr_cfg=self._xr_cfg, + xr_anchor_headset_path=self._xr_anchor_headset_path, + ) + # Subscribe to pre_sync_update to keep anchor in sync + if XRCoreEventType is not None: + self._xr_pre_sync_update_subscription = ( + self._xr_core.get_message_bus().create_subscription_to_pop_by_type( + XRCoreEventType.pre_sync_update, + lambda _: self._anchor_sync.sync_headset_to_anchor(), + name="isaaclab_xr_pre_sync_update", + ) + ) + except Exception as e: + logger.warning(f"XR: Failed to initialize anchor synchronizer: {e}") + + # Default convenience binding: toggle anchor rotation with right controller 'a' button + with contextlib.suppress(Exception): + self._bind_button_press( + "/user/hand/right", + "a", + "isaaclab_right_a", + lambda ev: self._toggle_anchor_rotation(), + ) + + 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 + if hasattr(self, "_xr_pre_sync_update_subscription") and self._xr_pre_sync_update_subscription is not None: + self._xr_pre_sync_update_subscription = None + # clear button subscriptions + if hasattr(self, "__button_subscriptions"): + self._unbind_all_buttons() + + # 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" + if self._xr_cfg.anchor_prim_path is not None: + msg += f"\tAnchor Prim Path: {self._xr_cfg.anchor_prim_path} (Dynamic Anchoring)\n" + else: + msg += "\tAnchor Mode: Static (Root Level)\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): + default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_joint_poses_left = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES} + self._previous_headpose = default_pose.copy() + if hasattr(self, "_anchor_sync") and self._anchor_sync is not None: + self._anchor_sync.reset() + + 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 with TrackingTarget enum keys (HAND_LEFT, HAND_RIGHT, HEAD) containing: + - Left hand joint poses: Dictionary of 26 joints with position and orientation + - Right hand joint poses: Dictionary of 26 joints with position and orientation + - Head pose: Single 7-element array with 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. + """ + data = {} + + if RetargeterBase.Requirement.HAND_TRACKING in self._required_features: + data[DeviceBase.TrackingTarget.HAND_LEFT] = self._calculate_joint_poses( + XRCore.get_singleton().get_input_device("/user/hand/left"), + self._previous_joint_poses_left, + ) + data[DeviceBase.TrackingTarget.HAND_RIGHT] = self._calculate_joint_poses( + XRCore.get_singleton().get_input_device("/user/hand/right"), + self._previous_joint_poses_right, + ) + + if RetargeterBase.Requirement.HEAD_TRACKING in self._required_features: + data[DeviceBase.TrackingTarget.HEAD] = self._calculate_headpose() + + if RetargeterBase.Requirement.MOTION_CONTROLLER in self._required_features: + # Optionally include motion controller pose+inputs if devices are available + try: + left_dev = XRCore.get_singleton().get_input_device("/user/hand/left") + right_dev = XRCore.get_singleton().get_input_device("/user/hand/right") + left_ctrl = self._query_controller(left_dev) if left_dev is not None else np.array([]) + right_ctrl = self._query_controller(right_dev) if right_dev is not None else np.array([]) + if left_ctrl.size: + data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] = left_ctrl + if right_ctrl.size: + data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] = right_ctrl + except Exception: + # Ignore controller data if XRCore/controller APIs are unavailable + pass + + return data + + """ + Internal helpers. + """ + + def _calculate_joint_poses( + self, hand_device: Any, previous_joint_poses: dict[str, np.ndarray] + ) -> dict[str, np.ndarray]: + """Calculate and update joint poses for a hand device. + + This function retrieves the current joint poses from the OpenXR hand device and updates + the previous joint poses with the new data. If a joint's position or orientation is not + valid, it will use the previous values. + + Args: + hand_device: The OpenXR input device for a hand (/user/hand/left or /user/hand/right). + previous_joint_poses: Dictionary mapping joint names to their previous poses. + Each pose is a 7-element array: [x, y, z, qw, qx, qy, qz]. + + Returns: + Updated dictionary of joint poses with the same structure as previous_joint_poses. + Each pose is represented as a 7-element numpy array: [x, y, z, qw, qx, qy, qz] + where the first 3 elements are position and the last 4 are quaternion orientation. + """ + if hand_device is None: + return previous_joint_poses + + joint_poses = hand_device.get_all_virtual_world_poses() + + # Update each joint that is present in the current data + for joint_name, joint_pose in joint_poses.items(): + if joint_name in HAND_JOINT_NAMES: + # Extract translation and rotation + if joint_pose.validity_flags & XRPoseValidityFlags.POSITION_VALID: + position = joint_pose.pose_matrix.ExtractTranslation() + else: + position = previous_joint_poses[joint_name][:3] + + if joint_pose.validity_flags & XRPoseValidityFlags.ORIENTATION_VALID: + quat = joint_pose.pose_matrix.ExtractRotationQuat() + quati = quat.GetImaginary() + quatw = quat.GetReal() + else: + quatw = previous_joint_poses[joint_name][3] + quati = previous_joint_poses[joint_name][4:] + + # Directly update the dictionary with new data + previous_joint_poses[joint_name] = np.array( + [position[0], position[1], position[2], quatw, quati[0], quati[1], quati[2]], dtype=np.float32 + ) + + # No need for conversion, just return the updated dictionary + return 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("/user/head") + 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 + + # ----------------------------- + # Controller button binding utilities + # ----------------------------- + def _bind_button_press( + self, + device_path: str, + button_name: str, + event_name: str, + on_button_press: Callable[[carb.events.IEvent], None], + ) -> None: + if self._xr_core is None: + logger.warning("XR core not available; skipping button binding") + return + + sub_key = f"{device_path}/{button_name}" + self.__button_subscriptions[sub_key] = {} + + def try_emit_button_events(): + if self.__button_subscriptions[sub_key].get("generator"): + return + device = self._xr_core.get_input_device(device_path) + if not device: + return + names = {str(n) for n in (device.get_input_names() or ())} + if button_name not in names: + return + gen = device.bind_event_generator(button_name, event_name, ("press",)) + if gen is not None: + logger.info(f"XR: Bound event generator for {sub_key}, {event_name}") + self.__button_subscriptions[sub_key]["generator"] = gen + + def on_inputs_change(_ev: carb.events.IEvent) -> None: + try_emit_button_events() + + def on_disable(_ev: carb.events.IEvent) -> None: + self.__button_subscriptions[sub_key]["generator"] = None + + message_bus = self._xr_core.get_message_bus() + event_suffix = device_path.strip("/").replace("/", "_") + self.__button_subscriptions[sub_key]["press_sub"] = message_bus.create_subscription_to_pop_by_type( + carb.events.type_from_string(f"{event_name}.press"), on_button_press + ) + self.__button_subscriptions[sub_key]["inputs_change_sub"] = message_bus.create_subscription_to_pop_by_type( + carb.events.type_from_string(f"xr_input.{event_suffix}.inputs_change"), on_inputs_change + ) + self.__button_subscriptions[sub_key]["disable_sub"] = message_bus.create_subscription_to_pop_by_type( + carb.events.type_from_string(f"xr_input.{event_suffix}.disable"), on_disable + ) + try_emit_button_events() + + def _unbind_all_buttons(self) -> None: + for sub_key, subs in self.__button_subscriptions.items(): + if "generator" in subs: + subs["generator"] = None + for key in ["press_sub", "inputs_change_sub", "disable_sub"]: + if key in subs: + subs[key] = None + self.__button_subscriptions.clear() + logger.info("XR: Unbound all button event handlers") + + def _toggle_anchor_rotation(self): + if self._anchor_sync is not None: + self._anchor_sync.toggle_anchor_rotation() + + def _query_controller(self, input_device) -> np.ndarray: + """Query motion controller pose and inputs as a 2x7 array. + + Row 0 (POSE): [x, y, z, w, x, y, z] + Row 1 (INPUTS): [thumbstick_x, thumbstick_y, trigger, squeeze, button_0, button_1, padding] + """ + if input_device is None: + return np.array([]) + + pose = input_device.get_virtual_world_pose() + position = pose.ExtractTranslation() + quat = pose.ExtractRotationQuat() + + thumbstick_x = 0.0 + thumbstick_y = 0.0 + trigger = 0.0 + squeeze = 0.0 + button_0 = 0.0 + button_1 = 0.0 + + if input_device.has_input_gesture("thumbstick", "x"): + thumbstick_x = float(input_device.get_input_gesture_value("thumbstick", "x")) + if input_device.has_input_gesture("thumbstick", "y"): + thumbstick_y = float(input_device.get_input_gesture_value("thumbstick", "y")) + if input_device.has_input_gesture("trigger", "value"): + trigger = float(input_device.get_input_gesture_value("trigger", "value")) + if input_device.has_input_gesture("squeeze", "value"): + squeeze = float(input_device.get_input_gesture_value("squeeze", "value")) + + # Determine which button pair exists on this device + if input_device.has_input_gesture("x", "click") or input_device.has_input_gesture("y", "click"): + if input_device.has_input_gesture("x", "click"): + button_0 = float(input_device.get_input_gesture_value("x", "click")) + if input_device.has_input_gesture("y", "click"): + button_1 = float(input_device.get_input_gesture_value("y", "click")) + else: + if input_device.has_input_gesture("a", "click"): + button_0 = float(input_device.get_input_gesture_value("a", "click")) + if input_device.has_input_gesture("b", "click"): + button_1 = float(input_device.get_input_gesture_value("b", "click")) + + pose_row = [ + position[0], + position[1], + position[2], + quat.GetReal(), + quat.GetImaginary()[0], + quat.GetImaginary()[1], + quat.GetImaginary()[2], + ] + + input_row = [ + thumbstick_x, + thumbstick_y, + trigger, + squeeze, + button_0, + button_1, + 0.0, + ] + + return np.array([pose_row, input_row], dtype=np.float32) + + 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"]() + self.reset() + + +@dataclass +class OpenXRDeviceCfg(DeviceCfg): + """Configuration for OpenXR devices.""" + + xr_cfg: XrCfg | None = None + class_type: type[DeviceBase] = OpenXRDevice diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py new file mode 100644 index 000000000000..94ef9c0e4e5f --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py @@ -0,0 +1,28 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +"""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.g1_motion_controller_locomotion import ( + G1LowerBodyStandingMotionControllerRetargeter, + G1LowerBodyStandingMotionControllerRetargeterCfg, +) +from .humanoid.unitree.inspire.g1_upper_body_retargeter import UnitreeG1Retargeter, UnitreeG1RetargeterCfg +from .humanoid.unitree.trihand.g1_upper_body_motion_ctrl_gripper import ( + G1TriHandUpperBodyMotionControllerGripperRetargeter, + G1TriHandUpperBodyMotionControllerGripperRetargeterCfg, +) +from .humanoid.unitree.trihand.g1_upper_body_motion_ctrl_retargeter import ( + G1TriHandUpperBodyMotionControllerRetargeter, + G1TriHandUpperBodyMotionControllerRetargeterCfg, +) +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 new file mode 100644 index 000000000000..1e203d11e7e8 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml @@ -0,0 +1,30 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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.2 + 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/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 new file mode 100644 index 000000000000..f67041bd9b60 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml @@ -0,0 +1,29 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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.2 + 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/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py new file mode 100644 index 000000000000..aaeb9bda0314 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py @@ -0,0 +1,262 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import logging +import os + +import numpy as np +import torch +import yaml +from dex_retargeting.retargeting_config import RetargetingConfig +from scipy.spatial.transform import Rotation as R + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +# import logger +logger = logging.getLogger(__name__) + +# 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 + + logger.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 + logger.info(f"Updated URDF path in {yaml_path} to {urdf_path}") + else: + logger.warning(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: + logger.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/fourier/gr1t2_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py new file mode 100644 index 000000000000..0f95d4b9d758 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py @@ -0,0 +1,168 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import contextlib +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +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, + cfg: GR1T2RetargeterCfg, + ): + """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 + """ + + super().__init__(cfg) + self._hand_joint_names = cfg.hand_joint_names + self._hands_controller = GR1TR2DexRetargeting(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[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.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(left_wrist, dtype=torch.float32, device=self._sim_device) + right_wrist_tensor = torch.tensor(self._retarget_abs(right_wrist), 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 get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + 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]) + + +@dataclass +class GR1T2RetargeterCfg(RetargeterCfg): + """Configuration for the GR1T2 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 + retargeter_type: type[RetargeterBase] = GR1T2Retargeter 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 000000000000..1692b4a86d9b --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import torch + +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +class G1LowerBodyStandingRetargeter(RetargeterBase): + """Provides lower body standing commands for the G1 robot.""" + + def __init__(self, cfg: G1LowerBodyStandingRetargeterCfg): + """Initialize the retargeter.""" + super().__init__(cfg) + 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) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + # This retargeter does not consume any device data + return [] + + +@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.""" + retargeter_type: type[RetargeterBase] = G1LowerBodyStandingRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_motion_controller_locomotion.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_motion_controller_locomotion.py new file mode 100644 index 000000000000..8acfe0abc027 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_motion_controller_locomotion.py @@ -0,0 +1,87 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import torch + +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.sim import SimulationContext + + +class G1LowerBodyStandingMotionControllerRetargeter(RetargeterBase): + """Provides lower body standing commands for the G1 robot.""" + + def __init__(self, cfg: G1LowerBodyStandingMotionControllerRetargeterCfg): + """Initialize the retargeter.""" + super().__init__(cfg) + self.cfg = cfg + self._hip_height = cfg.hip_height + + def retarget(self, data: dict) -> torch.Tensor: + left_thumbstick_x = 0.0 + left_thumbstick_y = 0.0 + right_thumbstick_x = 0.0 + right_thumbstick_y = 0.0 + + # Get controller data using enums + if ( + DeviceBase.TrackingTarget.CONTROLLER_LEFT in data + and data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] is not None + ): + left_controller_data = data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] + if len(left_controller_data) > DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + left_inputs = left_controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + if len(left_inputs) > DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value: + left_thumbstick_x = left_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_X.value] + left_thumbstick_y = left_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value] + + if ( + DeviceBase.TrackingTarget.CONTROLLER_RIGHT in data + and data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] is not None + ): + right_controller_data = data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] + if len(right_controller_data) > DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + right_inputs = right_controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + if len(right_inputs) > DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value: + right_thumbstick_x = right_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_X.value] + right_thumbstick_y = right_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value] + + # Thumbstick values are in the range of [-1, 1], so we need to scale them to the range of + # [-movement_scale, movement_scale] + left_thumbstick_x = left_thumbstick_x * self.cfg.movement_scale + left_thumbstick_y = left_thumbstick_y * self.cfg.movement_scale + + # Use rendering time step for deterministic hip height adjustment regardless of wall clock time. + dt = SimulationContext.instance().get_rendering_dt() + self._hip_height -= right_thumbstick_y * dt * self.cfg.rotation_scale + self._hip_height = max(0.4, min(1.0, self._hip_height)) + + return torch.tensor( + [-left_thumbstick_y, -left_thumbstick_x, -right_thumbstick_x, self._hip_height], + device=self.cfg.sim_device, + dtype=torch.float32, + ) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + +@dataclass +class G1LowerBodyStandingMotionControllerRetargeterCfg(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.""" + + movement_scale: float = 0.5 + """Scale the movement of the robot to the range of [-movement_scale, movement_scale].""" + + rotation_scale: float = 0.35 + """Scale the rotation of the robot to the range of [-rotation_scale, rotation_scale].""" + retargeter_type: type[RetargeterBase] = G1LowerBodyStandingMotionControllerRetargeter 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 000000000000..de72352738fd --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..5d0406da4365 --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..3d759003f854 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_dex_retargeting_utils.py @@ -0,0 +1,266 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import logging +import os + +import numpy as np +import torch +import yaml +from dex_retargeting.retargeting_config import RetargetingConfig +from scipy.spatial.transform import Rotation as R + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +# import logger +logger = logging.getLogger(__name__) + +# 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", # noqa: E501 + right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/G1_inspire_assets/retarget_inspire_white_right_hand.urdf", # noqa: E501 + ): + """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 + + logger.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 + logger.info(f"Updated URDF path in {yaml_path} to {urdf_path}") + else: + logger.warning(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: + logger.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 000000000000..17c73dc7ea40 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py @@ -0,0 +1,164 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import contextlib +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +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 + + +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 + """ + + super().__init__(cfg) + 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[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.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 get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + 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()]) + + +@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 + retargeter_type: type[RetargeterBase] = UnitreeG1Retargeter 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 000000000000..0f9f5416e1ca --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..3908adcce0f6 --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..6575eaaba41c --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_dex_retargeting_utils.py @@ -0,0 +1,258 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import logging +import os + +import numpy as np +import torch +import yaml +from dex_retargeting.retargeting_config import RetargetingConfig +from scipy.spatial.transform import Rotation as R + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +# import logger +logger = logging.getLogger(__name__) + +# 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", # noqa: E501 + right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_dexpilot_asset/G1_right_hand.urdf", # noqa: E501 + ): + """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 + + logger.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 + logger.info(f"Updated URDF path in {yaml_path} to {urdf_path}") + else: + logger.warning(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: + logger.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_motion_ctrl_gripper.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_gripper.py new file mode 100644 index 000000000000..c22f40a283f3 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_gripper.py @@ -0,0 +1,154 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +class G1TriHandUpperBodyMotionControllerGripperRetargeter(RetargeterBase): + """Retargeter for G1 gripper that outputs a boolean state based on controller trigger input, + concatenated with the retargeted wrist pose. + + Gripper: + - Uses hysteresis to prevent flickering when the trigger is near the threshold. + - Output is 0.0 for open, 1.0 for close. + + Wrist: + - Retargets absolute pose from controller to robot frame. + - Applies a fixed offset rotation for comfort/alignment. + """ + + def __init__(self, cfg: G1TriHandUpperBodyMotionControllerGripperRetargeterCfg): + """Initialize the retargeter. + + Args: + cfg: Configuration for the retargeter. + """ + super().__init__(cfg) + self._cfg = cfg + # Track previous state for hysteresis (left, right) + self._prev_left_state: float = 0.0 + self._prev_right_state: float = 0.0 + + def retarget(self, data: dict) -> torch.Tensor: + """Retarget controller inputs to gripper boolean state and wrist pose. + + Args: + data: Dictionary with MotionControllerTrackingTarget.LEFT/RIGHT keys + Each value is a 2D array: [pose(7), inputs(7)] + + Returns: + Tensor: [left_gripper_state(1), right_gripper_state(1), left_wrist(7), right_wrist(7)] + Wrist format: [x, y, z, qw, qx, qy, qz] + """ + # Get controller data + left_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_LEFT, np.array([])) + right_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_RIGHT, np.array([])) + + # --- Gripper Logic --- + # Extract hand state from controller data with hysteresis + left_hand_state: float = self._extract_hand_state(left_controller_data, self._prev_left_state) + right_hand_state: float = self._extract_hand_state(right_controller_data, self._prev_right_state) + + # Update previous states + self._prev_left_state = left_hand_state + self._prev_right_state = right_hand_state + + gripper_tensor = torch.tensor([left_hand_state, right_hand_state], dtype=torch.float32, device=self._sim_device) + + # --- Wrist Logic --- + # Default wrist poses (position + quaternion [w, x, y, z] as per default_wrist init) + # Note: default_wrist is [x, y, z, w, x, y, z] in reference, but seemingly used as [x,y,z, w,x,y,z] + default_wrist = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + + # Extract poses from controller data + left_wrist = self._extract_wrist_pose(left_controller_data, default_wrist) + right_wrist = self._extract_wrist_pose(right_controller_data, default_wrist) + + # Convert to tensors + left_wrist_tensor = torch.tensor(self._retarget_abs(left_wrist), dtype=torch.float32, device=self._sim_device) + right_wrist_tensor = torch.tensor(self._retarget_abs(right_wrist), dtype=torch.float32, device=self._sim_device) + + # Concatenate: [gripper(2), left_wrist(7), right_wrist(7)] + return torch.cat([gripper_tensor, left_wrist_tensor, right_wrist_tensor]) + + def _extract_hand_state(self, controller_data: np.ndarray, prev_state: float) -> float: + """Extract hand state from controller data with hysteresis. + + Args: + controller_data: 2D array [pose(7), inputs(7)] + prev_state: Previous hand state (0.0 or 1.0) + + Returns: + Hand state as float (0.0 for open, 1.0 for close) + """ + if len(controller_data) <= DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + return 0.0 + + # Extract inputs from second row + inputs = controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + if len(inputs) < len(DeviceBase.MotionControllerInputIndex): + return 0.0 + + # Extract specific inputs using enum + trigger = inputs[DeviceBase.MotionControllerInputIndex.TRIGGER.value] # 0.0 to 1.0 (analog) + + # Apply hysteresis + if prev_state < 0.5: # Currently open + return 1.0 if trigger > self._cfg.threshold_high else 0.0 + else: # Currently closed + return 0.0 if trigger < self._cfg.threshold_low else 1.0 + + def _extract_wrist_pose(self, controller_data: np.ndarray, default_pose: np.ndarray) -> np.ndarray: + """Extract wrist pose from controller data. + + Args: + controller_data: 2D array [pose(7), inputs(7)] + default_pose: Default pose to use if no data + + Returns: + Wrist pose array [x, y, z, w, x, y, z] + """ + if len(controller_data) > DeviceBase.MotionControllerDataRowIndex.POSE.value: + return controller_data[DeviceBase.MotionControllerDataRowIndex.POSE.value] + return default_pose + + def _retarget_abs(self, wrist: np.ndarray) -> np.ndarray: + """Handle absolute pose retargeting for controller wrists.""" + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + # Combined -75ยฐ (rather than -90ยฐ for wrist comfort) Y rotation + 90ยฐ Z rotation + # This is equivalent to (0, -75, 90) in euler angles + combined_quat = torch.tensor([0.5358, -0.4619, 0.5358, 0.4619], 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()]) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + +@dataclass +class G1TriHandUpperBodyMotionControllerGripperRetargeterCfg(RetargeterCfg): + """Configuration for the G1 boolean gripper and wrist retargeter.""" + + threshold_high: float = 0.6 # Threshold to close hand + threshold_low: float = 0.4 # Threshold to open hand + retargeter_type: type[RetargeterBase] = G1TriHandUpperBodyMotionControllerGripperRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_retargeter.py new file mode 100644 index 000000000000..0138bdf6d6b9 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_retargeter.py @@ -0,0 +1,230 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + + +class G1TriHandUpperBodyMotionControllerRetargeter(RetargeterBase): + """Simple retargeter that maps motion controller inputs to G1 hand joints. + + Mapping: + - A button (digital 0/1) โ†’ Thumb joints + - Trigger (analog 0-1) โ†’ Index finger joints + - Squeeze (analog 0-1) โ†’ Middle finger joints + """ + + def __init__(self, cfg: G1TriHandUpperBodyMotionControllerRetargeterCfg): + """Initialize the retargeter.""" + super().__init__(cfg) + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + self._enable_visualization = cfg.enable_visualization + + if cfg.hand_joint_names is None: + raise ValueError("hand_joint_names must be provided") + + # Initialize visualization if enabled + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_controller_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + """Convert controller inputs to robot commands. + + Args: + data: Dictionary with MotionControllerTrackingTarget.LEFT/RIGHT keys + Each value is a 2D array: [pose(7), inputs(7)] + + Returns: + Tensor: [left_wrist(7), right_wrist(7), hand_joints(14)] + hand_joints order: + [ + left_proximal(3), right_proximal(3), left_distal(2), left_thumb_middle(1), + right_distal(2), right_thumb_middle(1), left_thumb_tip(1), right_thumb_tip(1) + ] + """ + + # Get controller data + left_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_LEFT, np.array([])) + right_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_RIGHT, np.array([])) + + # Default wrist poses (position + quaternion) + default_wrist = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + + # Extract poses from controller data + left_wrist = self._extract_wrist_pose(left_controller_data, default_wrist) + right_wrist = self._extract_wrist_pose(right_controller_data, default_wrist) + + # Map controller inputs to hand joints + left_hand_joints = self._map_to_hand_joints(left_controller_data, is_left=True) + right_hand_joints = self._map_to_hand_joints(right_controller_data, is_left=False) + + # Negate left hand joints for proper mirroring + left_hand_joints = -left_hand_joints + + # Combine joints in the expected order: + # [left_proximal(3), right_proximal(3), left_distal(2), left_thumb_middle(1), + # right_distal(2), right_thumb_middle(1), left_thumb_tip(1), right_thumb_tip(1)] + all_hand_joints = np.array( + [ + left_hand_joints[3], # left_index_proximal + left_hand_joints[5], # left_middle_proximal + left_hand_joints[0], # left_thumb_base + right_hand_joints[3], # right_index_proximal + right_hand_joints[5], # right_middle_proximal + right_hand_joints[0], # right_thumb_base + left_hand_joints[4], # left_index_distal + left_hand_joints[6], # left_middle_distal + left_hand_joints[1], # left_thumb_middle + right_hand_joints[4], # right_index_distal + right_hand_joints[6], # right_middle_distal + right_hand_joints[1], # right_thumb_middle + left_hand_joints[2], # left_thumb_tip + right_hand_joints[2], # right_thumb_tip + ] + ) + + # Convert to tensors + 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(all_hand_joints, dtype=torch.float32, device=self._sim_device) + + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + def _extract_wrist_pose(self, controller_data: np.ndarray, default_pose: np.ndarray) -> np.ndarray: + """Extract wrist pose from controller data. + + Args: + controller_data: 2D array [pose(7), inputs(7)] + default_pose: Default pose to use if no data + + Returns: + Wrist pose array [x, y, z, w, x, y, z] + """ + if len(controller_data) > DeviceBase.MotionControllerDataRowIndex.POSE.value: + return controller_data[DeviceBase.MotionControllerDataRowIndex.POSE.value] + return default_pose + + def _map_to_hand_joints(self, controller_data: np.ndarray, is_left: bool) -> np.ndarray: + """Map controller inputs to hand joint angles. + + Args: + controller_data: 2D array [pose(7), inputs(7)] + is_left: True for left hand, False for right hand + + Returns: + Hand joint angles (7 joints per hand) in radians + """ + + # Initialize all joints to zero + hand_joints = np.zeros(7) + + if len(controller_data) <= DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + return hand_joints + + # Extract inputs from second row + inputs = controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + + if len(inputs) < len(DeviceBase.MotionControllerInputIndex): + return hand_joints + + # Extract specific inputs using enum + trigger = inputs[DeviceBase.MotionControllerInputIndex.TRIGGER.value] # 0.0 to 1.0 (analog) + squeeze = inputs[DeviceBase.MotionControllerInputIndex.SQUEEZE.value] # 0.0 to 1.0 (analog) + + # Grasping logic: + # If trigger is pressed, we grasp with index and thumb. + # If squeeze is pressed, we grasp with middle and thumb. + # If both are pressed, we grasp with index, middle, and thumb. + # The thumb rotates towards the direction of the pressing finger. + # If both are pressed, the thumb stays in the middle. + + thumb_button = max(trigger, squeeze) + + # Map to G1 hand joints (in radians) + # Thumb joints (3 joints) - controlled by A button (digital) + thumb_angle = -thumb_button # Max 1 radian โ‰ˆ 57ยฐ + + # Thumb rotation: + # If trigger is pressed, we rotate the thumb toward the index finger. + # If squeeze is pressed, we rotate the thumb toward the middle finger. + # If both are pressed, the thumb stays between the index and middle fingers. + # Trigger pushes toward +0.5, squeeze pushes toward -0.5 + # trigger=1,squeeze=0 โ†’ 0.5; trigger=0,squeeze=1 โ†’ -0.5; both=1 โ†’ 0 + thumb_rotation = 0.5 * trigger - 0.5 * squeeze + + if not is_left: + thumb_rotation = -thumb_rotation + + # These values were found empirically to get a good gripper pose. + + hand_joints[0] = thumb_rotation # thumb_0_joint (base) + hand_joints[1] = thumb_angle * 0.4 # thumb_1_joint (middle) + hand_joints[2] = thumb_angle * 0.7 # thumb_2_joint (tip) + + # Index finger joints (2 joints) - controlled by trigger (analog) + index_angle = trigger * 1.0 # Max 1.0 radians โ‰ˆ 57ยฐ + hand_joints[3] = index_angle # index_0_joint (proximal) + hand_joints[4] = index_angle # index_1_joint (distal) + + # Middle finger joints (2 joints) - controlled by squeeze (analog) + middle_angle = squeeze * 1.0 # Max 1.0 radians โ‰ˆ 57ยฐ + hand_joints[5] = middle_angle # middle_0_joint (proximal) + hand_joints[6] = middle_angle # middle_1_joint (distal) + + return hand_joints + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + """Handle absolute pose retargeting for controller wrists.""" + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + # Combined -75ยฐ (rather than -90ยฐ for wrist comfort) Y rotation + 90ยฐ Z rotation + # This is equivalent to (0, -75, 90) in euler angles + combined_quat = torch.tensor([0.5358, -0.4619, 0.5358, 0.4619], 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()]) + + +@dataclass +class G1TriHandUpperBodyMotionControllerRetargeterCfg(RetargeterCfg): + """Configuration for the G1 Controller Upper Body retargeter.""" + + enable_visualization: bool = False + hand_joint_names: list[str] | None = None # List of robot hand joint names + retargeter_type: type[RetargeterBase] = G1TriHandUpperBodyMotionControllerRetargeter 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 000000000000..9c8651f43de1 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py @@ -0,0 +1,173 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import contextlib +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +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 + + +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. + """ + super().__init__(cfg) + + # 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[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.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 get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + 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()]) + + +@dataclass +class G1TriHandUpperBodyRetargeterCfg(RetargeterCfg): + """Configuration for the G1 Controller Upper Body 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 + retargeter_type: type[RetargeterBase] = G1TriHandUpperBodyRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/__init__.py new file mode 100644 index 000000000000..426b8ac1002c --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Franka manipulator retargeting module. + +This module provides functionality for retargeting motion to Franka robots. +""" + +from .gripper_retargeter import GripperRetargeter, GripperRetargeterCfg +from .se3_abs_retargeter import Se3AbsRetargeter, Se3AbsRetargeterCfg +from .se3_rel_retargeter import Se3RelRetargeter, Se3RelRetargeterCfg diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py new file mode 100644 index 000000000000..9ae2031b4d81 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + +from dataclasses import dataclass +from typing import Final + +import numpy as np +import torch + +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +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, + cfg: GripperRetargeterCfg, + ): + super().__init__(cfg) + """Initialize the gripper retargeter.""" + # Store the hand to track + if cfg.bound_hand not in [DeviceBase.TrackingTarget.HAND_LEFT, DeviceBase.TrackingTarget.HAND_RIGHT]: + raise ValueError( + "bound_hand must be either DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT" + ) + self.bound_hand = cfg.bound_hand + # Initialize gripper state + self._previous_gripper_command = False + + def retarget(self, data: dict) -> torch.Tensor: + """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: + torch.Tensor: Tensor containing a single bool value 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_bool = self._calculate_gripper_command(thumb_tip[:3], index_tip[:3]) + gripper_value = -1.0 if gripper_command_bool else 1.0 + + return torch.tensor([gripper_value], dtype=torch.float32, device=self._sim_device) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + 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 + + +@dataclass +class GripperRetargeterCfg(RetargeterCfg): + """Configuration for gripper retargeter.""" + + bound_hand: DeviceBase.TrackingTarget = DeviceBase.TrackingTarget.HAND_RIGHT + retargeter_type: type[RetargeterBase] = GripperRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py new file mode 100644 index 000000000000..d69af88cfcce --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py @@ -0,0 +1,172 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch +from scipy.spatial.transform import Rotation, Slerp + +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +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, + cfg: Se3AbsRetargeterCfg, + ): + """Initialize the retargeter. + + Args: + bound_hand: The hand to track (DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.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 + device: The device to place the returned tensor on ('cpu' or 'cuda') + """ + super().__init__(cfg) + if cfg.bound_hand not in [DeviceBase.TrackingTarget.HAND_LEFT, DeviceBase.TrackingTarget.HAND_RIGHT]: + raise ValueError( + "bound_hand must be either DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT" + ) + self.bound_hand = cfg.bound_hand + + self._zero_out_xy_rotation = cfg.zero_out_xy_rotation + self._use_wrist_rotation = cfg.use_wrist_rotation + self._use_wrist_position = cfg.use_wrist_position + + # Initialize visualization if enabled + self._enable_visualization = cfg.enable_visualization + if cfg.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) -> torch.Tensor: + """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: + torch.Tensor: 7D tensor 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_np = self._retarget_abs(thumb_tip, index_tip, wrist) + + # Convert to torch tensor + ee_command = torch.tensor(ee_command_np, dtype=torch.float32, device=self._sim_device) + + return ee_command + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + 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) + + +@dataclass +class Se3AbsRetargeterCfg(RetargeterCfg): + """Configuration for absolute position retargeter.""" + + zero_out_xy_rotation: bool = True + use_wrist_rotation: bool = False + use_wrist_position: bool = True + enable_visualization: bool = False + bound_hand: DeviceBase.TrackingTarget = DeviceBase.TrackingTarget.HAND_RIGHT + retargeter_type: type[RetargeterBase] = Se3AbsRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py new file mode 100644 index 000000000000..360b1c29c347 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py @@ -0,0 +1,216 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch +from scipy.spatial.transform import Rotation + +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +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, + cfg: Se3RelRetargeterCfg, + ): + """Initialize the relative motion retargeter. + + Args: + bound_hand: The hand to track (DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.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 + device: The device to place the returned tensor on ('cpu' or 'cuda') + """ + # Store the hand to track + if cfg.bound_hand not in [DeviceBase.TrackingTarget.HAND_LEFT, DeviceBase.TrackingTarget.HAND_RIGHT]: + raise ValueError( + "bound_hand must be either DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT" + ) + super().__init__(cfg) + self.bound_hand = cfg.bound_hand + + self._zero_out_xy_rotation = cfg.zero_out_xy_rotation + self._use_wrist_rotation = cfg.use_wrist_rotation + self._use_wrist_position = cfg.use_wrist_position + self._delta_pos_scale_factor = cfg.delta_pos_scale_factor + self._delta_rot_scale_factor = cfg.delta_rot_scale_factor + self._alpha_pos = cfg.alpha_pos + self._alpha_rot = cfg.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 = cfg.enable_visualization + if cfg.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) -> torch.Tensor: + """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: + torch.Tensor: 6D tensor 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_np = 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() + + # Convert to torch tensor + ee_command = torch.tensor(ee_command_np, dtype=torch.float32, device=self._sim_device) + + return ee_command + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + 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) + + +@dataclass +class Se3RelRetargeterCfg(RetargeterCfg): + """Configuration for relative position retargeter.""" + + zero_out_xy_rotation: bool = True + 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 + bound_hand: DeviceBase.TrackingTarget = DeviceBase.TrackingTarget.HAND_RIGHT + retargeter_type: type[RetargeterBase] = Se3RelRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/se3_handtracking.py b/source/isaaclab/isaaclab/devices/openxr/se3_handtracking.py deleted file mode 100644 index 18dda1dc5e95..000000000000 --- a/source/isaaclab/isaaclab/devices/openxr/se3_handtracking.py +++ /dev/null @@ -1,298 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""OpenXR handtracking controller for SE(3) control.""" -import contextlib -import numpy as np -from collections.abc import Callable -from scipy.spatial.transform import Rotation, Slerp -from typing import Final - -import carb -from omni.kit.viewport.utility import get_active_viewport - -from ..device_base import DeviceBase - -with contextlib.suppress(ModuleNotFoundError): - from isaacsim.xr.openxr import OpenXR, OpenXRSpec - from omni.kit.xr.core import XRCore - - from . import teleop_command - - -class Se3HandTracking(DeviceBase): - """A OpenXR powered hand tracker for sending SE(3) commands as delta poses - as well as teleop commands via callbacks. - - The command comprises of two parts as well as callbacks for teleop commands: - - * 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. - * - * Teleop commands can be subscribed via the add_callback function, the available keys are: - * - "START": Resumes hand tracking flow - * - "STOP": Stops hand tracking flow - * - "RESET": signals simulation reset - """ - - GRIP_HYSTERESIS_METERS: Final[float] = 0.05 # 2.0 inch - - def __init__( - self, - hand, - abs=True, - zero_out_xy_rotation=False, - use_wrist_rotation=False, - delta_pos_scale_factor=10, - delta_rot_scale_factor=10, - ): - self._openxr = OpenXR() - self._previous_pos = np.zeros(3) - self._previous_rot = np.zeros(3) - self._previous_grip_distance = 0.0 - self._previous_gripper_command = False - self._hand = hand - self._abs = abs - self._zero_out_xy_rotation = zero_out_xy_rotation - self._use_wrist_rotation = use_wrist_rotation - self._additional_callbacks = dict() - self._vc = teleop_command.TeleopCommand() - self._vc_subscription = ( - XRCore.get_singleton() - .get_message_bus() - .create_subscription_to_pop_by_type( - carb.events.type_from_string(teleop_command.TELEOP_COMMAND_RESPONSE_EVENT_TYPE), self._on_teleop_command - ) - ) - self._tracking = False - - self._alpha_pos = 0.5 - self._alpha_rot = 0.5 - self._smoothed_delta_pos = np.zeros(3) - self._smoothed_delta_rot = np.zeros(3) - # Set the XR anchormode to active camera - carb.settings.get_settings().set_string("/xrstage/profile/ar/anchorMode", "active camera") - # Select RTX - RealTime for Renderer - viewport_api = get_active_viewport() - viewport_api.set_hd_engine("rtx", "RaytracedLighting") - self._delta_pos_scale_factor = delta_pos_scale_factor - self._delta_rot_scale_factor = delta_rot_scale_factor - - def __del__(self): - return - - def __str__(self) -> str: - return "" - - """ - Operations - """ - - def reset(self): - self._previous_pos = np.zeros(3) - self._previous_rot = np.zeros(3) - self._previous_grip_distance = 0.0 - self._previous_gripper_command = False - - def add_callback(self, key: str, func: Callable): - 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. - """ - - hand_joints = self._openxr.locate_hand_joints(self._hand) - - if hand_joints is None: - return np.zeros(6), self._previous_gripper_command - - if self._abs: - pose = self._calculate_abs_target_pose(hand_joints) - else: - pose = self._calculate_target_delta_pose(hand_joints) - - return pose, self._calculate_gripper_command(hand_joints) - - """ - Internal helpers. - """ - - def _calculate_abs_target_pose(self, hand_joints): - if not self._tracking: - return np.zeros(6) - - index_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_INDEX_TIP_EXT] - thumb_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_THUMB_TIP_EXT] - - # position: - if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT: - target_pos = self._previous_pos - if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT: - target_pos = self._previous_pos - else: - index_tip_pos = index_tip.pose.position - index_tip_position = np.array([index_tip_pos.x, index_tip_pos.y, index_tip_pos.z], dtype=np.float32) - - thumb_tip_pos = thumb_tip.pose.position - thumb_tip_position = np.array([thumb_tip_pos.x, thumb_tip_pos.y, thumb_tip_pos.z], dtype=np.float32) - - target_pos = (index_tip_position + thumb_tip_position) / 2 - self._previous_pos = target_pos - - # rotation - if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT: - target_rot = self._previous_rot - if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT: - target_rot = self._previous_rot - else: - index_tip_quat = index_tip.pose.orientation - index_tip_quat = np.array( - [index_tip_quat.x, index_tip_quat.y, index_tip_quat.z, index_tip_quat.w], dtype=np.float32 - ) - - thumb_tip_quat = thumb_tip.pose.orientation - thumb_tip_quat = np.array( - [thumb_tip_quat.x, thumb_tip_quat.y, thumb_tip_quat.z, thumb_tip_quat.w], dtype=np.float32 - ) - - r0 = Rotation.from_quat(index_tip_quat) - r1 = Rotation.from_quat(thumb_tip_quat) - key_times = [0, 1] - slerp = Slerp(key_times, Rotation.concatenate([r0, r1])) - interp_time = [0.5] - interp_rotation = slerp(interp_time)[0] - - target_rot = interp_rotation.as_rotvec() - self._previous_rot = target_rot - - return np.concatenate([target_pos, target_rot]) - - def _calculate_target_delta_pose(self, hand_joints): - index_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_INDEX_TIP_EXT] - thumb_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_THUMB_TIP_EXT] - wrist = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_WRIST_EXT] - - # Define thresholds - POSITION_THRESHOLD = 0.001 - ROTATION_THRESHOLD = 0.01 - - # position: - if not wrist.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT: - delta_pos = np.zeros(3) - else: - wrist_pos = wrist.pose.position - target_pos = np.array([wrist_pos.x, wrist_pos.y, wrist_pos.z], dtype=np.float32) - - delta_pos = target_pos - self._previous_pos - self._previous_pos = target_pos - - self._smoothed_delta_pos = (self._alpha_pos * delta_pos) + ((1 - self._alpha_pos) * self._smoothed_delta_pos) - - # Apply position threshold - if np.linalg.norm(self._smoothed_delta_pos) < POSITION_THRESHOLD: - self._smoothed_delta_pos = np.zeros(3) - - # rotation - if self._use_wrist_rotation: - # Rotation using wrist orientation only - if not wrist.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT: - delta_rot = np.zeros(3) - else: - wrist_quat = wrist.pose.orientation - wrist_quat = np.array([wrist_quat.x, wrist_quat.y, wrist_quat.z, wrist_quat.w], dtype=np.float32) - - wrist_rotation = Rotation.from_quat(wrist_quat) - target_rot = wrist_rotation.as_rotvec() - - delta_rot = target_rot - self._previous_rot - self._previous_rot = target_rot - else: - if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT: - delta_rot = np.zeros(3) - if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT: - delta_rot = np.zeros(3) - else: - index_tip_quat = index_tip.pose.orientation - index_tip_quat = np.array( - [index_tip_quat.x, index_tip_quat.y, index_tip_quat.z, index_tip_quat.w], dtype=np.float32 - ) - - thumb_tip_quat = thumb_tip.pose.orientation - thumb_tip_quat = np.array( - [thumb_tip_quat.x, thumb_tip_quat.y, thumb_tip_quat.z, thumb_tip_quat.w], dtype=np.float32 - ) - - r0 = Rotation.from_quat(index_tip_quat) - r1 = Rotation.from_quat(thumb_tip_quat) - key_times = [0, 1] - slerp = Slerp(key_times, Rotation.concatenate([r0, r1])) - interp_time = [0.5] - interp_rotation = slerp(interp_time)[0] - - target_rot = interp_rotation.as_rotvec() - delta_rot = target_rot - self._previous_rot - self._previous_rot = target_rot - - if self._zero_out_xy_rotation: - delta_rot[0] = 0.0 # Zero out rotation around x-axis - delta_rot[1] = 0.0 # Zero out rotation around y-axis - - self._smoothed_delta_rot = (self._alpha_rot * delta_rot) + ((1 - self._alpha_rot) * self._smoothed_delta_rot) - - # Apply rotation threshold - if np.linalg.norm(self._smoothed_delta_rot) < ROTATION_THRESHOLD: - self._smoothed_delta_rot = np.zeros(3) - - delta_pos = self._smoothed_delta_pos - delta_rot = self._smoothed_delta_rot - - # if not tracking still update prev positions but return no delta pose - if not self._tracking: - return np.zeros(6) - - return np.concatenate([delta_pos * self._delta_pos_scale_factor, delta_rot * self._delta_rot_scale_factor]) - - def _calculate_gripper_command(self, hand_joints): - index_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_INDEX_TIP_EXT] - thumb_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_THUMB_TIP_EXT] - - if not self._tracking: - return self._previous_gripper_command - if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT: - return self._previous_gripper_command - if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT: - return self._previous_gripper_command - - index_tip_pos = index_tip.pose.position - index_tip_pos = np.array([index_tip_pos.x, index_tip_pos.y, index_tip_pos.z], dtype=np.float32) - thumb_tip_pos = thumb_tip.pose.position - thumb_tip_pos = np.array([thumb_tip_pos.x, thumb_tip_pos.y, thumb_tip_pos.z], dtype=np.float32) - distance = np.linalg.norm(index_tip_pos - thumb_tip_pos) - if distance > self._previous_grip_distance + self.GRIP_HYSTERESIS_METERS: - self._previous_grip_distance = distance - self._previous_gripper_command = False - elif distance < self._previous_grip_distance - self.GRIP_HYSTERESIS_METERS: - self._previous_grip_distance = distance - self._previous_gripper_command = True - - return self._previous_gripper_command - - def _on_teleop_command(self, event: carb.events.IEvent): - msg = event.payload["message"] - - if teleop_command.Actions.START in msg: - if "START" in self._additional_callbacks: - self._additional_callbacks["START"]() - self._tracking = True - elif teleop_command.Actions.STOP in msg: - if "STOP" in self._additional_callbacks: - self._additional_callbacks["STOP"]() - self._tracking = False - elif teleop_command.Actions.RESET in msg: - if "RESET" in self._additional_callbacks: - self._additional_callbacks["RESET"]() diff --git a/source/isaaclab/isaaclab/devices/openxr/teleop_command.py b/source/isaaclab/isaaclab/devices/openxr/teleop_command.py deleted file mode 100644 index bfad37c60736..000000000000 --- a/source/isaaclab/isaaclab/devices/openxr/teleop_command.py +++ /dev/null @@ -1,57 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -import carb -from omni.kit.xr.core import XRCore - -TELEOP_COMMAND_EVENT_TYPE = "teleop_command" -# The event type for outgoing teleop command response text message. -TELEOP_COMMAND_RESPONSE_EVENT_TYPE = "teleop_command_response" - - -class Actions: - """Class for action options.""" - - START: str = "START" - STOP: str = "STOP" - RESET: str = "RESET" - UNKNOWN: str = "UNKNOWN" - - -class TeleopCommand: - """This class handles the message process with key word.""" - - def __init__(self) -> None: - self._message_bus = XRCore.get_singleton().get_message_bus() - self._incoming_message_event = carb.events.type_from_string(TELEOP_COMMAND_EVENT_TYPE) - self._outgoing_message_event = carb.events.type_from_string(TELEOP_COMMAND_RESPONSE_EVENT_TYPE) - self._subscription = self._message_bus.create_subscription_to_pop_by_type( - self._incoming_message_event, self._on_message - ) - - def _process_message(self, event: carb.events.IEvent): - """Processes the received message using key word.""" - message_in = event.payload["message"] - - if "start" in message_in: - message_out = Actions.START - elif "stop" in message_in: - message_out = Actions.STOP - elif "reset" in message_in: - message_out = Actions.RESET - else: - message_out = Actions.UNKNOWN - - print(f"[VC-Keyword] message_out: {message_out}") - # Send the response back through the message bus. - self._message_bus.push(self._outgoing_message_event, payload={"message": message_out}) - carb.log_info(f"Sent response: {message_out}") - - def _on_message(self, event: carb.events.IEvent): - carb.log_info(f"Received message: {event.payload['message']}") - self._process_message(event) - - def unsubscribe(self): - self._subscription.unsubscribe() diff --git a/source/isaaclab/isaaclab/devices/openxr/xr_anchor_utils.py b/source/isaaclab/isaaclab/devices/openxr/xr_anchor_utils.py new file mode 100644 index 000000000000..195d94c0dc15 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/xr_anchor_utils.py @@ -0,0 +1,176 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# SPDX-License-Identifier: BSD-3-Clause +"""Utilities for synchronizing XR anchor pose with a reference prim and XR config.""" + +from __future__ import annotations + +import contextlib +import logging +import math +from typing import Any + +import numpy as np + +# import logger +logger = logging.getLogger(__name__) + +from isaaclab.sim import SimulationContext +from isaaclab.sim.utils.stage import get_current_stage_id + +from .xr_cfg import XrAnchorRotationMode + +with contextlib.suppress(ModuleNotFoundError): + import usdrt + from pxr import Gf as pxrGf + from usdrt import Rt + + +class XrAnchorSynchronizer: + """Keeps the XR anchor prim aligned with a reference prim according to XR config.""" + + def __init__(self, xr_core: Any, xr_cfg: Any, xr_anchor_headset_path: str): + self._xr_core = xr_core + self._xr_cfg = xr_cfg + self._xr_anchor_headset_path = xr_anchor_headset_path + + self.__anchor_prim_initial_quat = None + self.__anchor_prim_initial_height = None + self.__smoothed_anchor_quat = None + self.__last_anchor_quat = None + self.__anchor_rotation_enabled = True + + # Resolve USD layer identifier of the anchor for updates + try: + from isaacsim.core.utils.stage import get_current_stage + + stage = get_current_stage() + xr_anchor_headset_prim = stage.GetPrimAtPath(self._xr_anchor_headset_path) + prim_stack = xr_anchor_headset_prim.GetPrimStack() if xr_anchor_headset_prim is not None else None + self.__anchor_headset_layer_identifier = prim_stack[0].layer.identifier if prim_stack else None + except Exception: + self.__anchor_headset_layer_identifier = None + + def reset(self): + self.__anchor_prim_initial_quat = None + self.__anchor_prim_initial_height = None + self.__smoothed_anchor_quat = None + self.__last_anchor_quat = None + self.__anchor_rotation_enabled = True + self.sync_headset_to_anchor() + + def toggle_anchor_rotation(self): + self.__anchor_rotation_enabled = not self.__anchor_rotation_enabled + logger.info(f"XR: Toggling anchor rotation: {self.__anchor_rotation_enabled}") + + def sync_headset_to_anchor(self): + """Sync XR anchor pose in USD from reference prim (in Fabric/usdrt).""" + try: + if self._xr_cfg.anchor_prim_path is None: + return + + stage_id = get_current_stage_id() + rt_stage = usdrt.Usd.Stage.Attach(stage_id) + if rt_stage is None: + return + + rt_prim = rt_stage.GetPrimAtPath(self._xr_cfg.anchor_prim_path) + if rt_prim is None: + return + + rt_xformable = Rt.Xformable(rt_prim) + if rt_xformable is None: + return + + world_matrix_attr = rt_xformable.GetFabricHierarchyWorldMatrixAttr() + if world_matrix_attr is None: + return + + rt_matrix = world_matrix_attr.Get() + rt_pos = rt_matrix.ExtractTranslation() + + if self.__anchor_prim_initial_quat is None: + self.__anchor_prim_initial_quat = rt_matrix.ExtractRotationQuat() + + if getattr(self._xr_cfg, "fixed_anchor_height", False): + if self.__anchor_prim_initial_height is None: + self.__anchor_prim_initial_height = rt_pos[2] + rt_pos[2] = self.__anchor_prim_initial_height + + pxr_anchor_pos = pxrGf.Vec3d(*rt_pos) + pxrGf.Vec3d(*self._xr_cfg.anchor_pos) + + w, x, y, z = self._xr_cfg.anchor_rot + pxr_cfg_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) + + pxr_anchor_quat = pxr_cfg_quat + + if self._xr_cfg.anchor_rotation_mode in ( + XrAnchorRotationMode.FOLLOW_PRIM, + XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED, + ): + rt_prim_quat = rt_matrix.ExtractRotationQuat() + rt_delta_quat = rt_prim_quat * self.__anchor_prim_initial_quat.GetInverse() + pxr_delta_quat = pxrGf.Quatd(rt_delta_quat.GetReal(), pxrGf.Vec3d(*rt_delta_quat.GetImaginary())) + + # yaw-only about Z (right-handed, Z-up) + wq = pxr_delta_quat.GetReal() + ix, iy, iz = pxr_delta_quat.GetImaginary() + yaw = math.atan2(2.0 * (wq * iz + ix * iy), 1.0 - 2.0 * (iy * iy + iz * iz)) + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + pxr_delta_yaw_only_quat = pxrGf.Quatd(cy, pxrGf.Vec3d(0.0, 0.0, sy)) + pxr_anchor_quat = pxr_delta_yaw_only_quat * pxr_cfg_quat + + if self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED: + if self.__smoothed_anchor_quat is None: + self.__smoothed_anchor_quat = pxr_anchor_quat + else: + dt = SimulationContext.instance().get_rendering_dt() + alpha = 1.0 - math.exp(-dt / max(self._xr_cfg.anchor_rotation_smoothing_time, 1e-6)) + alpha = min(1.0, max(0.05, alpha)) + self.__smoothed_anchor_quat = pxrGf.Slerp(alpha, self.__smoothed_anchor_quat, pxr_anchor_quat) + pxr_anchor_quat = self.__smoothed_anchor_quat + + elif self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.CUSTOM: + if self._xr_cfg.anchor_rotation_custom_func is not None: + rt_prim_quat = rt_matrix.ExtractRotationQuat() + anchor_prim_pose = np.array( + [ + rt_pos[0], + rt_pos[1], + rt_pos[2], + rt_prim_quat.GetReal(), + rt_prim_quat.GetImaginary()[0], + rt_prim_quat.GetImaginary()[1], + rt_prim_quat.GetImaginary()[2], + ], + dtype=np.float64, + ) + # Previous headpose must be provided by caller; fall back to zeros. + prev_head = getattr(self, "_previous_headpose", np.zeros(7, dtype=np.float64)) + np_array_quat = self._xr_cfg.anchor_rotation_custom_func(prev_head, anchor_prim_pose) + w, x, y, z = np_array_quat + pxr_anchor_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) + + pxr_mat = pxrGf.Matrix4d() + pxr_mat.SetTranslateOnly(pxr_anchor_pos) + + if self.__anchor_rotation_enabled: + pxr_mat.SetRotateOnly(pxr_anchor_quat) + self.__last_anchor_quat = pxr_anchor_quat + else: + if self.__last_anchor_quat is None: + self.__last_anchor_quat = pxr_anchor_quat + + pxr_mat.SetRotateOnly(self.__last_anchor_quat) + self.__smoothed_anchor_quat = self.__last_anchor_quat + + self._xr_core.set_world_transform_matrix( + self._xr_anchor_headset_path, pxr_mat, self.__anchor_headset_layer_identifier + ) + except Exception as e: + logger.warning(f"XR: Anchor sync failed: {e}") diff --git a/source/isaaclab/isaaclab/devices/openxr/xr_cfg.py b/source/isaaclab/isaaclab/devices/openxr/xr_cfg.py new file mode 100644 index 000000000000..1eaee292eaee --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/xr_cfg.py @@ -0,0 +1,151 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + +from __future__ import annotations + +import enum +from collections.abc import Callable + +import numpy as np + +from isaaclab.utils import configclass + + +class XrAnchorRotationMode(enum.Enum): + """Enumeration for XR anchor rotation modes.""" + + FIXED = "fixed" + """Fixed rotation mode: sets rotation once and doesn't change it.""" + + FOLLOW_PRIM = "follow_prim" + """Follow prim rotation mode: rotation follows prim's rotation.""" + + FOLLOW_PRIM_SMOOTHED = "follow_prim_smoothed" + """Follow prim rotation mode with smooth interpolation: rotation smoothly follows prim's rotation using slerp.""" + + CUSTOM = "custom_rotation" + """Custom rotation mode: user provided function to calculate the rotation.""" + + +@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, 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. + """ + + anchor_prim_path: str | None = None + """Specifies the prim path to attach the XR anchor to for dynamic positioning. + + When set, the XR anchor will be attached to the specified prim (e.g., robot root prim), + allowing the XR camera to move with the prim. This is particularly useful for locomotion + robot teleoperation where the robot moves and the XR camera should follow it. + + If None, the anchor will use the static :attr:`anchor_pos` and :attr:`anchor_rot` values. + """ + + anchor_rotation_mode: XrAnchorRotationMode = XrAnchorRotationMode.FIXED + """Specifies how the XR anchor rotation should behave when attached to a prim. + + The available modes are: + - :attr:`XrAnchorRotationMode.FIXED`: Sets rotation once to anchor_rot value + - :attr:`XrAnchorRotationMode.FOLLOW_PRIM`: Rotation follows prim's rotation + - :attr:`XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED`: Rotation smoothly follows prim's rotation using slerp + - :attr:`XrAnchorRotationMode.CUSTOM`: user provided function to calculate the rotation + """ + + anchor_rotation_smoothing_time: float = 1.0 + """Wall-clock time constant (seconds) for rotation smoothing in FOLLOW_PRIM_SMOOTHED mode. + + This time constant is applied using wall-clock delta time between frames (not physics dt). + Smaller values (e.g., 0.1) result in faster/snappier response but less smoothing. + Larger values (e.g., 0.75โ€“2.0) result in slower/smoother response but more lag. + Typical useful range: 0.3 โ€“ 1.5 seconds depending on runtime frame-rate and comfort. + """ + + anchor_rotation_custom_func: Callable[[np.ndarray, np.ndarray], np.ndarray] = lambda headpose, primpose: np.array( + [1, 0, 0, 0], dtype=np.float64 + ) + """Specifies the function to calculate the rotation of the XR anchor when anchor_rotation_mode is CUSTOM. + + Args: + headpose: Previous head pose as numpy array [x, y, z, w, x, y, z] (position + quaternion) + pose: Anchor prim pose as numpy array [x, y, z, w, x, y, z] (position + quaternion) + + Returns: + np.ndarray: Quaternion as numpy array [w, x, y, z] + """ + + 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. + """ + + fixed_anchor_height: bool = True + """Specifies if the anchor height should be fixed. + + If True, the anchor height will be fixed to the initial height of the anchor prim. + """ + + +from typing import Any + + +def remove_camera_configs(env_cfg: Any) -> Any: + """Removes cameras from environments when using XR devices. + + XR does not support additional cameras in the environment as they can cause + rendering conflicts and performance issues. This function scans the environment + configuration for camera objects and removes them, along with any associated + observation terms that reference these cameras. + + Args: + env_cfg: The environment configuration to modify. + + Returns: + The modified environment configuration with cameras removed. + """ + + import logging + + # import logger + logger = logging.getLogger(__name__) + + from isaaclab.managers import SceneEntityCfg + from isaaclab.sensors import CameraCfg + + for attr_name in dir(env_cfg.scene): + attr = getattr(env_cfg.scene, attr_name) + if isinstance(attr, CameraCfg): + delattr(env_cfg.scene, attr_name) + logger.info(f"Removed camera config: {attr_name}") + + # Remove any ObsTerms for the camera + if hasattr(env_cfg.observations, "policy"): + for obs_name in dir(env_cfg.observations.policy): + obsterm = getattr(env_cfg.observations.policy, obs_name) + if hasattr(obsterm, "params") and obsterm.params: + for param_value in obsterm.params.values(): + if isinstance(param_value, SceneEntityCfg) and param_value.name == attr_name: + delattr(env_cfg.observations.policy, attr_name) + logger.info(f"Removed camera observation term: {attr_name}") + break + return env_cfg diff --git a/source/isaaclab/isaaclab/devices/retargeter_base.py b/source/isaaclab/isaaclab/devices/retargeter_base.py new file mode 100644 index 000000000000..fcd443a155b2 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/retargeter_base.py @@ -0,0 +1,69 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from abc import ABC, abstractmethod +from dataclasses import dataclass +from enum import Enum +from typing import Any + + +@dataclass +class RetargeterCfg: + """Base configuration for hand tracking retargeters.""" + + sim_device: str = "cpu" + # Concrete retargeter class to construct for this config. Set by each retargeter module. + retargeter_type: type["RetargeterBase"] | None = None + + +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 + """ + + def __init__(self, cfg: RetargeterCfg): + """Initialize the retargeter. + + Args: + cfg: Configuration for the retargeter + """ + self._sim_device = cfg.sim_device + + class Requirement(Enum): + """Features a retargeter may require from a device's raw data feed.""" + + HAND_TRACKING = "hand_tracking" + HEAD_TRACKING = "head_tracking" + MOTION_CONTROLLER = "motion_controller" + + @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 + + def get_requirements(self) -> list["RetargeterBase.Requirement"]: + """Return the list of required data features for this retargeter. + + Defaults to requesting all available features for backward compatibility. + Implementations should override to narrow to only what they need. + """ + return [ + RetargeterBase.Requirement.HAND_TRACKING, + RetargeterBase.Requirement.HEAD_TRACKING, + RetargeterBase.Requirement.MOTION_CONTROLLER, + ] diff --git a/source/isaaclab/isaaclab/devices/spacemouse/__init__.py b/source/isaaclab/isaaclab/devices/spacemouse/__init__.py index 245b5a50df53..f3cc1c2fd9c4 100644 --- a/source/isaaclab/isaaclab/devices/spacemouse/__init__.py +++ b/source/isaaclab/isaaclab/devices/spacemouse/__init__.py @@ -1,9 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Spacemouse device for SE(2) and SE(3) control.""" -from .se2_spacemouse import Se2SpaceMouse -from .se3_spacemouse import Se3SpaceMouse +from .se2_spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg +from .se3_spacemouse import Se3SpaceMouse, Se3SpaceMouseCfg diff --git a/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py b/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py index f939c60809ff..b0d14b0469f3 100644 --- a/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py +++ b/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py @@ -1,17 +1,24 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Spacemouse controller for SE(2) control.""" -import hid -import numpy as np +from __future__ import annotations + import threading import time from collections.abc import Callable +from dataclasses import dataclass + +import hid +import numpy as np +import torch + +from isaaclab.utils.array import convert_to_torch -from ..device_base import DeviceBase +from ..device_base import DeviceBase, DeviceCfg from .utils import convert_buffer @@ -34,18 +41,17 @@ class Se2SpaceMouse(DeviceBase): """ - def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, omega_z_sensitivity: float = 1.0): + def __init__(self, cfg: Se2SpaceMouseCfg): """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. + cfg: Configuration for the spacemouse device. """ # store inputs - self.v_x_sensitivity = v_x_sensitivity - self.v_y_sensitivity = v_y_sensitivity - self.omega_z_sensitivity = omega_z_sensitivity + self.v_x_sensitivity = cfg.v_x_sensitivity + self.v_y_sensitivity = cfg.v_y_sensitivity + self.omega_z_sensitivity = cfg.omega_z_sensitivity + self._sim_device = cfg.sim_device # acquire device interface self._device = hid.device() self._find_device() @@ -82,19 +88,22 @@ def reset(self): 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. + """Add additional functions to bind spacemouse. + + 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: + def advance(self) -> torch.Tensor: """Provides the result from spacemouse event state. Returns: - A 3D array containing the linear (x,y) and angular velocity (z). + A 3D tensor containing the linear (x,y) and angular velocity (z). """ - return self._base_command + return convert_to_torch(self._base_command, device=self._sim_device) """ Internal helpers. @@ -152,3 +161,13 @@ def _run_device(self): # additional callbacks if "R" in self._additional_callbacks: self._additional_callbacks["R"] + + +@dataclass +class Se2SpaceMouseCfg(DeviceCfg): + """Configuration for SE2 space mouse devices.""" + + v_x_sensitivity: float = 0.8 + v_y_sensitivity: float = 0.4 + omega_z_sensitivity: float = 1.0 + class_type: type[DeviceBase] = Se2SpaceMouse diff --git a/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py b/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py index daf1ccde0dd7..1bc7c00ae567 100644 --- a/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py +++ b/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py @@ -1,18 +1,23 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Spacemouse controller for SE(3) control.""" -import hid -import numpy as np +from __future__ import annotations + import threading import time from collections.abc import Callable +from dataclasses import dataclass + +import hid +import numpy as np +import torch from scipy.spatial.transform import Rotation -from ..device_base import DeviceBase +from ..device_base import DeviceBase, DeviceCfg from .utils import convert_buffer @@ -38,16 +43,17 @@ class Se3SpaceMouse(DeviceBase): """ - def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): + def __init__(self, cfg: Se3SpaceMouseCfg): """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. + cfg: Configuration object for space-mouse settings. """ # store inputs - self.pos_sensitivity = pos_sensitivity - self.rot_sensitivity = rot_sensitivity + self.pos_sensitivity = cfg.pos_sensitivity + self.rot_sensitivity = cfg.rot_sensitivity + self.gripper_term = cfg.gripper_term + self._sim_device = cfg.sim_device # acquire device interface self._device = hid.device() self._find_device() @@ -93,21 +99,30 @@ def reset(self): 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. + """Add additional functions to bind spacemouse. + + 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]: + def advance(self) -> torch.Tensor: """Provides the result from spacemouse event state. Returns: - A tuple containing the delta pose command and gripper commands. + torch.Tensor: A 7-element tensor containing: + - delta pose: First 6 elements as [x, y, z, rx, ry, rz] in meters and radians. + - gripper command: Last element as a binary value (+1.0 for open, -1.0 for close). """ 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 + command = np.concatenate([self._delta_pos, rot_vec]) + if self.gripper_term: + gripper_value = -1.0 if self._close_gripper else 1.0 + command = np.append(command, gripper_value) + + return torch.tensor(command, dtype=torch.float32, device=self._sim_device) """ Internal helpers. @@ -122,6 +137,7 @@ def _find_device(self): if ( device["product_string"] == "SpaceMouse Compact" or device["product_string"] == "SpaceMouse Wireless" + or device["product_string"] == "3Dconnexion Universal Receiver" ): # set found flag found = True @@ -130,6 +146,7 @@ def _find_device(self): # connect to the device self._device.close() self._device.open(vendor_id, product_id) + self._device_name = device["product_string"] # check if device found if not found: time.sleep(1.0) @@ -144,19 +161,32 @@ def _run_device(self): # keep running while True: # read the device data - data = self._device.read(7) + if self._device_name == "3Dconnexion Universal Receiver": + data = self._device.read(7 + 6) + else: + 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 + if self._device_name == "3Dconnexion Universal Receiver": + 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 + + self._delta_rot[1] = self.rot_sensitivity * convert_buffer(data[1 + 6], data[2 + 6]) + self._delta_rot[0] = self.rot_sensitivity * convert_buffer(data[3 + 6], data[4 + 6]) + self._delta_rot[2] = self.rot_sensitivity * convert_buffer(data[5 + 6], data[6 + 6]) * -1.0 + else: + 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: + if data[0] == 3: # press left button if data[1] == 1: # close gripper @@ -173,3 +203,14 @@ def _run_device(self): self._additional_callbacks["R"]() if data[1] == 3: self._read_rotation = not self._read_rotation + + +@dataclass +class Se3SpaceMouseCfg(DeviceCfg): + """Configuration for SE3 space mouse devices.""" + + gripper_term: bool = True + pos_sensitivity: float = 0.4 + rot_sensitivity: float = 0.8 + retargeters: None = None + class_type: type[DeviceBase] = Se3SpaceMouse diff --git a/source/isaaclab/isaaclab/devices/spacemouse/utils.py b/source/isaaclab/isaaclab/devices/spacemouse/utils.py index 62145f3b8395..17510f70bce9 100644 --- a/source/isaaclab/isaaclab/devices/spacemouse/utils.py +++ b/source/isaaclab/isaaclab/devices/spacemouse/utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py new file mode 100644 index 000000000000..f7265c41c2c6 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -0,0 +1,88 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Factory to create teleoperation devices from configuration.""" + +import inspect +import logging +from collections.abc import Callable +from typing import cast + +from isaaclab.devices import DeviceBase, DeviceCfg +from isaaclab.devices.retargeter_base import RetargeterBase + +# import logger +logger = logging.getLogger(__name__) + + +def create_teleop_device( + device_name: str, devices_cfg: dict[str, DeviceCfg], callbacks: dict[str, Callable] | None = None +) -> DeviceBase: + """Create a teleoperation device based on configuration. + + Args: + device_name: The name of the device to create (must exist in devices_cfg) + devices_cfg: Dictionary of device configurations + callbacks: Optional dictionary of callbacks to register with the device + Keys are the button/gesture names, values are callback functions + + Returns: + The configured teleoperation device + + Raises: + ValueError: If the device name is not found in the configuration + ValueError: If the device configuration type is not supported + """ + if device_name not in devices_cfg: + raise ValueError(f"Device '{device_name}' not found in teleop device configurations") + + device_cfg = devices_cfg[device_name] + callbacks = callbacks or {} + + # Determine constructor from the configuration itself + device_constructor = getattr(device_cfg, "class_type", None) + if device_constructor is None: + raise ValueError( + f"Device configuration '{device_name}' does not declare class_type. " + "Set cfg.class_type to the concrete DeviceBase subclass." + ) + if not issubclass(device_constructor, DeviceBase): + raise TypeError(f"class_type for '{device_name}' must be a subclass of DeviceBase; got {device_constructor}") + + # Try to create retargeters if they are configured + retargeters = [] + if hasattr(device_cfg, "retargeters") and device_cfg.retargeters is not None: + try: + # Create retargeters based on configuration using per-config retargeter_type + for retargeter_cfg in device_cfg.retargeters: + retargeter_constructor = getattr(retargeter_cfg, "retargeter_type", None) + if retargeter_constructor is None: + raise ValueError( + f"Retargeter configuration {type(retargeter_cfg).__name__} does not declare retargeter_type. " + "Set cfg.retargeter_type to the concrete RetargeterBase subclass." + ) + if not issubclass(retargeter_constructor, RetargeterBase): + raise TypeError( + f"retargeter_type for {type(retargeter_cfg).__name__} must be a subclass of RetargeterBase; got" + f" {retargeter_constructor}" + ) + retargeters.append(retargeter_constructor(retargeter_cfg)) + + except NameError as e: + raise ValueError(f"Failed to create retargeters: {e}") + + # Build constructor kwargs based on signature + constructor_params = inspect.signature(device_constructor).parameters + params: dict = {"cfg": device_cfg} + if "retargeters" in constructor_params: + params["retargeters"] = retargeters + device = cast(DeviceBase, device_constructor(**params)) + + # Register callbacks + for key, callback in callbacks.items(): + device.add_callback(key, callback) + + logging.info(f"Created teleoperation device: {device_name}") + return device diff --git a/source/isaaclab/isaaclab/envs/__init__.py b/source/isaaclab/isaaclab/envs/__init__.py index dc561911ed9c..543ff2ad4bac 100644 --- a/source/isaaclab/isaaclab/envs/__init__.py +++ b/source/isaaclab/isaaclab/envs/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -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/common.py b/source/isaaclab/isaaclab/envs/common.py index 36ab00f34b27..f913005d1dbb 100644 --- a/source/isaaclab/isaaclab/envs/common.py +++ b/source/isaaclab/isaaclab/envs/common.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +from typing import Dict, Literal, TypeVar # noqa: UP035 + import gymnasium as gym import torch -from typing import Dict, Literal, TypeVar from isaaclab.utils import configclass @@ -44,7 +45,8 @@ class ViewerCfg: * ``"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`. + * ``"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 diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index cd1d620cf11f..206a4e7c01c3 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,33 +6,39 @@ from __future__ import annotations import builtins -import gymnasium as gym import inspect +import logging 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 gymnasium as gym +import numpy as np +import torch + import omni.kit.app -import omni.log -from isaacsim.core.version import get_version +import omni.physx from isaaclab.managers import EventManager from isaaclab.scene import InteractiveScene from isaaclab.sim import SimulationContext +from isaaclab.sim.utils.stage import attach_stage_to_usd_context, use_stage from isaaclab.utils.noise import NoiseModel +from isaaclab.utils.seed import configure_seed from isaaclab.utils.timer import Timer +from isaaclab.utils.version import get_isaac_sim_version 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 +# import logger +logger = logging.getLogger(__name__) + class DirectMARLEnv(gym.Env): """The superclass for the direct workflow to design multi-agent environments. @@ -58,7 +64,6 @@ class DirectMARLEnv(gym.Env): metadata: ClassVar[dict[str, Any]] = { "render_modes": [None, "human", "rgb_array"], - "isaac_sim_version": get_version(), } """Metadata for the environment.""" @@ -87,7 +92,7 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar 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.") + logger.warning("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: @@ -113,12 +118,15 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar 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) + logger.warning(msg) # generate scene with Timer("[INFO]: Time taken for scene creation", "scene_creation"): - self.scene = InteractiveScene(self.cfg.scene) - self._setup_scene() + # set the stage context for scene creation steps which use the stage + with use_stage(self.sim.get_initial_stage()): + self.scene = InteractiveScene(self.cfg.scene) + self._setup_scene() + attach_stage_to_usd_context() print("[INFO]: Scene manager: ", self.scene) # set up camera viewport controller @@ -135,7 +143,6 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # that must happen before the simulation starts. Example: randomizing mesh scale if self.cfg.events: self.event_manager = EventManager(self.cfg.events, self) - print("[INFO] Event Manager: ", self.event_manager) # apply USD-related randomization events if "prestartup" in self.event_manager.available_modes: @@ -147,10 +154,14 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar 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() + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + 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. + # 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 @@ -198,6 +209,9 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # 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") @@ -346,7 +360,8 @@ def step(self, actions: dict[AgentID, ActionType]) -> EnvStepReturn: 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). + A tuple containing the observations, rewards, resets (terminated and truncated) and + extras (keyed by the agent ID). Shape of individual tensors is (num_envs, ...). """ actions = {agent: action.to(self.device) for agent, action in actions.items()} @@ -354,7 +369,7 @@ def step(self, actions: dict[AgentID, ActionType]) -> EnvStepReturn: 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) + actions[agent] = self._action_noise_model[agent](action) # process actions self._pre_physics_step(actions) @@ -407,7 +422,7 @@ def step(self, actions: dict[AgentID, ActionType]) -> EnvStepReturn: 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) + self.obs_dict[agent] = self._observation_noise_model[agent](obs) # return observations, rewards, resets and extras return self.obs_dict, self.reward_dict, self.terminated_dict, self.time_out_dict, self.extras @@ -452,7 +467,7 @@ def seed(seed: int = -1) -> int: except ModuleNotFoundError: pass # set seed for torch and other libraries - return torch_utils.set_seed(seed) + return configure_seed(seed) def render(self, recompute: bool = False) -> np.ndarray | None: """Run rendering without stepping through the physics. @@ -460,7 +475,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: 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 + - **rgb_array**: Return a numpy.ndarray with shape (x, y, 3), representing RGB values for an x-by-y pixel image, suitable for turning into a video. Args: @@ -528,9 +543,18 @@ def close(self): del self.scene if self.viewport_camera_controller is not None: del self.viewport_camera_controller + # clear callbacks and instance + if get_isaac_sim_version().major >= 5: + if self.cfg.sim.create_stage_in_memory: + # detach physx stage + omni.physx.get_physx_simulation_interface().detach_stage() + self.sim.stop() + self.sim.clear() + self.sim.clear_all_callbacks() self.sim.clear_instance() + # destroy the window if self._window is not None: self._window = None @@ -583,17 +607,17 @@ def _configure_env_spaces(self): # 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.") + logger.warning("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( + logger.warning( "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.") + logger.warning("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 diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py b/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py index 0eafc9855aee..66b2bcf998d6 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py @@ -1,10 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from dataclasses import MISSING +from isaaclab.devices.openxr import XrCfg from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim import SimulationCfg from isaaclab.utils import configclass @@ -174,7 +175,8 @@ class DirectMARLEnvCfg: """ 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. + """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. """ @@ -211,7 +213,8 @@ class DirectMARLEnvCfg: """ 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. + """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. """ @@ -221,3 +224,9 @@ class DirectMARLEnvCfg: 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.""" + + 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 b82064621a7d..69be0edb78dd 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,34 +6,41 @@ from __future__ import annotations import builtins -import gymnasium as gym import inspect +import logging import math -import numpy as np -import torch +import warnings 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 gymnasium as gym +import numpy as np +import torch + import omni.kit.app -import omni.log +import omni.physx 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.sim.utils.stage import attach_stage_to_usd_context, use_stage from isaaclab.utils.noise import NoiseModel +from isaaclab.utils.seed import configure_seed from isaaclab.utils.timer import Timer +from isaaclab.utils.version import get_isaac_sim_version 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 +# import logger +logger = logging.getLogger(__name__) + class DirectRLEnv(gym.Env): """The superclass for the direct workflow to design environments. @@ -64,7 +71,6 @@ class DirectRLEnv(gym.Env): """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.""" @@ -93,7 +99,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs 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.") + logger.warning("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: @@ -119,12 +125,15 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs 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) + logger.warning(msg) # generate scene with Timer("[INFO]: Time taken for scene creation", "scene_creation"): - self.scene = InteractiveScene(self.cfg.scene) - self._setup_scene() + # set the stage context for scene creation steps which use the stage + with use_stage(self.sim.get_initial_stage()): + self.scene = InteractiveScene(self.cfg.scene) + self._setup_scene() + attach_stage_to_usd_context() print("[INFO]: Scene manager: ", self.scene) # set up camera viewport controller @@ -141,7 +150,6 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # that must happen before the simulation starts. Example: randomizing mesh scale if self.cfg.events: self.event_manager = EventManager(self.cfg.events, self) - print("[INFO] Event Manager: ", self.event_manager) # apply USD-related randomization events if "prestartup" in self.event_manager.available_modes: @@ -153,10 +161,14 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs 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() + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + 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. + # 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 @@ -202,6 +214,9 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # 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") @@ -209,6 +224,20 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # video matches the simulation self.metadata["render_fps"] = 1 / self.step_dt + # show deprecation message for rerender_on_reset + if self.cfg.rerender_on_reset: + msg = ( + "\033[93m\033[1m[DEPRECATION WARNING] DirectRLEnvCfg.rerender_on_reset is deprecated. Use" + " DirectRLEnvCfg.num_rerenders_on_reset instead.\033[0m" + ) + warnings.warn( + msg, + FutureWarning, + stacklevel=2, + ) + if self.cfg.num_rerenders_on_reset == 0: + self.cfg.num_rerenders_on_reset = 1 + # print the environment information print("[INFO]: Completed setting up the environment...") @@ -290,8 +319,9 @@ def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) 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.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + for _ in range(self.cfg.num_rerenders_on_reset): + self.sim.render() if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): while SimulationManager.assets_loading(): @@ -327,7 +357,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: action = action.to(self.device) # add action noise if self.cfg.action_noise_model: - action = self._action_noise_model.apply(action) + action = self._action_noise_model(action) # process actions self._pre_physics_step(action) @@ -366,12 +396,10 @@ 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() + if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + for _ in range(self.cfg.num_rerenders_on_reset): + self.sim.render() # post-step: step interval event if self.cfg.events: @@ -384,7 +412,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: # 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"]) + self.obs_buf["policy"] = self._observation_noise_model(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 @@ -407,7 +435,7 @@ def seed(seed: int = -1) -> int: except ModuleNotFoundError: pass # set seed for torch and other libraries - return torch_utils.set_seed(seed) + return configure_seed(seed) def render(self, recompute: bool = False) -> np.ndarray | None: """Run rendering without stepping through the physics. @@ -415,7 +443,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: 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 + - **rgb_array**: Return a numpy.ndarray with shape (x, y, 3), representing RGB values for an x-by-y pixel image, suitable for turning into a video. Args: @@ -483,9 +511,18 @@ def close(self): del self.scene if self.viewport_camera_controller is not None: del self.viewport_camera_controller + # clear callbacks and instance + if get_isaac_sim_version().major >= 5: + if self.cfg.sim.create_stage_in_memory: + # detach physx stage + omni.physx.get_physx_simulation_interface().detach_stage() + self.sim.stop() + self.sim.clear() + self.sim.clear_all_callbacks() self.sim.clear_instance() + # destroy the window if self._window is not None: self._window = None @@ -535,17 +572,17 @@ 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.") + logger.warning("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( + logger.warning( "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.") + logger.warning("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 diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py b/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py index a4cb4f3148b9..a1ebb883c65b 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py @@ -1,10 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from dataclasses import MISSING +from isaaclab.devices.openxr import XrCfg from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim import SimulationCfg from isaaclab.utils import configclass @@ -169,7 +170,8 @@ class DirectRLEnvCfg: """ 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. + """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. """ @@ -206,7 +208,8 @@ class DirectRLEnvCfg: """ 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. + """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. """ @@ -221,7 +224,30 @@ class DirectRLEnvCfg: 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. + .. deprecated:: 2.3.1 + This attribute is deprecated and will be removed in the future. Please use + :attr:`num_rerenders_on_reset` instead. + + To get the same behaviour as setting this parameter to ``True`` or ``False``, set + :attr:`num_rerenders_on_reset` to 1 or 0, respectively. + """ + + num_rerenders_on_reset: int = 0 + """Number of render steps to perform after reset. Defaults to 0, which means no render step will be performed + after reset. + + * When this is 0, no render step will be performed after reset. 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 greater than 0, the specified number of extra render steps will be performed to update the + sensor data to reflect the latest states from the reset. This comes at a cost of performance as additional + render steps 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.""" + + 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.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index ef8bfc23fc37..3ff6d291c0a7 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -1,30 +1,40 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import builtins -import torch +import logging +import warnings from collections.abc import Sequence from typing import Any -import isaacsim.core.utils.torch as torch_utils -import omni.log +import torch + +import omni.physx 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.sim.utils.stage import attach_stage_to_usd_context, use_stage from isaaclab.ui.widgets import ManagerLiveVisualizer +from isaaclab.utils.seed import configure_seed from isaaclab.utils.timer import Timer +from isaaclab.utils.version import get_isaac_sim_version from .common import VecEnvObs from .manager_based_env_cfg import ManagerBasedEnvCfg from .ui import ViewportCameraController +from .utils.io_descriptors import export_articulations_data, export_scene_data + +# import logger +logger = logging.getLogger(__name__) class ManagerBasedEnv: - """The base environment encapsulates the simulation scene and the environment managers for the manager-based workflow. + """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 @@ -85,7 +95,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): 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.") + logger.warning("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: @@ -117,14 +127,20 @@ def __init__(self, cfg: ManagerBasedEnvCfg): 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) + logger.warning(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) + # set the stage context for scene creation steps which use the stage + with use_stage(self.sim.get_initial_stage()): + self.scene = InteractiveScene(self.cfg.scene) + attach_stage_to_usd_context() print("[INFO]: Scene manager: ", self.scene) # set up camera viewport controller @@ -140,7 +156,6 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # 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) - print("[INFO] Event Manager: ", self.event_manager) # apply USD-related randomization events if "prestartup" in self.event_manager.available_modes: @@ -152,10 +167,14 @@ def __init__(self, cfg: ManagerBasedEnvCfg): 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() + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + 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. + # 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() @@ -171,12 +190,27 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # if no window, then we don't need to store the window self._window = None - # allocate dictionary to store metrics - self.extras = {} - # initialize observation buffers self.obs_buf = {} + # export IO descriptors if requested + if self.cfg.export_io_descriptors: + self.export_IO_descriptors() + + # show deprecation message for rerender_on_reset + if self.cfg.rerender_on_reset: + msg = ( + "\033[93m\033[1m[DEPRECATION WARNING] ManagerBasedEnvCfg.rerender_on_reset is deprecated. Use" + " ManagerBasedEnvCfg.num_rerenders_on_reset instead.\033[0m" + ) + warnings.warn( + msg, + FutureWarning, + stacklevel=2, + ) + if self.cfg.num_rerenders_on_reset == 0: + self.cfg.num_rerenders_on_reset = 1 + def __del__(self): """Cleanup for the environment.""" self.close() @@ -211,6 +245,48 @@ def device(self): """The device on which the environment is running.""" return self.sim.device + @property + def get_IO_descriptors(self): + """Get the IO descriptors for the environment. + + Returns: + A dictionary with keys as the group names and values as the IO descriptors. + """ + return { + "observations": self.observation_manager.get_IO_descriptors, + "actions": self.action_manager.get_IO_descriptors, + "articulations": export_articulations_data(self), + "scene": export_scene_data(self), + } + + def export_IO_descriptors(self, output_dir: str | None = None): + """Export the IO descriptors for the environment. + + Args: + output_dir: The directory to export the IO descriptors to. + """ + import os + + import yaml + + IO_descriptors = self.get_IO_descriptors + + if output_dir is None: + if self.cfg.log_dir is not None: + output_dir = os.path.join(self.cfg.log_dir, "io_descriptors") + else: + raise ValueError( + "Output directory is not set. Please set the log directory using the `log_dir`" + " configuration or provide an explicit output_dir parameter." + ) + + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + with open(os.path.join(output_dir, "IO_descriptors.yaml"), "w") as f: + print(f"[INFO]: Exporting IO descriptors to {os.path.join(output_dir, 'IO_descriptors.yaml')}") + yaml.safe_dump(IO_descriptors, f) + """ Operations - Setup. """ @@ -232,6 +308,8 @@ def load_managers(self): """ # 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) @@ -297,14 +375,15 @@ def reset( 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.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + for _ in range(self.cfg.num_rerenders_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() + self.obs_buf = self.observation_manager.compute(update_history=True) if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): while SimulationManager.assets_loading(): @@ -357,14 +436,15 @@ def reset_to( 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.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + for _ in range(self.cfg.num_rerenders_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() + self.obs_buf = self.observation_manager.compute(update_history=True) # return observations return self.obs_buf, self.extras @@ -415,7 +495,7 @@ def step(self, action: torch.Tensor) -> tuple[VecEnvObs, dict]: self.event_manager.apply(mode="interval", dt=self.step_dt) # -- compute observations - self.obs_buf = self.observation_manager.compute() + self.obs_buf = self.observation_manager.compute(update_history=True) self.recorder_manager.record_post_step() # return observations and extras @@ -439,7 +519,7 @@ def seed(seed: int = -1) -> int: except ModuleNotFoundError: pass # set seed for torch and other libraries - return torch_utils.set_seed(seed) + return configure_seed(seed) def close(self): """Cleanup for the environment.""" @@ -451,9 +531,18 @@ def close(self): del self.event_manager del self.recorder_manager del self.scene + # clear callbacks and instance + if get_isaac_sim_version().major >= 5: + if self.cfg.sim.create_stage_in_memory: + # detach physx stage + omni.physx.get_physx_simulation_interface().detach_stage() + self.sim.stop() + self.sim.clear() + self.sim.clear_all_callbacks() self.sim.clear_instance() + # destroy the window if self._window is not None: self._window = None diff --git a/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py b/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py index ef3d864fe590..e8f583ddfb31 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,9 +9,11 @@ configuring the environment instances, viewer settings, and simulation parameters. """ -from dataclasses import MISSING +from dataclasses import MISSING, field import isaaclab.envs.mdp as mdp +from isaaclab.devices.device_base import DevicesCfg +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 @@ -113,7 +115,36 @@ class ManagerBasedEnvCfg: 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. + .. deprecated:: 2.3.1 + This attribute is deprecated and will be removed in the future. Please use + :attr:`num_rerenders_on_reset` instead. + + To get the same behaviour as setting this parameter to ``True`` or ``False``, set + :attr:`num_rerenders_on_reset` to 1 or 0, respectively. + """ + + num_rerenders_on_reset: int = 0 + """Number of render steps to perform after reset. Defaults to 0, which means no render step will be + performed after reset. + + * When this is 0, no render step will be performed after reset. 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 greater than 0, the specified number of extra render steps will be performed to update the + sensor data to reflect the latest states from the reset. This comes at a cost of performance as additional + render steps 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.""" + + teleop_devices: DevicesCfg = field(default_factory=DevicesCfg) + """Configuration for teleoperation devices.""" + + export_io_descriptors: bool = False + """Whether to export the IO descriptors for the environment. Defaults to False.""" + + 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 43713ef5ad04..ab8c04155d2d 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,14 +6,13 @@ # 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 +import gymnasium as gym +import numpy as np +import torch from isaaclab.managers import CommandManager, CurriculumManager, RewardManager, TerminationManager from isaaclab.ui.widgets import ManagerLiveVisualizer @@ -57,7 +56,6 @@ class ManagerBasedRLEnv(ManagerBasedEnv, gym.Env): """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.""" @@ -75,15 +73,17 @@ def __init__(self, cfg: ManagerBasedRLEnvCfg, render_mode: str | None = None, ** # -- 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 - # -- init buffers - self.episode_length_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long) - # -- set the framerate of the gym video recorder wrapper so that the playback speed of the produced video matches the simulation + # -- 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...") @@ -187,6 +187,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: self.scene.write_data_to_sim() # simulate self.sim.step(render=False) + self.recorder_manager.record_post_physics_decimation_step() # 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. @@ -218,13 +219,11 @@ 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: - self.sim.render() + if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + for _ in range(self.cfg.num_rerenders_on_reset): + self.sim.render() # trigger recorder terms for post-reset calls self.recorder_manager.record_post_reset(reset_env_ids) @@ -236,7 +235,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: 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() + self.obs_buf = self.observation_manager.compute(update_history=True) # return observations, rewards, resets and extras return self.obs_buf, self.reward_buf, self.reset_terminated, self.reset_time_outs, self.extras @@ -247,7 +246,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: 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 + - **rgb_array**: Return a numpy.ndarray with shape (x, y, 3), representing RGB values for an x-by-y pixel image, suitable for turning into a video. Args: @@ -332,10 +331,13 @@ def _configure_gym_env_spaces(self): 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) - }) + group_term_cfgs = self.observation_manager._group_obs_term_cfgs[group_name] + term_dict = {} + for term_name, term_dim, term_cfg in zip(group_term_names, group_dim, group_term_cfgs): + 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] + term_dict[term_name] = gym.spaces.Box(low=low, high=high, shape=term_dim) + self.single_observation_space[group_name] = gym.spaces.Dict(term_dict) # 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,)) diff --git a/source/isaaclab/isaaclab/envs/manager_based_rl_env_cfg.py b/source/isaaclab/isaaclab/envs/manager_based_rl_env_cfg.py index 6e4d7e2e2998..eac633e8b99c 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/envs/manager_based_rl_mimic_env.py b/source/isaaclab/isaaclab/envs/manager_based_rl_mimic_env.py index 0510e79b7ddc..8518d716332e 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_mimic_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_mimic_env.py @@ -1,15 +1,23 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations -import torch from collections.abc import Sequence +import torch + import isaaclab.utils.math as PoseUtils from isaaclab.envs import ManagerBasedRLEnv +def optional_method(func): + """Decorator to mark a method as optional.""" + func.__is_optional__ = True + return func + + class ManagerBasedRLMimicEnv(ManagerBasedRLEnv): """The superclass for the Isaac Lab Mimic environments. @@ -47,7 +55,11 @@ def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None raise NotImplementedError def target_eef_pose_to_action( - self, target_eef_pose_dict: dict, gripper_action_dict: dict, noise: float | None = None, env_id: int = 0 + 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 @@ -57,7 +69,7 @@ def target_eef_pose_to_action( 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. + 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: @@ -112,6 +124,22 @@ def get_object_poses(self, env_ids: Sequence[int] | None = None): ) return object_pose_matrix + def get_subtask_start_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Gets a dictionary of start signal flags for each subtask in a task. The flag is 1 + when the subtask has started and 0 otherwise. The implementation of this method is + required if intending to enable automatic subtask start signal annotation when running the + dataset annotation tool. This method can be kept unimplemented if intending to use manual + subtask start signal annotation. + + Args: + env_ids: Environment indices to get the start signals for. If None, all envs are considered. + + Returns: + A dictionary start signal flags (False or True) for each subtask. + """ + raise NotImplementedError + 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 @@ -135,3 +163,21 @@ def serialize(self): and used in utils/env_utils.py. """ return dict(env_name=self.spec.id, type=2, env_kwargs=dict()) + + @optional_method + def get_navigation_state(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Optional method. Only required when using navigation controller locomanipulation data generation. + + Gets the navigation state of the robot. Required when use of the navigation controller is + enabled. The navigation state includes a boolean flag "is_navigating" to indicate when the + robot is under control by the navigation controller, and a boolean flag "navigation_goal_reached" + to indicate when the navigation goal has been reached. + + Args: + env_ids: The environment index to get the navigation state for. If None, all envs are considered. + + Returns: + A dictionary that of navigation state flags (False or True). + """ + raise NotImplementedError diff --git a/source/isaaclab/isaaclab/envs/mdp/__init__.py b/source/isaaclab/isaaclab/envs/mdp/__init__.py index 535f9d7edba3..bdb507f3eb0a 100644 --- a/source/isaaclab/isaaclab/envs/mdp/__init__.py +++ b/source/isaaclab/isaaclab/envs/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/__init__.py b/source/isaaclab/isaaclab/envs/mdp/actions/__init__.py index e5c8583fed4e..56b3ae25ff4e 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/__init__.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,3 +10,4 @@ from .joint_actions import * from .joint_actions_to_limits import * from .non_holonomic_actions import * +from .surface_gripper_actions import * diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py index 0e14ce256433..bf8748bdf3ad 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,7 +9,14 @@ 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 +from . import ( + binary_joint_actions, + joint_actions, + joint_actions_to_limits, + non_holonomic_actions, + surface_gripper_actions, + task_space_actions, +) ## # Joint actions. @@ -124,6 +131,9 @@ class JointPositionToLimitsActionCfg(ActionTermCfg): This operation is performed after applying the scale factor. """ + preserve_order: bool = False + """Whether to preserve the order of the joint names in the action output. Defaults to False.""" + @configclass class EMAJointPositionToLimitsActionCfg(JointPositionToLimitsActionCfg): @@ -181,6 +191,41 @@ class BinaryJointVelocityActionCfg(BinaryJointActionCfg): class_type: type[ActionTerm] = binary_joint_actions.BinaryJointVelocityAction +@configclass +class AbsBinaryJointPositionActionCfg(ActionTermCfg): + """Configuration for the absolute binary joint position action term. + + This action term is used for robust grasping by converting continuous gripper joint position actions + into binary open/close commands. Unlike directly applying continuous gripper joint position actions, this class + applies a threshold-based decision mechanism to determine whether to + open or close the gripper. + + The action works by: + 1. Taking a continuous input action value + 2. Comparing it against a configurable threshold + 3. Mapping the result to either open or close commands based on the threshold comparison + 4. Applying the corresponding gripper open/close commands + + This approach provides more predictable and stable grasping behavior compared to directly applying + continuous gripper joint position actions. + + See :class:`AbsBinaryJointPositionAction` 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.""" + threshold: float = 0.5 + """The threshold for the binary action. Defaults to 0.5.""" + positive_threshold: bool = True + """Whether to use positive (Open actions > Close actions) threshold. Defaults to True.""" + + class_type: type[ActionTerm] = binary_joint_actions.AbsBinaryJointPositionAction + + ## # Non-holonomic actions. ## @@ -310,3 +355,25 @@ class OffsetCfg: Note: Functional only when ``nullspace_control`` is set to ``"position"`` within the ``OperationalSpaceControllerCfg``. """ + + +## +# Surface Gripper actions. +## + + +@configclass +class SurfaceGripperBinaryActionCfg(ActionTermCfg): + """Configuration for the binary surface gripper action term. + + See :class:`SurfaceGripperBinaryAction` for more details. + """ + + asset_name: str = MISSING + """Name of the surface gripper asset in the scene.""" + open_command: float = -1.0 + """The command value to open the gripper. Defaults to -1.0.""" + close_command: float = 1.0 + """The command value to close the gripper. Defaults to 1.0.""" + + class_type: type[ActionTerm] = surface_gripper_actions.SurfaceGripperBinaryAction diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py index 9693e147a9cc..289045bd37ba 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py @@ -1,15 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch import isaaclab.utils.string as string_utils from isaaclab.assets.articulation import Articulation @@ -17,9 +17,13 @@ if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor from . import actions_cfg +# import logger +logger = logging.getLogger(__name__) + class BinaryJointAction(ActionTerm): """Base class for binary joint actions. @@ -53,7 +57,7 @@ def __init__(self, cfg: actions_cfg.BinaryJointActionCfg, env: ManagerBasedEnv) 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( + logger.info( f"Resolved joint names for the action term {self.__class__.__name__}:" f" {self._joint_names} [{self._joint_ids}]" ) @@ -111,6 +115,15 @@ def raw_actions(self) -> torch.Tensor: def processed_actions(self) -> torch.Tensor: return self._processed_actions + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "JointAction" + self._IO_descriptor.joint_names = self._joint_names + return self._IO_descriptor + """ Operations. """ @@ -154,3 +167,47 @@ class BinaryJointVelocityAction(BinaryJointAction): def apply_actions(self): self._asset.set_joint_velocity_target(self._processed_actions, joint_ids=self._joint_ids) + + +class AbsBinaryJointPositionAction(BinaryJointAction): + """Absolute Binary joint action that sets the binary action into joint position targets. + + This class extends BinaryJointAction to accept absolute position control + for gripper joints. It converts continuous input actions into binary open/close commands + using a configurable threshold mechanism. + + The key difference from the base BinaryJointAction is that this class: + - Receives absolute joint position actions for gripper control + - Implements a threshold-based decision system to determine open/close state + + The action processing works by: + 1. Taking a continuous input action value + 2. Comparing it against the configured threshold value + 3. Based on the threshold comparison and positive_threshold flag, determining + whether to open or close the gripper + 4. Setting the target joint positions to either the open or close configuration + + """ + + cfg: actions_cfg.AbsBinaryJointPositionActionCfg + """The configuration of the action term.""" + + def process_actions(self, actions: torch.Tensor): + # store the raw actions + self._raw_actions[:] = actions + # compute the binary mask + if self.cfg.positive_threshold: + # true: open 0.785, false: close 0.0 + binary_mask = actions > self.cfg.threshold + else: + # true: close 0.0, false: open 0.785 + binary_mask = actions < self.cfg.threshold + # compute the command + self._processed_actions = torch.where(binary_mask, self._open_command, self._close_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 apply_actions(self): + self._asset.set_joint_position_target(self._processed_actions, joint_ids=self._joint_ids) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions.py index 55f3652583fc..7ca7fe66c4b4 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions.py @@ -1,15 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch import isaaclab.utils.string as string_utils from isaaclab.assets.articulation import Articulation @@ -17,9 +17,13 @@ if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor from . import actions_cfg +# import logger +logger = logging.getLogger(__name__) + class JointAction(ActionTerm): r"""Base class for joint actions. @@ -63,7 +67,7 @@ def __init__(self, cfg: actions_cfg.JointActionCfg, env: ManagerBasedEnv) -> Non ) self._num_joints = len(self._joint_ids) # log the resolved joint names for debugging - omni.log.info( + logger.info( f"Resolved joint names for the action term {self.__class__.__name__}:" f" {self._joint_names} [{self._joint_ids}]" ) @@ -123,6 +127,41 @@ def raw_actions(self) -> torch.Tensor: def processed_actions(self) -> torch.Tensor: return self._processed_actions + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the joint action. + It adds the following information to the base descriptor: + - joint_names: The names of the joints. + - scale: The scale of the action term. + - offset: The offset of the action term. + - clip: The clip of the action term. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "JointAction" + self._IO_descriptor.joint_names = self._joint_names + self._IO_descriptor.scale = self._scale + # This seems to be always [4xNum_joints] IDK why. Need to check. + if isinstance(self._offset, torch.Tensor): + self._IO_descriptor.offset = self._offset[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.offset = self._offset + # FIXME: This is not correct. Add list support. + if self.cfg.clip is not None: + if isinstance(self._clip, torch.Tensor): + self._IO_descriptor.clip = self._clip[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.clip = self._clip + else: + self._IO_descriptor.clip = None + return self._IO_descriptor + """ Operations. """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions_to_limits.py b/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions_to_limits.py index 18415445876d..3fc50ef2d712 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions_to_limits.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/joint_actions_to_limits.py @@ -1,15 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch import isaaclab.utils.math as math_utils import isaaclab.utils.string as string_utils @@ -18,9 +18,13 @@ if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor from . import actions_cfg +# import logger +logger = logging.getLogger(__name__) + class JointPositionToLimitsAction(ActionTerm): """Joint position action term that scales the input actions to the joint limits and applies them to the @@ -52,10 +56,12 @@ def __init__(self, cfg: actions_cfg.JointPositionToLimitsActionCfg, env: Manager 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._joint_ids, self._joint_names = self._asset.find_joints( + self.cfg.joint_names, preserve_order=cfg.preserve_order + ) self._num_joints = len(self._joint_ids) # log the resolved joint names for debugging - omni.log.info( + logger.info( f"Resolved joint names for the action term {self.__class__.__name__}:" f" {self._joint_names} [{self._joint_ids}]" ) @@ -74,17 +80,22 @@ def __init__(self, cfg: actions_cfg.JointPositionToLimitsActionCfg, env: Manager 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) + index_list, _, value_list = string_utils.resolve_matching_names_values( + self.cfg.scale, self._joint_names, preserve_order=cfg.preserve_order + ) 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) + index_list, _, value_list = string_utils.resolve_matching_names_values( + self.cfg.clip, self._joint_names, preserve_order=cfg.preserve_order + ) 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.") @@ -105,6 +116,37 @@ def raw_actions(self) -> torch.Tensor: def processed_actions(self) -> torch.Tensor: return self._processed_actions + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the joint position to limits action. + It adds the following information to the base descriptor: + - joint_names: The names of the joints. + - scale: The scale of the action term. + - offset: The offset of the action term. + - clip: The clip of the action term. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "JointAction" + self._IO_descriptor.joint_names = self._joint_names + self._IO_descriptor.scale = self._scale + # This seems to be always [4xNum_joints] IDK why. Need to check. + if isinstance(self._offset, torch.Tensor): + self._IO_descriptor.offset = self._offset[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.offset = self._offset + if self.cfg.clip is not None: + self._IO_descriptor.clip = self._clip + else: + self._IO_descriptor.clip = None + return self._IO_descriptor + """ Operations. """ @@ -149,7 +191,9 @@ class EMAJointPositionToLimitsAction(JointPositionToLimitsAction): .. math:: - \text{applied action} = \alpha \times \text{processed actions} + (1 - \alpha) \times \text{previous applied action} + \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 @@ -195,15 +239,42 @@ def __init__(self, cfg: actions_cfg.EMAJointPositionToLimitsActionCfg, env: Mana # initialize the previous targets self._prev_applied_actions = torch.zeros_like(self.processed_actions) + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the EMA joint position to limits action. + It adds the following information to the base descriptor: + - joint_names: The names of the joints. + - scale: The scale of the action term. + - offset: The offset of the action term. + - clip: The clip of the action term. + - alpha: The moving average weight. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + if isinstance(self._alpha, float): + self._IO_descriptor.alpha = self._alpha + elif isinstance(self._alpha, torch.Tensor): + self._IO_descriptor.alpha = self._alpha[0].detach().cpu().numpy().tolist() + else: + raise ValueError( + f"Unsupported moving average weight type: {type(self._alpha)}. Supported types are float and" + " torch.Tensor." + ) + return self._IO_descriptor + 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) + super().reset(slice(None)) + self._prev_applied_actions[:] = self._asset.data.joint_pos[:, self._joint_ids] 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] + super().reset(env_ids) + curr_applied_actions = self._asset.data.joint_pos[env_ids[:, None], self._joint_ids].view(len(env_ids), -1) + self._prev_applied_actions[env_ids, :] = curr_applied_actions def process_actions(self, actions: torch.Tensor): # apply affine transformations diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py index 0c3af3c5f463..0a6c65f91022 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py @@ -1,15 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch import isaaclab.utils.string as string_utils from isaaclab.assets.articulation import Articulation @@ -18,9 +18,13 @@ if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor from . import actions_cfg +# import logger +logger = logging.getLogger(__name__) + class NonHolonomicAction(ActionTerm): r"""Non-holonomic action that maps a two dimensional action to the velocity of the robot in @@ -91,11 +95,11 @@ def __init__(self, cfg: actions_cfg.NonHolonomicActionCfg, env: ManagerBasedEnv) 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( + logger.info( f"Resolved joint names for the action term {self.__class__.__name__}:" f" {self._joint_names} [{self._joint_ids}]" ) - omni.log.info( + logger.info( f"Resolved body name for the action term {self.__class__.__name__}: {self._body_name} [{self._body_idx}]" ) @@ -134,6 +138,36 @@ def raw_actions(self) -> torch.Tensor: def processed_actions(self) -> torch.Tensor: return self._processed_actions + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the non-holonomic action. + It adds the following information to the base descriptor: + - scale: The scale of the action term. + - offset: The offset of the action term. + - clip: The clip of the action term. + - body_name: The name of the body. + - x_joint_name: The name of the x joint. + - y_joint_name: The name of the y joint. + - yaw_joint_name: The name of the yaw joint. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "non holonomic actions" + self._IO_descriptor.scale = self._scale + self._IO_descriptor.offset = self._offset + self._IO_descriptor.clip = self._clip + self._IO_descriptor.body_name = self._body_name + self._IO_descriptor.x_joint_name = self._joint_names[0] + self._IO_descriptor.y_joint_name = self._joint_names[1] + self._IO_descriptor.yaw_joint_name = self._joint_names[2] + return self._IO_descriptor + """ Operations. """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py new file mode 100644 index 000000000000..a82433be84cc --- /dev/null +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.controllers.pink_ik 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.""" + + 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. + + This dictionary should map the task names (e.g., 'left_wrist', 'right_wrist') to the + corresponding link names in the URDF that will be controlled by the IK solver. + """ 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 new file mode 100644 index 000000000000..8ae90af3a4a2 --- /dev/null +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -0,0 +1,366 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch +from pink.tasks import FrameTask + +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: + from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor + + 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) + + 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._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) + + # 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(self._env.num_envs): + self._ik_controllers.append( + PinkIKController( + cfg=self.cfg.controller.copy(), + robot_cfg=self._env.scene.cfg.robot, + device=self.device, + controlled_joint_indices=self._isaaclab_controlled_joint_ids, + ) + ) + + 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) + + # 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) + + # 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) + + # 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: + """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).""" + # Count only FrameTask instances in variable_input_tasks + frame_tasks_count = sum( + 1 for task in self._ik_controllers[0].cfg.variable_input_tasks if isinstance(task, FrameTask) + ) + return frame_tasks_count * 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 + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the pink inverse kinematics action. + It adds the following information to the base descriptor: + - scale: The scale of the action term. + - offset: The offset of the action term. + - clip: The clip of the action term. + - pink_controller_joint_names: The names of the pink controller joints. + - hand_joint_names: The names of the hand joints. + - controller_cfg: The configuration of the pink controller. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + 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._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 + + # """ + # Operations. + # """ + + 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 raw actions + self._raw_actions[:] = actions + + # Extract hand joint positions directly (no cloning needed) + self._target_hand_joint_positions = actions[:, -self.hand_joint_dim :] + + # 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], + ) + + # 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) + ) + + 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 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 + 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() + + task.set_target(target) + + # ==================== Action Application ==================== + + def apply_actions(self) -> None: + """Apply the computed joint positions based on the inverse kinematics solution.""" + # 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): + # 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) + + return torch.stack(ik_solutions) + + # ==================== Reset ==================== + + 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/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_actions_cfg.py new file mode 100644 index 000000000000..dfd7a72dcb73 --- /dev/null +++ b/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_actions_cfg.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from dataclasses import MISSING + +from isaaclab.controllers.rmp_flow import RmpFlowControllerCfg +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + +from . import rmpflow_task_space_actions + + +@configclass +class RMPFlowActionCfg(ActionTermCfg): + @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) + + class_type: type[ActionTerm] = rmpflow_task_space_actions.RMPFlowAction + + 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 + + controller: RmpFlowControllerCfg = MISSING + + articulation_prim_expr: str = MISSING # The expression to find the articulation prim paths. + """The configuration for the RMPFlow controller.""" + + use_relative_mode: bool = False + """ + 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. + """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py new file mode 100644 index 000000000000..83e7bd3e3654 --- /dev/null +++ b/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py @@ -0,0 +1,218 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Articulation +from isaaclab.controllers.rmp_flow import RmpFlowController +from isaaclab.managers.action_manager import ActionTerm + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import rmpflow_actions_cfg + +# import logger +logger = logging.getLogger(__name__) + + +class RMPFlowAction(ActionTerm): + """RMPFlow task space action term.""" + + cfg: rmpflow_actions_cfg.RMPFlowActionCfg + """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: rmpflow_actions_cfg.RMPFlowActionCfg, 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 + logger.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + logger.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._rmpflow_controller = RmpFlowController(cfg=self.cfg.controller, 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: + if self.cfg.use_relative_mode: + return 6 # delta_eef_xyz, delta_eef_rpy + else: + return 7 # absolute_eef_xyz, absolute_eef_quat + # self._rmpflow_controller.num_actions = 7 since it use quaternions (w,x,y,z) as command + + @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. + """ + + # This is called each env.step() + 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] + ) + # If use_relative_mode is True, then the controller will apply delta change to the current ee_pose. + if self.cfg.use_relative_mode: + # obtain quantities from simulation + ee_pos_curr, ee_quat_curr = self._compute_frame_pose() + + # compute ee_pose_targets use_relative_actions + if ee_pos_curr is None or ee_quat_curr 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 = math_utils.apply_delta_pose( + ee_pos_curr, ee_quat_curr, self._processed_actions + ) + else: # If use_relative_mode is False, then the controller will apply absolute ee_pose. + self.ee_pos_des = self._processed_actions[:, 0:3] + self.ee_quat_des = self._processed_actions[:, 3:7] + + self.ee_pose_des = torch.cat([self.ee_pos_des, self.ee_quat_des], dim=1) # shape: [n, 7] + + # set command into controller + self._rmpflow_controller.set_command(self.ee_pose_des) + + # This is called each simulationcontext.step(), step *decimation* times when env.step() update actions + 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: + joint_pos_des, joint_vel_des = self._rmpflow_controller.compute() + else: + joint_pos_des = joint_pos.clone() + # set the joint position command + self._asset.set_joint_position_target(joint_pos_des, self._joint_ids) + self._asset.set_joint_velocity_target(joint_vel_des, self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + self._raw_actions[env_ids] = 0.0 + self._rmpflow_controller.initialize(self.cfg.articulation_prim_expr) + + """ + 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 diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/surface_gripper_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/surface_gripper_actions.py new file mode 100644 index 000000000000..f16d1403853b --- /dev/null +++ b/source/isaaclab/isaaclab/envs/mdp/actions/surface_gripper_actions.py @@ -0,0 +1,109 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets.surface_gripper import SurfaceGripper +from isaaclab.managers.action_manager import ActionTerm + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import actions_cfg + +# import logger +logger = logging.getLogger(__name__) + + +class SurfaceGripperBinaryAction(ActionTerm): + """Surface gripper binary action. + + This action term maps a binary action to the *open* or *close* surface gripper configurations. + The surface gripper behavior is as follows: + - [-1, -0.3] --> Gripper is Opening + - [-0.3, 0.3] --> Gripper is Idle (do nothing) + - [0.3, 1] --> Gripper is Closing + + 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 is specifically designed for surface grippers, which use a different + interface than joint-based grippers. + """ + + cfg: actions_cfg.SurfaceGripperBinaryActionCfg + """The configuration of the action term.""" + _asset: SurfaceGripper + """The surface gripper asset on which the action term is applied.""" + + def __init__(self, cfg: actions_cfg.SurfaceGripperBinaryActionCfg, env: ManagerBasedEnv) -> None: + # initialize the action term + super().__init__(cfg, env) + + # log the resolved asset name for debugging + logger.info( + f"Resolved surface gripper asset for the action term {self.__class__.__name__}: {self.cfg.asset_name}" + ) + + # 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, 1, device=self.device) + + # parse open command + self._open_command = torch.tensor(self.cfg.open_command, device=self.device) + # parse close command + self._close_command = torch.tensor(self.cfg.close_command, device=self.device) + + """ + 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) + + def apply_actions(self): + """Apply the processed actions to the surface gripper.""" + self._asset.set_grippers_command(self._processed_actions.view(-1)) + self._asset.write_data_to_sim() + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + if env_ids is None: + self._raw_actions[:] = 0.0 + else: + self._raw_actions[env_ids] = 0.0 diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index ae6fd324752d..47f9cec23490 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -1,15 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch + from pxr import UsdPhysics import isaaclab.utils.math as math_utils @@ -23,9 +24,13 @@ if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor from . import actions_cfg +# import logger +logger = logging.getLogger(__name__) + class DifferentialInverseKinematicsAction(ActionTerm): r"""Inverse Kinematics action term. @@ -77,11 +82,11 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env: self._jacobi_joint_ids = [i + 6 for i in self._joint_ids] # log info for debugging - omni.log.info( + logger.info( f"Resolved joint names for the action term {self.__class__.__name__}:" f" {self._joint_names} [{self._joint_ids}]" ) - omni.log.info( + logger.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 @@ -148,6 +153,37 @@ def jacobian_b(self) -> torch.Tensor: jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) return jacobian + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the pink inverse kinematics action. + It adds the following information to the base descriptor: + - body_name: The name of the body. + - joint_names: The names of the joints. + - scale: The scale of the action term. + - clip: The clip of the action term. + - controller_cfg: The configuration of the controller. + - body_offset: The offset of the body. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "TaskSpaceAction" + self._IO_descriptor.body_name = self._body_name + self._IO_descriptor.joint_names = self._joint_names + self._IO_descriptor.scale = self._scale + if self.cfg.clip is not None: + self._IO_descriptor.clip = self.cfg.clip + else: + self._IO_descriptor.clip = None + self._IO_descriptor.extras["controller_cfg"] = self.cfg.controller.__dict__ + self._IO_descriptor.extras["body_offset"] = self.cfg.body_offset.__dict__ + return self._IO_descriptor + """ Operations. """ @@ -274,11 +310,11 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma self._jacobi_joint_idx = [i + 6 for i in self._joint_ids] # log info for debugging - omni.log.info( + logger.info( f"Resolved joint names for the action term {self.__class__.__name__}:" f" {self._joint_names} [{self._joint_ids}]" ) - omni.log.info( + logger.info( f"Resolved ee body name for the action term {self.__class__.__name__}:" f" {self._ee_body_name} [{self._ee_body_idx}]" ) @@ -324,7 +360,7 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma 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 wrt the root frame + # 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 @@ -409,6 +445,47 @@ def jacobian_b(self) -> torch.Tensor: jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) return jacobian + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the pink inverse kinematics action. + It adds the following information to the base descriptor: + - body_name: The name of the body. + - joint_names: The names of the joints. + - position_scale: The scale of the position. + - orientation_scale: The scale of the orientation. + - wrench_scale: The scale of the wrench. + - stiffness_scale: The scale of the stiffness. + - damping_ratio_scale: The scale of the damping ratio. + - nullspace_joint_pos_target: The nullspace joint pos target. + - clip: The clip of the action term. + - controller_cfg: The configuration of the controller. + - body_offset: The offset of the body. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "TaskSpaceAction" + self._IO_descriptor.body_name = self._ee_body_name + self._IO_descriptor.joint_names = self._joint_names + self._IO_descriptor.position_scale = self.cfg.position_scale + self._IO_descriptor.orientation_scale = self.cfg.orientation_scale + self._IO_descriptor.wrench_scale = self.cfg.wrench_scale + self._IO_descriptor.stiffness_scale = self.cfg.stiffness_scale + self._IO_descriptor.damping_ratio_scale = self.cfg.damping_ratio_scale + self._IO_descriptor.nullspace_joint_pos_target = self.cfg.nullspace_joint_pos_target + if self.cfg.clip is not None: + self._IO_descriptor.clip = self.cfg.clip + else: + self._IO_descriptor.clip = None + self._IO_descriptor.extras["controller_cfg"] = self.cfg.controller_cfg.__dict__ + self._IO_descriptor.extras["body_offset"] = self.cfg.body_offset.__dict__ + return self._IO_descriptor + """ Operations. """ @@ -589,10 +666,14 @@ def _compute_ee_jacobian(self): # 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 + 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 + 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.""" @@ -622,13 +703,13 @@ def _compute_ee_velocity(self): 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_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 0:3]) - self._ee_vel_b[:, 3:6] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 3:6]) + 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_rotate(self._ee_pose_b_no_offset[:, 3:7], self._offset_pos) + 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 @@ -640,7 +721,7 @@ def _compute_ee_force(self): 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_rotate_inverse(self._asset.data.root_quat_w, self._ee_force_w) + 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.""" @@ -690,9 +771,9 @@ def _preprocess_actions(self, actions: torch.Tensor): 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] *= ( + 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], diff --git a/source/isaaclab/isaaclab/envs/mdp/commands/__init__.py b/source/isaaclab/isaaclab/envs/mdp/commands/__init__.py index 28846fed0921..bdfce40473b2 100644 --- a/source/isaaclab/isaaclab/envs/mdp/commands/__init__.py +++ b/source/isaaclab/isaaclab/envs/mdp/commands/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/envs/mdp/commands/commands_cfg.py b/source/isaaclab/isaaclab/envs/mdp/commands/commands_cfg.py index a2af9bbb0fab..390980f3454d 100644 --- a/source/isaaclab/isaaclab/envs/mdp/commands/commands_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/commands/commands_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/envs/mdp/commands/null_command.py b/source/isaaclab/isaaclab/envs/mdp/commands/null_command.py index 6175a465188e..0fc2a378810f 100644 --- a/source/isaaclab/isaaclab/envs/mdp/commands/null_command.py +++ b/source/isaaclab/isaaclab/envs/mdp/commands/null_command.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/envs/mdp/commands/pose_2d_command.py b/source/isaaclab/isaaclab/envs/mdp/commands/pose_2d_command.py index 6c37a1146490..a10ee0473e4a 100644 --- a/source/isaaclab/isaaclab/envs/mdp/commands/pose_2d_command.py +++ b/source/isaaclab/isaaclab/envs/mdp/commands/pose_2d_command.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,15 +7,16 @@ from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + 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_from_euler_xyz, quat_rotate_inverse, wrap_to_pi, yaw_quat +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 @@ -117,11 +118,11 @@ def _resample_command(self, env_ids: Sequence[int]): 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_rotate_inverse(yaw_quat(self.robot.data.root_quat_w), target_vec) + 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 + # create markers if necessary for the first time if debug_vis: if not hasattr(self, "goal_pose_visualizer"): self.goal_pose_visualizer = VisualizationMarkers(self.cfg.goal_pose_visualizer_cfg) diff --git a/source/isaaclab/isaaclab/envs/mdp/commands/pose_command.py b/source/isaaclab/isaaclab/envs/mdp/commands/pose_command.py index 136262ea3c3e..2c62c4baf4b8 100644 --- a/source/isaaclab/isaaclab/envs/mdp/commands/pose_command.py +++ b/source/isaaclab/isaaclab/envs/mdp/commands/pose_command.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,10 +7,11 @@ from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation from isaaclab.managers import CommandTerm from isaaclab.markers import VisualizationMarkers @@ -101,8 +102,8 @@ def _update_metrics(self): pos_error, rot_error = compute_pose_error( self.pose_command_w[:, :3], self.pose_command_w[:, 3:], - self.robot.data.body_state_w[:, self.body_idx, :3], - self.robot.data.body_state_w[:, self.body_idx, 3:7], + 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) @@ -127,7 +128,7 @@ def _update_command(self): pass def _set_debug_vis_impl(self, debug_vis: bool): - # create markers if necessary for the first tome + # create markers if necessary for the first time if debug_vis: if not hasattr(self, "goal_pose_visualizer"): # -- goal pose @@ -151,5 +152,5 @@ def _debug_vis_callback(self, event): # -- goal pose self.goal_pose_visualizer.visualize(self.pose_command_w[:, :3], self.pose_command_w[:, 3:]) # -- current body pose - body_link_state_w = self.robot.data.body_state_w[:, self.body_idx] - self.current_pose_visualizer.visualize(body_link_state_w[:, :3], body_link_state_w[:, 3:7]) + 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/source/isaaclab/isaaclab/envs/mdp/commands/velocity_command.py b/source/isaaclab/isaaclab/envs/mdp/commands/velocity_command.py index b3645acd4d27..38bc076a9591 100644 --- a/source/isaaclab/isaaclab/envs/mdp/commands/velocity_command.py +++ b/source/isaaclab/isaaclab/envs/mdp/commands/velocity_command.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,11 @@ from __future__ import annotations -import torch +import logging from collections.abc import Sequence from typing import TYPE_CHECKING -import omni.log +import torch import isaaclab.utils.math as math_utils from isaaclab.assets import Articulation @@ -23,6 +23,9 @@ from .commands_cfg import NormalVelocityCommandCfg, UniformVelocityCommandCfg +# import logger +logger = logging.getLogger(__name__) + class UniformVelocityCommand(CommandTerm): r"""Command generator that generates a velocity command in SE(2) from uniform distribution. @@ -65,7 +68,7 @@ def __init__(self, cfg: UniformVelocityCommandCfg, env: ManagerBasedEnv): " parameter is set to None." ) if self.cfg.ranges.heading and not self.cfg.heading_command: - omni.log.warn( + logger.warning( 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." ) @@ -163,7 +166,7 @@ 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 + # create markers if necessary for the first time if not hasattr(self, "goal_vel_visualizer"): # -- goal self.goal_vel_visualizer = VisualizationMarkers(self.cfg.goal_vel_visualizer_cfg) diff --git a/source/isaaclab/isaaclab/envs/mdp/curriculums.py b/source/isaaclab/isaaclab/envs/mdp/curriculums.py index 17835381febd..8438e5bec925 100644 --- a/source/isaaclab/isaaclab/envs/mdp/curriculums.py +++ b/source/isaaclab/isaaclab/envs/mdp/curriculums.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,26 +11,286 @@ from __future__ import annotations +import re from collections.abc import Sequence -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, ClassVar + +from isaaclab.managers import CurriculumTermCfg, ManagerTermBase 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. +class modify_reward_weight(ManagerTermBase): + """Curriculum that modifies the reward weight based on a step-wise schedule.""" - 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) + def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + + # obtain term configuration + term_name = cfg.params["term_name"] + self._term_cfg = env.reward_manager.get_term_cfg(term_name) + + def __call__( + self, + env: ManagerBasedRLEnv, + env_ids: Sequence[int], + term_name: str, + weight: float, + num_steps: int, + ) -> float: # update term settings - term_cfg.weight = weight - env.reward_manager.set_term_cfg(term_name, term_cfg) + if env.common_step_counter > num_steps: + self._term_cfg.weight = weight + env.reward_manager.set_term_cfg(term_name, self._term_cfg) + + return self._term_cfg.weight + + +class modify_env_param(ManagerTermBase): + """Curriculum term for modifying an environment parameter at runtime. + + This term helps modify an environment parameter (or attribute) at runtime. + This parameter can be any attribute of the environment, such as the physics material properties, + observation ranges, or any other configurable parameter that can be accessed via a dotted path. + + The term uses the ``address`` parameter to specify the target attribute as a dotted path string. + For instance, "event_manager.cfg.object_physics_material.func.material_buckets" would + refer to the attribute ``material_buckets`` in the event manager's event term "object_physics_material", + which is a tensor of sampled physics material properties. + + The term uses the ``modify_fn`` parameter to specify the function that modifies the value of the target attribute. + The function should have the signature: + + .. code-block:: python + + def modify_fn(env, env_ids, old_value, **modify_params) -> new_value | modify_env_param.NO_CHANGE: + # modify the value based on the old value and the modify parameters + new_value = old_value + modify_params["value"] + return new_value + + where ``env`` is the learning environment, ``env_ids`` are the sub-environment indices, + ``old_value`` is the current value of the target attribute, and ``modify_params`` + are additional parameters that can be passed to the function. The function should return + the new value to be set for the target attribute, or the special token ``modify_env_param.NO_CHANGE`` + to indicate that the value should not be changed. + + At the first call to the term after initialization, it compiles getter and setter functions + for the target attribute specified by the ``address`` parameter. The getter retrieves the + current value, and the setter writes a new value back to the attribute. + + This term processes getter/setter accessors for a target attribute in an(specified by + as an "address" in the term configuration :attr:`cfg.params["address"]`) the first time it is called, + then on each invocation reads the current value, applies a user-provided :attr:`modify_fn`, + and writes back the result. Since :obj:`None` in this case can sometime be desirable value + to write, we use token, :attr:`NO_CHANGE`, as non-modification signal to this class, see usage below. + + Usage: + .. code-block:: python + + def resample_bucket_range( + env, env_id, data, static_friction_range, dynamic_friction_range, restitution_range, num_steps + ): + if env.common_step_counter > num_steps: + range_list = [static_friction_range, dynamic_friction_range, restitution_range] + ranges = torch.tensor(range_list, device="cpu") + new_buckets = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(data), 3), device="cpu") + return new_buckets + + # if the step counter is not reached, return NO_CHANGE to indicate no modification. + # we do this instead of returning None, since None is a valid value to set. + # additionally, returning the input data would not change the value but still lead + # to the setter being called, which may add overhead. + return mdp.modify_env_param.NO_CHANGE + + + object_physics_material_curriculum = CurrTerm( + func=mdp.modify_env_param, + params={ + "address": "event_manager.cfg.object_physics_material.func.material_buckets", + "modify_fn": resample_bucket_range, + "modify_params": { + "static_friction_range": [0.5, 1.0], + "dynamic_friction_range": [0.3, 1.0], + "restitution_range": [0.0, 0.5], + "num_step": 120000, + }, + }, + ) + """ + + NO_CHANGE: ClassVar = object() + """Special token to indicate no change in the value to be set. + + This token is used to signal that the `modify_fn` did not produce a new value. It can + be returned by the `modify_fn` to indicate that the current value should remain unchanged. + """ + + def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + # resolve term configuration + if "address" not in cfg.params: + raise ValueError("The 'address' parameter must be specified in the curriculum term configuration.") + + # store current address + self._address: str = cfg.params["address"] + # store accessor functions + self._get_fn: callable = None + self._set_fn: callable = None + + def __del__(self): + """Destructor to clean up the compiled functions.""" + # clear the getter and setter functions + self._get_fn = None + self._set_fn = None + self._container = None + self._last_path = None + + """ + Operations. + """ + + def __call__( + self, + env: ManagerBasedRLEnv, + env_ids: Sequence[int], + address: str, + modify_fn: callable, + modify_params: dict | None = None, + ): + # fetch the getter and setter functions if not already compiled + if not self._get_fn: + self._get_fn, self._set_fn = self._process_accessors(self._env, self._address) + + # resolve none type + modify_params = {} if modify_params is None else modify_params + + # get the current value of the target attribute + data = self._get_fn() + # modify the value using the provided function + new_val = modify_fn(self._env, env_ids, data, **modify_params) + # set the modified value back to the target attribute + # note: if the modify_fn return NO_CHANGE signal, we do not invoke self.set_fn + if new_val is not self.NO_CHANGE: + self._set_fn(new_val) + + """ + Helper functions. + """ + + def _process_accessors(self, root: ManagerBasedRLEnv, path: str) -> tuple[callable, callable]: + """Process and return the (getter, setter) functions for a dotted attribute path. + + This function resolves a dotted path string to an attribute in the given root object. + The dotted path can include nested attributes, dictionary keys, and sequence indexing. + + For instance, the path "foo.bar[2].baz" would resolve to `root.foo.bar[2].baz`. This + allows accessing attributes in a nested structure, such as a dictionary or a list. + + Args: + root: The main object from which to resolve the attribute. + path: Dotted path string to the attribute variable. For e.g., "foo.bar[2].baz". + + Returns: + A tuple of two functions (getter, setter), where: + the getter retrieves the current value of the attribute, and + the setter writes a new value back to the attribute. + """ + # Turn "a.b[2].c" into ["a", ("b", 2), "c"] and store in parts + path_parts: list[str | tuple[str, int]] = [] + for part in path.split("."): + m = re.compile(r"^(\w+)\[(\d+)\]$").match(part) + if m: + path_parts.append((m.group(1), int(m.group(2)))) + else: + path_parts.append(part) + + # Traverse the parts to find the container + container = root + for container_path in path_parts[:-1]: + if isinstance(container_path, tuple): + # we are accessing a list element + name, idx = container_path + # find underlying attribute + if isinstance(container_path, dict): + seq = container[name] # type: ignore[assignment] + else: + seq = getattr(container, name) + # save the container for the next iteration + container = seq[idx] + else: + # we are accessing a dictionary key or an attribute + if isinstance(container, dict): + container = container[container_path] + else: + container = getattr(container, container_path) + + # save the container and the last part of the path + self._container = container + self._last_path = path_parts[-1] # for "a.b[2].c", this is "c", while for "a.b[2]" it is 2 + + # build the getter and setter + if isinstance(self._container, tuple): + get_value = lambda: self._container[self._last_path] # noqa: E731 + + def set_value(val): + tuple_list = list(self._container) + tuple_list[self._last_path] = val + self._container = tuple(tuple_list) + + elif isinstance(self._container, (list, dict)): + get_value = lambda: self._container[self._last_path] # noqa: E731 + + def set_value(val): + self._container[self._last_path] = val + + elif isinstance(self._container, object): + get_value = lambda: getattr(self._container, self._last_path) # noqa: E731 + set_value = lambda val: setattr(self._container, self._last_path, val) # noqa: E731 + else: + raise TypeError( + f"Unable to build accessors for address '{path}'. Unknown type found for access variable:" + f" '{type(self._container)}'. Expected a list, dict, or object with attributes." + ) + + return get_value, set_value + + +class modify_term_cfg(modify_env_param): + """Curriculum for modifying a manager term configuration at runtime. + + This class inherits from :class:`modify_env_param` and is specifically designed to modify + the configuration of a manager term in the environment. It mainly adds the convenience of + using a simplified address style that uses "s." as a prefix to refer to the manager's configuration. + + For instance, instead of writing "event_manager.cfg.object_physics_material.func.material_buckets", + you can write "events.object_physics_material.func.material_buckets" to refer to the same term configuration. + The same applies to other managers, such as "observations", "commands", "rewards", and "terminations". + + Internally, it replaces the first occurrence of "s." in the address with "_manager.cfg.", + thus transforming the simplified address into a full manager path. + + Usage: + .. code-block:: python + + def override_value(env, env_ids, data, value, num_steps): + if env.common_step_counter > num_steps: + return value + return mdp.modify_term_cfg.NO_CHANGE + + + command_object_pose_xrange_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "commands.object_pose.ranges.pos_x", # note: `_manager.cfg` is omitted + "modify_fn": override_value, + "modify_params": {"value": (-0.75, -0.25), "num_steps": 12000}, + }, + ) + """ + + def __init__(self, cfg, env): + # initialize the parent + super().__init__(cfg, env) + # overwrite the simplified address with the full manager path + self._address = self._address.replace("s.", "_manager.cfg.", 1) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 3d322d062350..484121f4e491 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,13 +14,15 @@ from __future__ import annotations +import logging import math -import torch +import re from typing import TYPE_CHECKING, Literal +import torch + 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 @@ -29,11 +31,16 @@ from isaaclab.actuators import ImplicitActuator from isaaclab.assets import Articulation, DeformableObject, RigidObject from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg +from isaaclab.sim.utils.stage import get_current_stage from isaaclab.terrains import TerrainImporter +from isaaclab.utils.version import compare_versions, get_isaac_sim_version if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv +# import logger +logger = logging.getLogger(__name__) + def randomize_rigid_body_scale( env: ManagerBasedEnv, @@ -53,9 +60,9 @@ def randomize_rigid_body_scale( 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". + 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 @@ -92,7 +99,7 @@ def randomize_rigid_body_scale( env_ids = env_ids.cpu() # acquire stage - stage = omni.usd.get_context().get_stage() + stage = get_current_stage() # resolve prim paths for spawning and cloning prim_paths = sim_utils.find_matching_prim_paths(asset.cfg.prim_path) @@ -276,32 +283,134 @@ def __call__( 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, -): +class randomize_rigid_body_mass(ManagerTermBase): """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. + 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. + If the :attr:`recompute_inertia` flag is set to :obj:`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] + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + + Raises: + TypeError: If `params` is not a tuple of two numbers. + ValueError: If the operation is not supported. + ValueError: If the lower bound is negative or zero when not allowed. + ValueError: If the upper bound is less than the lower bound. + """ + 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] + # check for valid operation + if cfg.params["operation"] == "scale": + if "mass_distribution_params" in cfg.params: + _validate_scale_range( + cfg.params["mass_distribution_params"], "mass_distribution_params", allow_zero=False + ) + elif cfg.params["operation"] not in ("abs", "add"): + raise ValueError( + "Randomization term 'randomize_rigid_body_mass' does not support operation:" + f" '{cfg.params['operation']}'." + ) + if cfg.params.get("min_mass") is not None: + if cfg.params.get("min_mass") < 1e-6: + raise ValueError( + "Randomization term 'randomize_rigid_body_mass' does not support 'min_mass' less than 1e-6 to avoid" + " physics errors." + ) + + def __call__( + self, + 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, + min_mass: float = 1e-6, + ): + # 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 self.asset_cfg.body_ids == slice(None): + body_ids = torch.arange(self.asset.num_bodies, dtype=torch.int, device="cpu") + else: + body_ids = torch.tensor(self.asset_cfg.body_ids, dtype=torch.int, device="cpu") + + # get the current masses of the bodies (num_assets, num_bodies) + masses = self.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] = self.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 + ) + masses = torch.clamp(masses, min=min_mass) # ensure masses are positive + + # set the mass into the physics simulation + self.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] / self.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 = self.asset.root_physx_view.get_inertias() + if isinstance(self.asset, Articulation): + # inertia has shape: (num_envs, num_bodies, 9) for articulation + inertias[env_ids[:, None], body_ids] = ( + self.asset.data.default_inertia[env_ids[:, None], body_ids] * ratios[..., None] + ) + else: + # inertia has shape: (num_envs, 9) for rigid object + inertias[env_ids] = self.asset.data.default_inertia[env_ids] * ratios + # set the inertia tensors into the physics simulation + self.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") @@ -314,41 +423,19 @@ def randomize_rigid_body_mass( 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() + # 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) - # 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() + # get the current com of the bodies (num_assets, num_bodies) + coms = asset.root_physx_view.get_coms().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 - ) + # Randomize the com in range + coms[env_ids[:, None], body_ids, :3] += rand_samples - # 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) + # Set the new coms + asset.root_physx_view.set_coms(coms, env_ids) def randomize_rigid_body_collider_offsets( @@ -451,90 +538,118 @@ def randomize_physics_scene_gravity( 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", -): +class randomize_actuator_gains(ManagerTermBase): """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. + 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 __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. - 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 - ) + Args: + cfg: The configuration of the event term. + env: The environment instance. + + Raises: + TypeError: If `params` is not a tuple of two numbers. + ValueError: If the operation is not supported. + ValueError: If the lower bound is negative or zero when not allowed. + ValueError: If the upper bound is less than the lower bound. + """ + super().__init__(cfg, env) - # 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) + # 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] + # check for valid operation + if cfg.params["operation"] == "scale": + if "stiffness_distribution_params" in cfg.params: + _validate_scale_range( + cfg.params["stiffness_distribution_params"], "stiffness_distribution_params", allow_zero=False + ) + if "damping_distribution_params" in cfg.params: + _validate_scale_range(cfg.params["damping_distribution_params"], "damping_distribution_params") + elif cfg.params["operation"] not in ("abs", "add"): + raise ValueError( + "Randomization term 'randomize_actuator_gains' does not support operation:" + f" '{cfg.params['operation']}'." + ) + + def __call__( + self, + 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", + ): + # Resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=self.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 self.asset.actuators.values(): + if isinstance(self.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) + elif isinstance(actuator.joint_indices, torch.Tensor): + global_indices = actuator.joint_indices.to(self.asset.device) + else: + 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: - 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) + # we take the intersection of the actuator joints and the asset config joints + 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) + 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] = self.asset.data.default_joint_stiffness[env_ids][ + :, global_indices + ].clone() + randomize(stiffness, stiffness_distribution_params) + actuator.stiffness[env_ids] = stiffness + if isinstance(actuator, ImplicitActuator): + self.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] = self.asset.data.default_joint_damping[env_ids][:, global_indices].clone() + randomize(damping, damping_distribution_params) + actuator.damping[env_ids] = damping + if isinstance(actuator, ImplicitActuator): + self.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", -): +class randomize_joint_parameters(ManagerTermBase): """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 @@ -549,216 +664,347 @@ def randomize_joint_parameters( 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) + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. - # 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 - ) + Args: + cfg: The configuration of the event term. + env: The environment instance. - # 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, + Raises: + TypeError: If `params` is not a tuple of two numbers. + ValueError: If the operation is not supported. + ValueError: If the lower bound is negative or zero when not allowed. + ValueError: If the upper bound is less than the lower bound. + """ + 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] + # check for valid operation + if cfg.params["operation"] == "scale": + if "friction_distribution_params" in cfg.params: + _validate_scale_range(cfg.params["friction_distribution_params"], "friction_distribution_params") + if "armature_distribution_params" in cfg.params: + _validate_scale_range(cfg.params["armature_distribution_params"], "armature_distribution_params") + elif cfg.params["operation"] not in ("abs", "add"): + raise ValueError( + "Randomization term 'randomize_fixed_tendon_parameters' does not support operation:" + f" '{cfg.params['operation']}'." + ) + + def __call__( + self, + 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", + ): + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=self.asset.device) + + # resolve joint indices + if self.asset_cfg.joint_ids == slice(None): + joint_ids = slice(None) # for optimization purposes + else: + joint_ids = torch.tensor(self.asset_cfg.joint_ids, dtype=torch.int, device=self.asset.device) + + if env_ids != slice(None) and joint_ids != slice(None): + env_ids_for_slice = env_ids[:, None] + else: + env_ids_for_slice = env_ids + + # 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( + self.asset.data.default_joint_friction_coeff.clone(), + friction_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, + + # 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_for_slice, joint_ids] + + # if isaacsim version is lower than 5.0.0 we can set only the static friction coefficient + if get_isaac_sim_version().major >= 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_for_slice, joint_ids] + viscous_friction_coeff = viscous_friction_coeff[env_ids_for_slice, 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( + 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 + if armature_distribution_params is not None: + armature = _randomize_prop_by_op( + self.asset.data.default_joint_armature.clone(), + armature_distribution_params, env_ids, joint_ids, operation=operation, distribution=distribution, ) + self.asset.write_joint_armature_to_sim( + armature[env_ids_for_slice, joint_ids], joint_ids=joint_ids, env_ids=env_ids + ) - # 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." + # joint position limits + if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None: + joint_pos_limits = self.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_for_slice, 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 + self.asset.write_joint_position_limit_to_sim( + joint_pos_limits, joint_ids=joint_ids, env_ids=env_ids, warn_limit_violation=False ) - # 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", -): +class randomize_fixed_tendon_parameters(ManagerTermBase): """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. - + 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) + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. - # 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, + Args: + cfg: The configuration of the event term. + env: The environment instance. + + Raises: + TypeError: If `params` is not a tuple of two numbers. + ValueError: If the operation is not supported. + ValueError: If the lower bound is negative or zero when not allowed. + ValueError: If the upper bound is less than the lower bound. + """ + 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] + # check for valid operation + if cfg.params["operation"] == "scale": + if "stiffness_distribution_params" in cfg.params: + _validate_scale_range( + cfg.params["stiffness_distribution_params"], "stiffness_distribution_params", allow_zero=False + ) + if "damping_distribution_params" in cfg.params: + _validate_scale_range(cfg.params["damping_distribution_params"], "damping_distribution_params") + if "limit_stiffness_distribution_params" in cfg.params: + _validate_scale_range( + cfg.params["limit_stiffness_distribution_params"], "limit_stiffness_distribution_params" + ) + elif cfg.params["operation"] not in ("abs", "add"): + raise ValueError( + "Randomization term 'randomize_fixed_tendon_parameters' does not support operation:" + f" '{cfg.params['operation']}'." + ) + + def __call__( + self, + 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", + ): + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=self.asset.device) + + # resolve joint indices + if self.asset_cfg.fixed_tendon_ids == slice(None): + tendon_ids = slice(None) # for optimization purposes + else: + tendon_ids = torch.tensor(self.asset_cfg.fixed_tendon_ids, dtype=torch.int, device=self.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( + self.asset.data.default_fixed_tendon_stiffness.clone(), + stiffness_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, + self.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( + self.asset.data.default_fixed_tendon_damping.clone(), + damping_distribution_params, env_ids, tendon_ids, operation=operation, distribution=distribution, ) + self.asset.set_fixed_tendon_damping(damping[env_ids[:, None], tendon_ids], tendon_ids, env_ids) - # 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." + # limit stiffness + if limit_stiffness_distribution_params is not None: + limit_stiffness = _randomize_prop_by_op( + self.asset.data.default_fixed_tendon_limit_stiffness.clone(), + limit_stiffness_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + self.asset.set_fixed_tendon_limit_stiffness( + limit_stiffness[env_ids[:, None], tendon_ids], tendon_ids, env_ids ) - 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) + # position limits + if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None: + limit = self.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." + ) + self.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( + self.asset.data.default_fixed_tendon_rest_length.clone(), + rest_length_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + self.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( + self.asset.data.default_fixed_tendon_offset.clone(), + offset_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + self.asset.set_fixed_tendon_offset(offset[env_ids[:, None], tendon_ids], tendon_ids, env_ids) + + # write the fixed tendon properties into the simulation + self.asset.write_fixed_tendon_properties_to_sim(tendon_ids, env_ids) def apply_external_force_torque( @@ -789,7 +1035,12 @@ def apply_external_force_torque( 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) + asset.permanent_wrench_composer.set_forces_and_torques( + forces=forces, + torques=torques, + body_ids=asset_cfg.body_ids, + env_ids=env_ids, + ) def push_by_setting_velocity( @@ -998,23 +1249,30 @@ def reset_joints_by_scale( """ # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] + + # cast env_ids to allow broadcasting + if asset_cfg.joint_ids != slice(None): + iter_env_ids = env_ids[:, None] + else: + iter_env_ids = env_ids + # get default joint state - joint_pos = asset.data.default_joint_pos[env_ids].clone() - joint_vel = asset.data.default_joint_vel[env_ids].clone() + joint_pos = asset.data.default_joint_pos[iter_env_ids, asset_cfg.joint_ids].clone() + joint_vel = asset.data.default_joint_vel[iter_env_ids, asset_cfg.joint_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_limits = asset.data.soft_joint_pos_limits[iter_env_ids, asset_cfg.joint_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_limits = asset.data.soft_joint_vel_limits[iter_env_ids, asset_cfg.joint_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) + asset.write_joint_state_to_sim(joint_pos, joint_vel, joint_ids=asset_cfg.joint_ids, env_ids=env_ids) def reset_joints_by_offset( @@ -1032,23 +1290,29 @@ def reset_joints_by_offset( # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] + # cast env_ids to allow broadcasting + if asset_cfg.joint_ids != slice(None): + iter_env_ids = env_ids[:, None] + else: + iter_env_ids = env_ids + # get default joint state - joint_pos = asset.data.default_joint_pos[env_ids].clone() - joint_vel = asset.data.default_joint_vel[env_ids].clone() + joint_pos = asset.data.default_joint_pos[iter_env_ids, asset_cfg.joint_ids].clone() + joint_vel = asset.data.default_joint_vel[iter_env_ids, asset_cfg.joint_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_limits = asset.data.soft_joint_pos_limits[iter_env_ids, asset_cfg.joint_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_limits = asset.data.soft_joint_vel_limits[iter_env_ids, asset_cfg.joint_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) + asset.write_joint_state_to_sim(joint_pos, joint_vel, joint_ids=asset_cfg.joint_ids, env_ids=env_ids) def reset_nodal_state_uniform( @@ -1093,8 +1357,14 @@ def reset_nodal_state_uniform( 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.""" +def reset_scene_to_default(env: ManagerBasedEnv, env_ids: torch.Tensor, reset_joint_targets: bool = False): + """Reset the scene to the default state specified in the scene configuration. + + If :attr:`reset_joint_targets` is True, the joint position and velocity targets of the articulations are + also reset to their default values. This might be useful for some cases to clear out any previously set targets. + However, this is not the default behavior as based on our experience, it is not always desired to reset + targets to default values, especially when the targets should be handled by action terms and not event terms. + """ # rigid bodies for rigid_object in env.scene.rigid_objects.values(): # obtain default and deal with the offset for env origins @@ -1116,6 +1386,10 @@ def reset_scene_to_default(env: ManagerBasedEnv, env_ids: torch.Tensor): 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) + # reset joint targets if required + if reset_joint_targets: + articulation_asset.set_joint_position_target(default_joint_pos, env_ids=env_ids) + articulation_asset.set_joint_velocity_target(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 @@ -1151,18 +1425,9 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): """ 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 warning + # 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." @@ -1170,8 +1435,14 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): " by setting 'replicate_physics' to False in 'InteractiveSceneCfg'." ) - # convert from radians to degrees - texture_rotation = tuple(math.degrees(angle) for angle in texture_rotation) + # 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") # obtain the asset entity asset = env.scene[asset_cfg.name] @@ -1186,23 +1457,71 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): 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" + # Check if the pattern with '/visuals' yields results when matching `body_names_regex`. + # If not, fall back to a broader pattern without '/visuals'. + asset_main_prim_path = asset.cfg.prim_path + pattern_with_visuals = f"{asset_main_prim_path}/{body_names_regex}/visuals" + # Use sim_utils to check if any prims currently match this pattern + matching_prims = sim_utils.find_matching_prim_paths(pattern_with_visuals) + if matching_prims: + # If matches are found, use the pattern with /visuals + prim_path = pattern_with_visuals + else: + # If no matches found, fall back to the broader pattern without /visuals + # This pattern (e.g., /World/envs/env_.*/Table/.*) should match visual prims + # whether they end in /visuals or have other structures. + prim_path = f"{asset_main_prim_path}/.*" + logging.info( + f"Pattern '{pattern_with_visuals}' found no prims. Falling back to '{prim_path}' for texture" + " randomization." + ) - # Create the omni-graph node for the randomization term - def rep_texture_randomization(): - prims_group = rep.get.prims(path_pattern=prim_path) + # extract the replicator version + version = re.match(r"^(\d+\.\d+\.\d+)", rep.__file__.split("/")[-5][21:]).group(1) - with prims_group: - rep.randomizer.texture( - textures=texture_paths, project_uvw=True, texture_rotate=rep.distribution.uniform(*texture_rotation) - ) + # use different path for different version of replicator + if compare_versions(version, "1.12.4") < 0: + 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)) + + # convert from radians to degrees + texture_rotation = tuple(math.degrees(angle) for angle in texture_rotation) - return prims_group.node + # Create the omni-graph node for the randomization term + def rep_texture_randomization(): + prims_group = rep.get.prims(path_pattern=prim_path) - # Register the event to the replicator - with rep.trigger.on_custom_event(event_name=event_name): - rep_texture_randomization() + 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() + else: + # acquire stage + stage = get_current_stage() + prims_group = rep.functional.get.prims(path_pattern=prim_path, stage=stage) + + num_prims = len(prims_group) + # rng that randomizes the texture and rotation + self.texture_rng = rep.rng.ReplicatorRNG() + + # Create the material first and bind it to the prims + for i, prim in enumerate(prims_group): + # Disable instancble + if prim.IsInstanceable(): + prim.SetInstanceable(False) + + # TODO: Should we specify the value when creating the material? + self.material_prims = rep.functional.create_batch.material( + mdl="OmniPBR.mdl", bind_prims=prims_group, count=num_prims, project_uvw=True + ) def __call__( self, @@ -1213,13 +1532,36 @@ def __call__( 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) + # we import the module here since we may not always need the replicator + import omni.replicator.core as rep + + # extract the replicator version + version = re.match(r"^(\d+\.\d+\.\d+)", rep.__file__.split("/")[-5][21:]).group(1) + + # use different path for different version of replicator + if compare_versions(version, "1.12.4") < 0: + rep.utils.send_og_event(event_name) + else: + # read parameters from the configuration + texture_paths = texture_paths if texture_paths else self._cfg.params.get("texture_paths") + texture_rotation = ( + texture_rotation if texture_rotation else self._cfg.params.get("texture_rotation", (0.0, 0.0)) + ) + + # convert from radians to degrees + texture_rotation = tuple(math.degrees(angle) for angle in texture_rotation) + + num_prims = len(self.material_prims) + random_textures = self.texture_rng.generator.choice(texture_paths, size=num_prims) + random_rotations = self.texture_rng.generator.uniform( + texture_rotation[0], texture_rotation[1], size=num_prims + ) + + # modify the material properties + rep.functional.modify.attribute(self.material_prims, "diffuse_texture", random_textures) + rep.functional.modify.attribute(self.material_prims, "texture_rotate", random_rotations) class randomize_visual_color(ManagerTermBase): @@ -1246,7 +1588,12 @@ class randomize_visual_color(ManagerTermBase): """ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): - """Initialize the randomization term.""" + """Initialize the randomization term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + """ super().__init__(cfg, env) # enable replicator extension if not already enabled @@ -1256,11 +1603,11 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): # 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 warning + # 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." @@ -1277,27 +1624,51 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): 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) + # extract the replicator version + version = re.match(r"^(\d+\.\d+\.\d+)", rep.__file__.split("/")[-5][21:]).group(1) + + # use different path for different version of replicator + if compare_versions(version, "1.12.4") < 0: + colors = cfg.params.get("colors") + event_name = cfg.params.get("event_name") - # Create the omni-graph node for the randomization term - def rep_texture_randomization(): - prims_group = rep.get.prims(path_pattern=mesh_prim_path) + # 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_color_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_color_randomization() + else: + stage = get_current_stage() + prims_group = rep.functional.get.prims(path_pattern=mesh_prim_path, stage=stage) - with prims_group: - rep.randomizer.color(colors=colors) + num_prims = len(prims_group) + self.color_rng = rep.rng.ReplicatorRNG() - return prims_group.node + # Create the material first and bind it to the prims + for i, prim in enumerate(prims_group): + # Disable instancble + if prim.IsInstanceable(): + prim.SetInstanceable(False) - # Register the event to the replicator - with rep.trigger.on_custom_event(event_name=event_name): - rep_texture_randomization() + # TODO: Should we specify the value when creating the material? + self.material_prims = rep.functional.create_batch.material( + mdl="OmniPBR.mdl", bind_prims=prims_group, count=num_prims, project_uvw=True + ) def __call__( self, @@ -1308,11 +1679,33 @@ def __call__( colors: list[tuple[float, float, float]] | dict[str, tuple[float, float]], mesh_name: str = "", ): - # import 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. + + # we import the module here since we may not always need the replicator import omni.replicator.core as rep - # only send the event to the replicator - rep.utils.send_og_event(event_name) + version = re.match(r"^(\d+\.\d+\.\d+)", rep.__file__.split("/")[-5][21:]).group(1) + + # use different path for different version of replicator + if compare_versions(version, "1.12.4") < 0: + rep.utils.send_og_event(event_name) + else: + colors = colors if colors else self._cfg.params.get("colors") + + # 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 = [color_low, color_high] + else: + colors = list(colors) + + num_prims = len(self.material_prims) + random_colors = self.color_rng.generator.uniform(colors[0], colors[1], size=(num_prims, 3)) + + rep.functional.modify.attribute(self.material_prims, "diffuse_color_constant", random_colors) """ @@ -1383,3 +1776,47 @@ def _randomize_prop_by_op( f"Unknown operation: '{operation}' for property randomization. Please use 'add', 'scale', or 'abs'." ) return data + + +def _validate_scale_range( + params: tuple[float, float] | None, + name: str, + *, + allow_negative: bool = False, + allow_zero: bool = True, +) -> None: + """ + Validates a (low, high) tuple used in scale-based randomization. + + This function ensures the tuple follows expected rules when applying a 'scale' + operation. It performs type and value checks, optionally allowing negative or + zero lower bounds. + + Args: + params (tuple[float, float] | None): The (low, high) range to validate. If None, + validation is skipped. + name (str): The name of the parameter being validated, used for error messages. + allow_negative (bool, optional): If True, allows the lower bound to be negative. + Defaults to False. + allow_zero (bool, optional): If True, allows the lower bound to be zero. + Defaults to True. + + Raises: + TypeError: If `params` is not a tuple of two numbers. + ValueError: If the lower bound is negative or zero when not allowed. + ValueError: If the upper bound is less than the lower bound. + + Example: + _validate_scale_range((0.5, 1.5), "mass_scale") + """ + if params is None: # caller didnโ€™t request randomisation for this field + return + low, high = params + if not isinstance(low, (int, float)) or not isinstance(high, (int, float)): + raise TypeError(f"{name}: expected (low, high) to be a tuple of numbers, got {params}.") + if not allow_negative and not allow_zero and low <= 0: + raise ValueError(f"{name}: lower bound must be > 0 when using the 'scale' operation (got {low}).") + if not allow_negative and allow_zero and low < 0: + raise ValueError(f"{name}: lower bound must be โ‰ฅ 0 when using the 'scale' operation (got {low}).") + if high < low: + raise ValueError(f"{name}: upper bound ({high}) must be โ‰ฅ lower bound ({low}).") diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index 12e93b69d287..9839e0d70837 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,9 +11,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils from isaaclab.assets import Articulation, RigidObject from isaaclab.managers import SceneEntityCfg @@ -24,12 +25,22 @@ if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv +from isaaclab.envs.utils.io_descriptors import ( + generic_io_descriptor, + record_body_names, + record_dtype, + record_joint_names, + record_joint_pos_offsets, + record_joint_vel_offsets, + record_shape, +) """ Root state. """ +@generic_io_descriptor(units="m", axes=["Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype]) 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) @@ -37,6 +48,9 @@ def base_pos_z(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg( return asset.data.root_pos_w[:, 2].unsqueeze(-1) +@generic_io_descriptor( + units="m/s", axes=["X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] +) 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) @@ -44,6 +58,9 @@ def base_lin_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCf return asset.data.root_lin_vel_b +@generic_io_descriptor( + units="rad/s", axes=["X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] +) 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) @@ -51,6 +68,9 @@ def base_ang_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCf return asset.data.root_ang_vel_b +@generic_io_descriptor( + units="m/s^2", axes=["X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] +) 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) @@ -58,6 +78,9 @@ def projected_gravity(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEnt return asset.data.projected_gravity_b +@generic_io_descriptor( + units="m", axes=["X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] +) 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) @@ -65,6 +88,9 @@ def root_pos_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg( return asset.data.root_pos_w - env.scene.env_origins +@generic_io_descriptor( + units="unit", axes=["W", "X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] +) def root_quat_w( env: ManagerBasedEnv, make_quat_unique: bool = False, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") ) -> torch.Tensor: @@ -82,6 +108,9 @@ def root_quat_w( return math_utils.quat_unique(quat) if make_quat_unique else quat +@generic_io_descriptor( + units="m/s", axes=["X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] +) 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) @@ -89,6 +118,9 @@ def root_lin_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntity return asset.data.root_lin_vel_w +@generic_io_descriptor( + units="rad/s", axes=["X", "Y", "Z"], observation_type="RootState", on_inspect=[record_shape, record_dtype] +) 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) @@ -96,11 +128,72 @@ def root_ang_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntity return asset.data.root_ang_vel_w +""" +Body state +""" + + +@generic_io_descriptor(observation_type="BodyState", on_inspect=[record_shape, record_dtype, record_body_names]) +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] + + # access the body poses in world frame + pose = asset.data.body_pose_w[:, asset_cfg.body_ids, :7] + if isinstance(asset_cfg.body_ids, (slice, int)): + pose = pose.clone() # if slice or int, make a copy to avoid modifying original data + pose[..., :3] = pose[..., :3] - env.scene.env_origins.unsqueeze(1) + return pose.reshape(env.num_envs, -1) + + +@generic_io_descriptor(observation_type="BodyState", on_inspect=[record_shape, record_dtype, record_body_names]) +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. """ +@generic_io_descriptor( + observation_type="JointState", on_inspect=[record_joint_names, record_dtype, record_shape], units="rad" +) def joint_pos(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """The joint positions of the asset. @@ -111,6 +204,11 @@ def joint_pos(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg(" return asset.data.joint_pos[:, asset_cfg.joint_ids] +@generic_io_descriptor( + observation_type="JointState", + on_inspect=[record_joint_names, record_dtype, record_shape, record_joint_pos_offsets], + units="rad", +) 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. @@ -121,6 +219,7 @@ def joint_pos_rel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityC return asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.default_joint_pos[:, asset_cfg.joint_ids] +@generic_io_descriptor(observation_type="JointState", on_inspect=[record_joint_names, record_dtype, record_shape]) def joint_pos_limit_normalized( env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") ) -> torch.Tensor: @@ -137,6 +236,9 @@ def joint_pos_limit_normalized( ) +@generic_io_descriptor( + observation_type="JointState", on_inspect=[record_joint_names, record_dtype, record_shape], units="rad/s" +) def joint_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")): """The joint velocities of the asset. @@ -147,6 +249,11 @@ def joint_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg(" return asset.data.joint_vel[:, asset_cfg.joint_ids] +@generic_io_descriptor( + observation_type="JointState", + on_inspect=[record_joint_names, record_dtype, record_shape, record_joint_vel_offsets], + units="rad/s", +) def joint_vel_rel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")): """The joint velocities of the asset w.r.t. the default joint velocities. @@ -157,6 +264,26 @@ def joint_vel_rel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityC return asset.data.joint_vel[:, asset_cfg.joint_ids] - asset.data.default_joint_vel[:, asset_cfg.joint_ids] +@generic_io_descriptor( + observation_type="JointState", on_inspect=[record_joint_names, record_dtype, record_shape], units="N.m" +) +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. """ @@ -181,8 +308,8 @@ def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> tor # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] # obtain the link incoming forces in world frame - link_incoming_forces = asset.root_physx_view.get_link_incoming_joint_force()[:, asset_cfg.body_ids] - return link_incoming_forces.view(env.num_envs, -1) + 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: @@ -201,6 +328,21 @@ def imu_orientation(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntit return asset.data.quat_w +def imu_projected_gravity(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor: + """Imu sensor orientation w.r.t the env.scene.origin. + + Args: + env: The environment. + asset_cfg: The SceneEntity associated with an Imu sensor. + + Returns: + Gravity projected on imu_frame, shape of torch.tensor is (num_env,3). + """ + + asset: Imu = env.scene[asset_cfg.name] + return asset.data.projected_gravity_b + + 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. @@ -268,7 +410,7 @@ def image( 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 + # rgb/depth/normals image normalization if normalize: if data_type == "rgb": images = images.float() / 255.0 @@ -276,6 +418,8 @@ def image( images -= mean_tensor elif "distance_to" in data_type or "depth" in data_type: images[images == float("inf")] = 0 + elif "normals" in data_type: + images = (images + 1.0) * 0.5 return images.clone() @@ -509,6 +653,7 @@ def _inference(model, images: torch.Tensor) -> torch.Tensor: """ +@generic_io_descriptor(dtype=torch.float32, observation_type="Action", on_inspect=[record_shape]) def last_action(env: ManagerBasedEnv, action_name: str | None = None) -> torch.Tensor: """The last input action to the environment. @@ -526,6 +671,22 @@ def last_action(env: ManagerBasedEnv, action_name: str | None = None) -> torch.T """ -def generated_commands(env: ManagerBasedRLEnv, command_name: str) -> torch.Tensor: +@generic_io_descriptor(dtype=torch.float32, observation_type="Command", on_inspect=[record_shape]) +def generated_commands(env: ManagerBasedRLEnv, command_name: str | None = None) -> 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/source/isaaclab/isaaclab/envs/mdp/recorders/__init__.py b/source/isaaclab/isaaclab/envs/mdp/recorders/__init__.py index bf92aa9463af..cacc5e15c7f5 100644 --- a/source/isaaclab/isaaclab/envs/mdp/recorders/__init__.py +++ b/source/isaaclab/isaaclab/envs/mdp/recorders/__init__.py @@ -1,8 +1,7 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """Various recorder terms that can be used in the environment.""" from .recorders import * diff --git a/source/isaaclab/isaaclab/envs/mdp/recorders/recorders.py b/source/isaaclab/isaaclab/envs/mdp/recorders/recorders.py index 658bd0002c28..a9315bbca63d 100644 --- a/source/isaaclab/isaaclab/envs/mdp/recorders/recorders.py +++ b/source/isaaclab/isaaclab/envs/mdp/recorders/recorders.py @@ -1,12 +1,13 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - from __future__ import annotations from collections.abc import Sequence +import torch + from isaaclab.managers.recorder_manager import RecorderTerm @@ -42,3 +43,20 @@ class PreStepFlatPolicyObservationsRecorder(RecorderTerm): def record_pre_step(self): return "obs", self._env.obs_buf["policy"] + + +class PostStepProcessedActionsRecorder(RecorderTerm): + """Recorder term that records processed actions at the end of each step.""" + + def record_post_step(self): + processed_actions = None + + # Loop through active terms and concatenate their processed actions + for term_name in self._env.action_manager.active_terms: + term_actions = self._env.action_manager.get_term(term_name).processed_actions.clone() + if processed_actions is None: + processed_actions = term_actions + else: + processed_actions = torch.cat([processed_actions, term_actions], dim=-1) + + return "processed_actions", processed_actions diff --git a/source/isaaclab/isaaclab/envs/mdp/recorders/recorders_cfg.py b/source/isaaclab/isaaclab/envs/mdp/recorders/recorders_cfg.py index 69e34ca2c8e3..d67350d7f209 100644 --- a/source/isaaclab/isaaclab/envs/mdp/recorders/recorders_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/recorders/recorders_cfg.py @@ -1,8 +1,7 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - from isaaclab.managers.recorder_manager import RecorderManagerBaseCfg, RecorderTerm, RecorderTermCfg from isaaclab.utils import configclass @@ -41,6 +40,13 @@ class PreStepFlatPolicyObservationsRecorderCfg(RecorderTermCfg): class_type: type[RecorderTerm] = recorders.PreStepFlatPolicyObservationsRecorder +@configclass +class PostStepProcessedActionsRecorderCfg(RecorderTermCfg): + """Configuration for the post step processed actions recorder term.""" + + class_type: type[RecorderTerm] = recorders.PostStepProcessedActionsRecorder + + ## # Recorder manager configurations. ## @@ -54,3 +60,4 @@ class ActionStateRecorderManagerCfg(RecorderManagerBaseCfg): record_post_step_states = PostStepStatesRecorderCfg() record_pre_step_actions = PreStepActionsRecorderCfg() record_pre_step_flat_policy_observations = PreStepFlatPolicyObservationsRecorderCfg() + record_post_step_processed_actions = PostStepProcessedActionsRecorderCfg() diff --git a/source/isaaclab/isaaclab/envs/mdp/rewards.py b/source/isaaclab/isaaclab/envs/mdp/rewards.py index b1601ec44af6..774c37aefa91 100644 --- a/source/isaaclab/isaaclab/envs/mdp/rewards.py +++ b/source/isaaclab/isaaclab/envs/mdp/rewards.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,9 +11,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation, RigidObject from isaaclab.managers import SceneEntityCfg from isaaclab.managers.manager_base import ManagerTermBase @@ -136,7 +137,9 @@ def body_lin_acc_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEnt 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. + .. 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] @@ -153,7 +156,9 @@ def joint_vel_l1(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Ten 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. + .. 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] @@ -163,7 +168,9 @@ def joint_vel_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntity 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. + .. 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] @@ -268,6 +275,16 @@ def undesired_contacts(env: ManagerBasedRLEnv, threshold: float, sensor_cfg: Sce return torch.sum(is_contact, dim=1) +def desired_contacts(env, sensor_cfg: SceneEntityCfg, threshold: float = 1.0) -> torch.Tensor: + """Penalize if none of the desired contacts are present.""" + 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] > threshold + ) + zero_contact = (~contacts).all(dim=1) + return 1.0 * zero_contact + + 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) diff --git a/source/isaaclab/isaaclab/envs/mdp/terminations.py b/source/isaaclab/isaaclab/envs/mdp/terminations.py index 20eadf6417a5..84c83b2a213e 100644 --- a/source/isaaclab/isaaclab/envs/mdp/terminations.py +++ b/source/isaaclab/isaaclab/envs/mdp/terminations.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,9 +11,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation, RigidObject from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import ContactSensor @@ -81,10 +82,13 @@ def joint_pos_out_of_limit(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = S """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]) + if asset_cfg.joint_ids is None: + asset_cfg.joint_ids = slice(None) + + limits = asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids] + out_of_upper_limits = torch.any(asset.data.joint_pos[:, asset_cfg.joint_ids] > limits[..., 1], dim=1) + out_of_lower_limits = torch.any(asset.data.joint_pos[:, asset_cfg.joint_ids] < limits[..., 0], dim=1) + return torch.logical_or(out_of_upper_limits, out_of_lower_limits) def joint_pos_out_of_manual_limit( @@ -131,12 +135,12 @@ def joint_effort_out_of_limit( 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. + torques. If they are not, it means that clipping has occurred. """ # 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( + 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) diff --git a/source/isaaclab/isaaclab/envs/mimic_env_cfg.py b/source/isaaclab/isaaclab/envs/mimic_env_cfg.py index 0e19613113e0..c506df7f20bf 100644 --- a/source/isaaclab/isaaclab/envs/mimic_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/mimic_env_cfg.py @@ -1,8 +1,7 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - # Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the NVIDIA Source Code License [see LICENSE for details]. @@ -10,6 +9,10 @@ """ Base MimicEnvCfg object for Isaac Lab Mimic data generation. """ + +import enum + +from isaaclab.managers.recorder_manager import RecorderManagerBaseCfg from isaaclab.utils import configclass @@ -17,119 +20,281 @@ class DataGenConfig: """Configuration settings for data generation processes within the Isaac Lab Mimic environment.""" - # The name of the datageneration, default is "demo" name: str = "demo" + """The name of the data generation process. Defaults to "demo".""" - # If set to True, generation will be retried until - # generation_num_trials successful demos have been generated. - # If set to False, generation will stop after generation_num_trails, - # independent of whether they were all successful or not. generation_guarantee: bool = True + """Whether to retry generation until generation_num_trials successful demos have been generated. - ############################################################## - # Debugging parameters, which can help determining low success - # rates. + 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. + """ - # Whether to keep failed generation trials. Keeping failed - # demonstrations is useful for visualizing and debugging low - # success rates. generation_keep_failed: bool = False + """Whether to keep failed generation trials. + + Keeping failed demonstrations is useful for visualizing and debugging low success rates. + """ - # Maximum number of failures allowed before stopping generation max_num_failures: int = 50 + """Maximum number of failures allowed before stopping generation.""" - # Seed for randomization to ensure reproducibility seed: int = 1 + """Seed for randomization to ensure reproducibility.""" - ############################################################## - # The following values can be changed on the command line, and - # only serve as defaults. + """The following configuration values can be changed on the command line, and only serve as defaults.""" - # Path to the source dataset for mimic generation source_dataset_path: str = None + """Path to the source dataset for mimic generation.""" - # Path where the generated data will be saved generation_path: str = None + """Path where the generated data will be saved.""" - # Number of trial to be generated generation_num_trials: int = 10 + """Number of trials to be generated.""" - # Name of the task being configured task_name: str = None + """Name of the task being configured.""" - ############################################################## - # Advanced configuration, does not usually need to be changed + """The following configurations are advanced and do not usually need to be changed.""" - # 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_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.""" - # Whether to transform the first robot pose during generation generation_transform_first_robot_pose: bool = False + """Whether to transform the first robot pose during generation.""" - # Whether to interpolate from last target pose generation_interpolate_from_last_target_pose: bool = True + """Whether to interpolate from last target pose.""" + + use_skillgen: bool = False + """Whether to use skillgen to generate motion trajectories.""" + + use_navigation_controller: bool = False + """Whether to use a navigation controller to generate loco-manipulation trajectories.""" @configclass class SubTaskConfig: """ - Configuration settings specific to the management of individual - subtasks. + Configuration settings for specifying subtasks used in Mimic environments. """ - ############################################################## - # Mandatory options that should be defined for every subtask + """Mandatory options that should be defined for every subtask.""" - # Reference to the object involved in this subtask, None if no - # object is involved (this is rarely the case). 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). + """ - # Signal for subtask termination subtask_term_signal: str = None + """Subtask termination signal name.""" + + """Advanced options for tuning the generation results.""" - ############################################################## - # Advanced options for tuning the generation results - - # Strategy on how to select a subtask segment. Can be either - # 'random', 'nearest_neighbor_object' or - # 'nearest_neighbor_robot_distance'. Details can be found in - # source/isaaclab_mimic/isaaclab_mimic/datagen/selection_strategy.py - # - # Note: for 'nearest_neighbor_object' and - # 'nearest_neighbor_robot_distance', the subtask needs to have - # 'object_ref' set to a value other than 'None' above. At the - # same time, if 'object_ref' is not 'None', then either of - # those strategies will usually yield higher success rates - # than the default 'random' strategy. 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. + """ - # 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. 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.""" - # Range for start offset of the first subtask first_subtask_start_offset_range: tuple = (0, 0) + """Range for start offset of the first subtask.""" + + subtask_start_offset_range: tuple = (0, 0) + """Range for start offset of the subtask (only used if use_skillgen is True) + + Note: This value overrides the first_subtask_start_offset_range when skillgen is enabled + """ - # Range for offsetting subtask termination subtask_term_offset_range: tuple = (0, 0) + """Range for offsetting subtask termination.""" - # Amplitude of action noise applied action_noise: float = 0.03 + """Amplitude of action noise applied.""" - # Number of steps for interpolation between waypoints num_interpolation_steps: int = 5 + """Number of steps for interpolation between waypoints.""" - # Number of fixed steps for the subtask num_fixed_steps: int = 0 + """Number of fixed steps for the subtask.""" - # Whether to apply noise during interpolation 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 @@ -146,6 +311,10 @@ class MimicEnvCfg: # Dictionary of list of subtask configurations for each end-effector. # Keys are end-effector names. - # Currently, only a single end-effector is supported by Isaac Lab Mimic - # so `subtask_configs` must always be of size 1. subtask_configs: dict[str, list[SubTaskConfig]] = {} + + # List of configurations for subtask constraints + task_constraint_configs: list[SubTaskConstraintConfig] = [] + + # Optional recorder configuration + mimic_recorder_config: RecorderManagerBaseCfg | None = None diff --git a/source/isaaclab/isaaclab/envs/ui/__init__.py b/source/isaaclab/isaaclab/envs/ui/__init__.py index 3fd8fbff5fc1..93db88399e45 100644 --- a/source/isaaclab/isaaclab/envs/ui/__init__.py +++ b/source/isaaclab/isaaclab/envs/ui/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,5 +11,6 @@ """ 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/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index 5914c3926c2e..2aafe5e6bba2 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -17,6 +17,7 @@ import omni.usd from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics +from isaaclab.sim.utils.stage import get_current_stage from isaaclab.ui.widgets import ManagerLiveVisualizer if TYPE_CHECKING: @@ -60,6 +61,9 @@ def __init__(self, env: ManagerBasedEnv, window_name: str = "IsaacLab"): *self.env.scene.articulations.keys(), ] + # get stage handle + self.stage = get_current_stage() + # Listeners for environment selection changes self._ui_listeners: list[ManagerLiveVisualizer] = [] @@ -300,8 +304,7 @@ def _toggle_recording_animation_fn(self, value: bool): # stop the recording _ = omni.kit.commands.execute("StopRecording") # save the current stage - stage = omni.usd.get_context().get_stage() - source_layer = stage.GetRootLayer() + source_layer = self.stage.GetRootLayer() # output the stage to a file stage_usd_path = os.path.join(self.animation_log_dir, "Stage.usd") source_prim_path = "/" @@ -311,8 +314,8 @@ def _toggle_recording_animation_fn(self, value: bool): 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)) + UsdGeom.SetStageUpAxis(temp_stage, UsdGeom.GetStageUpAxis(self.stage)) + UsdGeom.SetStageMetersPerUnit(temp_stage, UsdGeom.GetStageMetersPerUnit(self.stage)) # copy the prim Sdf.CreatePrimInLayer(temp_layer, source_prim_path) Sdf.CopySpec(source_layer, source_prim_path, temp_layer, source_prim_path) diff --git a/source/isaaclab/isaaclab/envs/ui/empty_window.py b/source/isaaclab/isaaclab/envs/ui/empty_window.py new file mode 100644 index 000000000000..bc12f862d1b0 --- /dev/null +++ b/source/isaaclab/isaaclab/envs/ui/empty_window.py @@ -0,0 +1,79 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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/source/isaaclab/isaaclab/envs/ui/manager_based_rl_env_window.py b/source/isaaclab/isaaclab/envs/ui/manager_based_rl_env_window.py index 77b2d9ed4860..72f0be06f199 100644 --- a/source/isaaclab/isaaclab/envs/ui/manager_based_rl_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/manager_based_rl_env_window.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py index f6bc6bb485ba..0ba5368734b0 100644 --- a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py +++ b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,12 +6,13 @@ 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 numpy as np +import torch + import omni.kit.app import omni.timeline @@ -52,8 +53,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 +208,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/envs/utils/__init__.py b/source/isaaclab/isaaclab/envs/utils/__init__.py index 42055468bcb2..d28381b15b76 100644 --- a/source/isaaclab/isaaclab/envs/utils/__init__.py +++ b/source/isaaclab/isaaclab/envs/utils/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/envs/utils/io_descriptors.py b/source/isaaclab/isaaclab/envs/utils/io_descriptors.py new file mode 100644 index 000000000000..84b8a5cf8a0c --- /dev/null +++ b/source/isaaclab/isaaclab/envs/utils/io_descriptors.py @@ -0,0 +1,393 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Callable +from typing import TYPE_CHECKING, Any, Concatenate, ParamSpec, TypeVar + +from isaaclab.utils import configclass + +if TYPE_CHECKING: + import torch + + from isaaclab.assets.articulation import Articulation + from isaaclab.envs import ManagerBasedEnv + +import dataclasses +import functools +import inspect + + +@configclass +class GenericActionIODescriptor: + """Generic action IO descriptor. + + This descriptor is used to describe the action space of a policy. + It can be extended as needed to add more information about the action term that is being described. + """ + + mdp_type: str = "Action" + """The type of MDP that the action term belongs to.""" + + name: str = None + """The name of the action term. + + By default, the name of the action term class is used. + """ + + full_path: str = None + """The full path of the action term class. + + By default, python's will retrieve the path from the file that the action term class is defined in + and the name of the action term class. + """ + + description: str = None + """The description of the action term. + + By default, the docstring of the action term class is used. + """ + + shape: tuple[int, ...] = None + """The shape of the action term. + + This should be populated by the user.""" + + dtype: str = None + """The dtype of the action term. + + This should be populated by the user.""" + + action_type: str = None + """The type of the action term. + + This attribute is purely informative and should be populated by the user.""" + + extras: dict[str, Any] = {} + """Extra information about the action term. + + This attribute is purely informative and should be populated by the user.""" + + export: bool = True + """Whether to export the action term. + + Should be set to False if the class is not meant to be exported. + """ + + +@configclass +class GenericObservationIODescriptor: + """Generic observation IO descriptor. + + This descriptor is used to describe the observation space of a policy. + It can be extended as needed to add more information about the observation term that is being described. + """ + + mdp_type: str = "Observation" + name: str = None + full_path: str = None + description: str = None + shape: tuple[int, ...] = None + dtype: str = None + observation_type: str = None + extras: dict[str, Any] = {} + + +# These are defined to help with type hinting +P = ParamSpec("P") +R = TypeVar("R") + + +# Automatically builds a descriptor from the kwargs +def _make_descriptor(**kwargs: Any) -> GenericObservationIODescriptor: + """Split *kwargs* into (known dataclass fields) and (extras).""" + field_names = {f.name for f in dataclasses.fields(GenericObservationIODescriptor)} + known = {k: v for k, v in kwargs.items() if k in field_names} + extras = {k: v for k, v in kwargs.items() if k not in field_names} + + desc = GenericObservationIODescriptor(**known) + # User defined extras are stored in the descriptor under the `extras` field + desc.extras = extras + return desc + + +# Decorator factory for generic IO descriptors. +def generic_io_descriptor( + _func: Callable[Concatenate[ManagerBasedEnv, P], R] | None = None, + *, + on_inspect: Callable[..., Any] | list[Callable[..., Any]] | None = None, + **descriptor_kwargs: Any, +) -> Callable[[Callable[Concatenate[ManagerBasedEnv, P], R]], Callable[Concatenate[ManagerBasedEnv, P], R]]: + """Decorator factory for generic IO descriptors. + + This decorator can be used in different ways: + + 1. The default decorator has all the information I need for my use case: + + ..code-block:: python + @generic_io_descriptor(GenericIODescriptor(description="..", dtype="..")) + def my_func(env: ManagerBasedEnv, *args, **kwargs): + ... + + ..note:: If description is not set, the function's docstring is used to populate it. + + 2. I need to add more information to the descriptor: + + ..code-block:: python + @generic_io_descriptor(description="..", new_var_1="a", new_var_2="b") + def my_func(env: ManagerBasedEnv, *args, **kwargs): + ... + + 3. I need to add a hook to the descriptor: + + ..code-block:: python + def record_shape(tensor: torch.Tensor, desc: GenericIODescriptor, **kwargs): + desc.shape = (tensor.shape[-1],) + + @generic_io_descriptor(description="..", new_var_1="a", new_var_2="b", on_inspect=[record_shape, record_dtype]) + def my_func(env: ManagerBasedEnv, *args, **kwargs): + ... + + ..note:: + + The hook is called after the function is called, if and only if the `inspect` flag is set when + calling the function. + + For example: + + ..code-block:: python + my_func(env, inspect=True) + + 4. I need to add a hook to the descriptor and this hook will write to a variable that is not part of + the base descriptor. + + ..code-block:: python + + def record_joint_names(output: torch.Tensor, descriptor: GenericIODescriptor, **kwargs): + asset: Articulation = kwargs["env"].scene[kwargs["asset_cfg"].name] + joint_ids = kwargs["asset_cfg"].joint_ids + if joint_ids == slice(None, None, None): + joint_ids = list(range(len(asset.joint_names))) + descriptor.joint_names = [asset.joint_names[i] for i in joint_ids] + + @generic_io_descriptor( + new_var_1="a", + new_var_2="b", + on_inspect=[record_shape, record_dtype, record_joint_names], + ) + def my_func(env: ManagerBasedEnv, *args, **kwargs): + ... + + ..note:: + + The hook can access all the variables in the wrapped function's signature. While it is useful, + the user should be careful to access only existing variables. + + Args: + _func: The function to decorate. + **descriptor_kwargs: Keyword arguments to pass to the descriptor. + + Returns: + A decorator that can be used to decorate a function. + """ + # If the decorator is used with a descriptor, use it as the descriptor. + if _func is not None and isinstance(_func, GenericObservationIODescriptor): + descriptor = _func + _func = None + else: + descriptor = _make_descriptor(**descriptor_kwargs) + + # Ensures the hook is a list + if callable(on_inspect): + inspect_hooks: list[Callable[..., Any]] = [on_inspect] + else: + inspect_hooks: list[Callable[..., Any]] = list(on_inspect or []) # handles None + + def _apply(func: Callable[Concatenate[ManagerBasedEnv, P], R]) -> Callable[Concatenate[ManagerBasedEnv, P], R]: + # Capture the signature of the function + sig = inspect.signature(func) + + @functools.wraps(func) + def wrapper(env: ManagerBasedEnv, *args: P.args, **kwargs: P.kwargs) -> R: + inspect_flag: bool = kwargs.pop("inspect", False) + out = func(env, *args, **kwargs) + if inspect_flag: + # Injects the function's arguments into the hooks and applies the defaults + bound = sig.bind(env, *args, **kwargs) + bound.apply_defaults() + call_kwargs = { + "output": out, + "descriptor": descriptor, + **bound.arguments, + } + for hook in inspect_hooks: + hook(**call_kwargs) + return out + + # --- Descriptor bookkeeping --- + descriptor.name = func.__name__ + descriptor.full_path = f"{func.__module__}.{func.__name__}" + descriptor.dtype = str(descriptor.dtype) + # Check if description is set in the descriptor + if descriptor.description is None: + descriptor.description = " ".join(func.__doc__.split()) + + # Adds the descriptor to the wrapped function as an attribute + wrapper._descriptor = descriptor + wrapper._has_descriptor = True + # Alters the signature of the wrapped function to make it match the original function. + # This allows the wrapped functions to pass the checks in the managers. + wrapper.__signature__ = sig + return wrapper + + # If the decorator is used without parentheses, _func will be the function itself. + if callable(_func): + return _apply(_func) + return _apply + + +def record_shape(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs) -> None: + """Record the shape of the output tensor. + + Args: + output: The output tensor. + descriptor: The descriptor to record the shape to. + **kwargs: Additional keyword arguments. + """ + descriptor.shape = (output.shape[-1],) + + +def record_dtype(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs) -> None: + """Record the dtype of the output tensor. + + Args: + output: The output tensor. + descriptor: The descriptor to record the dtype to. + **kwargs: Additional keyword arguments. + """ + descriptor.dtype = str(output.dtype) + + +def record_joint_names(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs) -> None: + """Record the joint names of the output tensor. + + Expects the `asset_cfg` keyword argument to be set. + + Args: + output: The output tensor. + descriptor: The descriptor to record the joint names to. + **kwargs: Additional keyword arguments. + """ + asset: Articulation = kwargs["env"].scene[kwargs["asset_cfg"].name] + joint_ids = kwargs["asset_cfg"].joint_ids + if joint_ids == slice(None, None, None): + joint_ids = list(range(len(asset.joint_names))) + descriptor.joint_names = [asset.joint_names[i] for i in joint_ids] + + +def record_body_names(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs) -> None: + """Record the body names of the output tensor. + + Expects the `asset_cfg` keyword argument to be set. + + Args: + output: The output tensor. + descriptor: The descriptor to record the body names to. + **kwargs: Additional keyword arguments. + """ + asset: Articulation = kwargs["env"].scene[kwargs["asset_cfg"].name] + body_ids = kwargs["asset_cfg"].body_ids + if body_ids == slice(None, None, None): + body_ids = list(range(len(asset.body_names))) + descriptor.body_names = [asset.body_names[i] for i in body_ids] + + +def record_joint_pos_offsets(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs): + """Record the joint position offsets of the output tensor. + + Expects the `asset_cfg` keyword argument to be set. + + Args: + output: The output tensor. + descriptor: The descriptor to record the joint position offsets to. + **kwargs: Additional keyword arguments. + """ + asset: Articulation = kwargs["env"].scene[kwargs["asset_cfg"].name] + ids = kwargs["asset_cfg"].joint_ids + # Get the offsets of the joints for the first robot in the scene. + # This assumes that all robots have the same joint offsets. + descriptor.joint_pos_offsets = asset.data.default_joint_pos[:, ids][0] + + +def record_joint_vel_offsets(output: torch.Tensor, descriptor: GenericObservationIODescriptor, **kwargs): + """Record the joint velocity offsets of the output tensor. + + Expects the `asset_cfg` keyword argument to be set. + + Args: + output: The output tensor. + descriptor: The descriptor to record the joint velocity offsets to. + **kwargs: Additional keyword arguments. + """ + asset: Articulation = kwargs["env"].scene[kwargs["asset_cfg"].name] + ids = kwargs["asset_cfg"].joint_ids + # Get the offsets of the joints for the first robot in the scene. + # This assumes that all robots have the same joint offsets. + descriptor.joint_vel_offsets = asset.data.default_joint_vel[:, ids][0] + + +def export_articulations_data(env: ManagerBasedEnv) -> dict[str, dict[str, list[float]]]: + """Export the articulations data. + + Args: + env: The environment. + + Returns: + A dictionary containing the articulations data. + """ + # Create a dictionary for all the articulations in the scene. + articulation_joint_data = {} + for articulation_name, articulation in env.scene.articulations.items(): + # For each articulation, create a dictionary with the articulation's data. + # Some of the data may be redundant with other information provided by the observation descriptors. + articulation_joint_data[articulation_name] = {} + articulation_joint_data[articulation_name]["joint_names"] = articulation.joint_names + articulation_joint_data[articulation_name]["default_joint_pos"] = ( + articulation.data.default_joint_pos[0].detach().cpu().numpy().tolist() + ) + articulation_joint_data[articulation_name]["default_joint_vel"] = ( + articulation.data.default_joint_vel[0].detach().cpu().numpy().tolist() + ) + articulation_joint_data[articulation_name]["default_joint_pos_limits"] = ( + articulation.data.default_joint_pos_limits[0].detach().cpu().numpy().tolist() + ) + articulation_joint_data[articulation_name]["default_joint_damping"] = ( + articulation.data.default_joint_damping[0].detach().cpu().numpy().tolist() + ) + articulation_joint_data[articulation_name]["default_joint_stiffness"] = ( + articulation.data.default_joint_stiffness[0].detach().cpu().numpy().tolist() + ) + articulation_joint_data[articulation_name]["default_joint_friction"] = ( + articulation.data.default_joint_friction[0].detach().cpu().numpy().tolist() + ) + articulation_joint_data[articulation_name]["default_joint_armature"] = ( + articulation.data.default_joint_armature[0].detach().cpu().numpy().tolist() + ) + return articulation_joint_data + + +def export_scene_data(env: ManagerBasedEnv) -> dict[str, Any]: + """Export the scene data. + + Args: + env: The environment. + + Returns: + A dictionary containing the scene data. + """ + # Create a dictionary for the scene data. + scene_data = {"physics_dt": env.physics_dt, "dt": env.step_dt, "decimation": env.cfg.decimation} + return scene_data diff --git a/source/isaaclab/isaaclab/envs/utils/marl.py b/source/isaaclab/isaaclab/envs/utils/marl.py index 3fac404a865a..010f6e5e27bd 100644 --- a/source/isaaclab/isaaclab/envs/utils/marl.py +++ b/source/isaaclab/isaaclab/envs/utils/marl.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import gymnasium as gym import math +from typing import Any + +import gymnasium as gym 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 @@ -17,8 +18,8 @@ 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 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, diff --git a/source/isaaclab/isaaclab/envs/utils/spaces.py b/source/isaaclab/isaaclab/envs/utils/spaces.py index 4efdc8ca8c33..a21ecd82667c 100644 --- a/source/isaaclab/isaaclab/envs/utils/spaces.py +++ b/source/isaaclab/isaaclab/envs/utils/spaces.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import gymnasium as gym import json +from typing import Any + +import gymnasium as gym import numpy as np import torch -from typing import Any from ..common import SpaceType @@ -54,14 +55,16 @@ def sample_space(space: gym.spaces.Space, device: str, batch_size: int = -1, fil 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. + 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): + def tensorize(s: gym.spaces.Space, x: Any) -> Any: 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: @@ -89,6 +92,9 @@ def tensorize(s, x): elif isinstance(s, gym.spaces.Tuple): return tuple([tensorize(_s, v) for _s, v in zip(s, x)]) + # If the space is not supported, raise an error + raise ValueError(f"Unsupported Gymnasium space for tensorization: {s}") + sample = (gym.vector.utils.batch_space(space, batch_size) if batch_size > 0 else space).sample() return tensorize(space, sample) @@ -106,13 +112,15 @@ def serialize_space(space: SpaceType) -> str: 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, - }) + 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): diff --git a/source/isaaclab/isaaclab/managers/__init__.py b/source/isaaclab/isaaclab/managers/__init__.py index b3d241e8ea31..4a8266801b47 100644 --- a/source/isaaclab/isaaclab/managers/__init__.py +++ b/source/isaaclab/isaaclab/managers/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/managers/action_manager.py b/source/isaaclab/isaaclab/managers/action_manager.py index 11fe7766fed4..6d138451f998 100644 --- a/source/isaaclab/isaaclab/managers/action_manager.py +++ b/source/isaaclab/isaaclab/managers/action_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,21 +8,24 @@ from __future__ import annotations import inspect -import torch +import re import weakref from abc import abstractmethod from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import torch from prettytable import PrettyTable -from typing import TYPE_CHECKING import omni.kit.app -from isaaclab.assets import AssetBase +from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor from .manager_base import ManagerBase, ManagerTermBase from .manager_term_cfg import ActionTermCfg if TYPE_CHECKING: + from isaaclab.assets import AssetBase from isaaclab.envs import ManagerBasedEnv @@ -50,6 +53,8 @@ def __init__(self, cfg: ActionTermCfg, env: ManagerBasedEnv): 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] + self._IO_descriptor = GenericActionIODescriptor() + self._export_IO_descriptor = True # add handle for debug visualization (this is set to a valid handle inside set_debug_vis) self._debug_vis_handle = None @@ -91,6 +96,20 @@ def has_debug_vis_implementation(self) -> bool: source_code = inspect.getsource(self._set_debug_vis_impl) return "NotImplementedError" not in source_code + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor for the action term.""" + self._IO_descriptor.name = re.sub(r"([a-z])([A-Z])", r"\1_\2", self.__class__.__name__).lower() + self._IO_descriptor.full_path = f"{self.__class__.__module__}.{self.__class__.__name__}" + self._IO_descriptor.description = " ".join(self.__class__.__doc__.split()) + self._IO_descriptor.export = self.export_IO_descriptor + return self._IO_descriptor + + @property + def export_IO_descriptor(self) -> bool: + """Whether to export the IO descriptor for the action term.""" + return self._export_IO_descriptor + """ Operations. """ @@ -259,6 +278,41 @@ def has_debug_vis_implementation(self) -> bool: has_debug_vis |= term.has_debug_vis_implementation return has_debug_vis + @property + def get_IO_descriptors(self) -> list[dict[str, Any]]: + """Get the IO descriptors for the action manager. + + Returns: + A dictionary with keys as the term names and values as the IO descriptors. + """ + + data = [] + + for term_name, term in self._terms.items(): + try: + data.append(term.IO_descriptor.__dict__.copy()) + except Exception as e: + print(f"Error getting IO descriptor for term '{term_name}': {e}") + + formatted_data = [] + for item in data: + name = item.pop("name") + formatted_item = {"name": name, "extras": item.pop("extras")} + print(item["export"]) + if not item.pop("export"): + continue + for k, v in item.items(): + # Check if v is a tuple and convert to list + if isinstance(v, tuple): + v = list(v) + if k in ["description", "units"]: + formatted_item["extras"][k] = v + else: + formatted_item[k] = v + formatted_data.append(formatted_item) + + return formatted_data + """ Operations. """ @@ -358,6 +412,14 @@ def get_term(self, name: str) -> ActionTerm: """ 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. """ diff --git a/source/isaaclab/isaaclab/managers/command_manager.py b/source/isaaclab/isaaclab/managers/command_manager.py index 3821a5c8bbda..0fe66ff6b963 100644 --- a/source/isaaclab/isaaclab/managers/command_manager.py +++ b/source/isaaclab/isaaclab/managers/command_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,13 +8,14 @@ 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 torch +from prettytable import PrettyTable + import omni.kit.app from .manager_base import ManagerBase, ManagerTermBase @@ -181,10 +182,10 @@ def _resample(self, env_ids: Sequence[int]): 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) - # increment the command counter - self.command_counter[env_ids] += 1 # resample the command self._resample_command(env_ids) + # increment the command counter + self.command_counter[env_ids] += 1 """ Implementation specific functions. diff --git a/source/isaaclab/isaaclab/managers/curriculum_manager.py b/source/isaaclab/isaaclab/managers/curriculum_manager.py index 946bc6ab9dda..5354641d9e7e 100644 --- a/source/isaaclab/isaaclab/managers/curriculum_manager.py +++ b/source/isaaclab/isaaclab/managers/curriculum_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,12 @@ from __future__ import annotations -import torch from collections.abc import Sequence -from prettytable import PrettyTable from typing import TYPE_CHECKING +import torch +from prettytable import PrettyTable + from .manager_base import ManagerBase, ManagerTermBase from .manager_term_cfg import CurriculumTermCfg diff --git a/source/isaaclab/isaaclab/managers/event_manager.py b/source/isaaclab/isaaclab/managers/event_manager.py index 17f76b8e87e6..8f92d6859c1e 100644 --- a/source/isaaclab/isaaclab/managers/event_manager.py +++ b/source/isaaclab/isaaclab/managers/event_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,19 +7,23 @@ from __future__ import annotations -import torch +import inspect +import logging from collections.abc import Sequence -from prettytable import PrettyTable from typing import TYPE_CHECKING -import omni.log +import torch +from prettytable import PrettyTable -from .manager_base import ManagerBase, ManagerTermBase +from .manager_base import ManagerBase from .manager_term_cfg import EventTermCfg if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv +# import logger +logger = logging.getLogger(__name__) + class EventManager(ManagerBase): """Manager for orchestrating operations based on different simulation events. @@ -133,8 +137,8 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: # 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_class_term_cfgs: - for index, term_cfg in enumerate(self._mode_class_term_cfgs["interval"]): + if "interval" in self._mode_term_cfgs: + for index, term_cfg in enumerate(self._mode_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 @@ -184,17 +188,8 @@ def apply( """ # 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.") + logger.warning(f"Event mode '{mode}' is not defined. Skipping event.") return - # check if mode is pre-startup and scene replication is enabled - if mode == "prestartup" and self._env.scene.cfg.replicate_physics: - omni.log.warn( - "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, consider disabling scene replication" - " by setting 'replicate_physics' to False in 'InteractiveSceneCfg'." - ) # check if mode is interval and dt is not provided if mode == "interval" and dt is None: @@ -356,13 +351,31 @@ def _prepare_terms(self): ) if term_cfg.mode != "reset" and term_cfg.min_step_count_between_reset != 0: - omni.log.warn( + logger.warning( 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": + logger.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 @@ -374,7 +387,7 @@ def _prepare_terms(self): self._mode_term_cfgs[term_cfg.mode].append(term_cfg) # check if the term is a class - if isinstance(term_cfg.func, ManagerTermBase): + if inspect.isclass(term_cfg.func): self._mode_class_term_cfgs[term_cfg.mode].append(term_cfg) # resolve the mode of the events diff --git a/source/isaaclab/isaaclab/managers/manager_base.py b/source/isaaclab/isaaclab/managers/manager_base.py index c734bbc836ae..158c713abfa3 100644 --- a/source/isaaclab/isaaclab/managers/manager_base.py +++ b/source/isaaclab/isaaclab/managers/manager_base.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,16 +7,16 @@ import copy import inspect +import logging 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 string_to_callable +from isaaclab.utils import class_to_dict, string_to_callable from .manager_term_cfg import ManagerTermBaseCfg from .scene_entity_cfg import SceneEntityCfg @@ -24,6 +24,9 @@ if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv +# import logger +logger = logging.getLogger(__name__) + class ManagerTermBase(ABC): """Base class for manager terms. @@ -42,13 +45,14 @@ class should also have a corresponding configuration class that defines the conf 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) @@ -79,6 +83,11 @@ 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. """ @@ -92,6 +101,10 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: """ 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. @@ -112,7 +125,7 @@ def __call__(self, *args) -> Any: Returns: The value of the term. """ - raise NotImplementedError + raise NotImplementedError("The method '__call__' should be implemented by the subclass.") class ManagerBase(ABC): @@ -136,32 +149,38 @@ def __init__(self, cfg: object, env: ManagerBasedEnv): self.cfg = copy.deepcopy(cfg) self._env = env - # parse config to create terms information - if self.cfg: - self._prepare_terms() + # 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_scene_entities_handle = timeline_event_stream.create_subscription_to_pop_by_type( + 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_scene_entities_callback(event), + lambda event, obj=weakref.proxy(self): obj._resolve_terms_callback(event), order=20, ) else: - self._resolve_scene_entities_handle = None + 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_scene_entities_handle: - self._resolve_scene_entities_handle.unsubscribe() - self._resolve_scene_entities_handle = None + if self._resolve_terms_handle: + self._resolve_terms_handle.unsubscribe() + self._resolve_terms_handle = None """ Properties. @@ -206,7 +225,7 @@ def find_terms(self, name_keys: str | Sequence[str]) -> list[str]: 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 + Please check the :meth:`~isaaclab.utils.string_utils.resolve_matching_names` function for more information on the name matching. Args: @@ -249,12 +268,14 @@ def _prepare_terms(self): Internal callbacks. """ - def _resolve_scene_entities_callback(self, event): - """Resolve the scene entities configuration. + def _resolve_terms_callback(self, event): + """Resolve configurations of terms once the simulation starts. - This callback is called when the simulation starts. It is used to resolve the - scene entities configuration for the terms. + 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() @@ -266,17 +287,29 @@ def _resolve_scene_entities_callback(self, event): # check for non config if term_cfg is None: continue - # resolve the scene entity configuration - self._resolve_scene_entity_cfg(term_name, term_cfg) + # 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 """ - Helper functions. + Internal functions. """ def _resolve_common_term_cfg(self, term_name: str, term_cfg: ManagerTermBaseCfg, min_argc: int = 1): - """Resolve common term configuration. + """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. - Usually, called by the :meth:`_prepare_terms` method to resolve common term configuration. + 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 @@ -303,29 +336,31 @@ def _resolve_common_term_cfg(self, term_name: str, term_cfg: ManagerTermBaseCfg, f" Received: '{type(term_cfg)}'." ) - # iterate over all the entities and parse the joint and body names - if self._env.sim.is_playing(): - self._resolve_scene_entity_cfg(term_name, 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}") - # initialize the term if it is a class + # 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)}'." ) - term_cfg.func = term_cfg.func(cfg=term_cfg, env=self._env) + 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(term_cfg.func): + if not callable(func_static): raise AttributeError(f"The term '{term_name}' is not callable. Received: {term_cfg.func}") - # check if term's arguments are matched by params + # check statically if the term's arguments are matched by params term_params = list(term_cfg.params.keys()) - args = inspect.signature(term_cfg.func).parameters + 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 @@ -338,8 +373,22 @@ def _resolve_common_term_cfg(self, term_name: str, term_cfg: ManagerTermBaseCfg, f" and optional parameters: {args_with_defaults}, but received: {term_params}." ) - def _resolve_scene_entity_cfg(self, term_name: str, term_cfg: ManagerTermBaseCfg): - """Resolve the scene entity configuration for the term. + # 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. @@ -359,6 +408,11 @@ def _resolve_scene_entity_cfg(self, term_name: str, term_cfg: ManagerTermBaseCfg 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) + logger.info(msg) # store the entity term_cfg.params[key] = value + + # initialize the term if it is a class + if inspect.isclass(term_cfg.func): + logger.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/source/isaaclab/isaaclab/managers/manager_term_cfg.py b/source/isaaclab/isaaclab/managers/manager_term_cfg.py index 199409c2e222..de7c23aa220b 100644 --- a/source/isaaclab/isaaclab/managers/manager_term_cfg.py +++ b/source/isaaclab/isaaclab/managers/manager_term_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,14 +7,15 @@ from __future__ import annotations -import torch from collections.abc import Callable from dataclasses import MISSING from typing import TYPE_CHECKING, Any +import torch + from isaaclab.utils import configclass from isaaclab.utils.modifiers import ModifierCfg -from isaaclab.utils.noise import NoiseCfg +from isaaclab.utils.noise import NoiseCfg, NoiseModelCfg from .scene_entity_cfg import SceneEntityCfg @@ -64,7 +65,7 @@ class RecorderTermCfg: class_type: type[RecorderTerm] = MISSING """The associated recorder term class. - The class should inherit from :class:`isaaclab.managers.action_manager.RecorderTerm`. + The class should inherit from :class:`isaaclab.managers.recorder_manager.RecorderTerm`. """ @@ -165,7 +166,7 @@ class ObservationTermCfg(ManagerTermBaseCfg): For more information on modifiers, see the :class:`~isaaclab.utils.modifiers.ModifierCfg` class. """ - noise: NoiseCfg | None = None + noise: NoiseCfg | NoiseModelCfg | 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 @@ -183,14 +184,14 @@ class ObservationTermCfg(ManagerTermBaseCfg): 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. + 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 2-D 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. + """Whether or not the observation manager should flatten history-based observation terms to a 2-D (N, D) tensor. Defaults to True.""" @@ -201,12 +202,22 @@ class ObservationGroupCfg: 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 last dimension. - Otherwise, they are kept separate and returned as a dictionary. + 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 2-D 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. @@ -217,13 +228,13 @@ class ObservationGroupCfg: 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. + 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 :attr:`ObservationTermCfg.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. + """Flag to flatten history-based observation terms to a 2-D (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 diff --git a/source/isaaclab/isaaclab/managers/observation_manager.py b/source/isaaclab/isaaclab/managers/observation_manager.py index a235b0d7c1e9..a1bde0266f4b 100644 --- a/source/isaaclab/isaaclab/managers/observation_manager.py +++ b/source/isaaclab/isaaclab/managers/observation_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,13 +8,14 @@ from __future__ import annotations import inspect +from collections.abc import Sequence +from typing import TYPE_CHECKING + import numpy as np import torch -from collections.abc import Sequence from prettytable import PrettyTable -from typing import TYPE_CHECKING -from isaaclab.utils import modifiers +from isaaclab.utils import class_to_dict, modifiers, noise from isaaclab.utils.buffers import CircularBuffer from .manager_base import ManagerBase, ManagerTermBase @@ -88,8 +89,18 @@ def __init__(self, cfg: object, env: ManagerBasedEnv): # otherwise, keep the list of shapes as is if self._group_obs_concatenate[group_name]: try: - term_dims = [torch.tensor(dims, device="cpu") for dims in group_term_dims] - self._group_obs_dim[group_name] = tuple(torch.sum(torch.stack(term_dims, dim=0), dim=0).tolist()) + 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}'." @@ -157,16 +168,20 @@ def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequenc continue idx = 0 + concat_dim = self._group_obs_concatenate_dim[group_name] + # handle cases where concat dim is positive, account for the batch dimension + if concat_dim > 0: + concat_dim -= 1 # 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] + # extract the term from the buffer based on the shape + term = data[env_idx].narrow(dim=concat_dim, start=idx, length=shape[concat_dim]) terms.append((group_name + "-" + name, term.cpu().tolist())) - idx += data_length + idx += shape[concat_dim] return terms @@ -215,6 +230,70 @@ def group_obs_concatenate(self) -> dict[str, bool]: """ return self._group_obs_concatenate + @property + def get_IO_descriptors(self, group_names_to_export: list[str] = ["policy"]): + """Get the IO descriptors for the observation manager. + + Returns: + A dictionary with keys as the group names and values as the IO descriptors. + """ + + group_data = {} + + for group_name in self._group_obs_term_names: + group_data[group_name] = [] + # check if 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] + # read attributes for each term + obs_terms = zip(group_term_names, self._group_obs_term_cfgs[group_name]) + + for term_name, term_cfg in obs_terms: + # Call to the observation function to get the IO descriptor with the inspect flag set to True + try: + term_cfg.func(self._env, **term_cfg.params, inspect=True) + # Copy the descriptor and update with the term's own extra parameters + desc = term_cfg.func._descriptor.__dict__.copy() + # Create a dictionary to store the overloads + overloads = {} + # Iterate over the term's own parameters and add them to the overloads dictionary + for k, v in term_cfg.__dict__.items(): + # For now we do not add the noise modifier + if k in ["modifiers", "clip", "scale", "history_length", "flatten_history_dim"]: + overloads[k] = v + desc.update(overloads) + group_data[group_name].append(desc) + except Exception as e: + print(f"Error getting IO descriptor for term '{term_name}' in group '{group_name}': {e}") + # Format the data for YAML export + formatted_data = {} + for group_name, data in group_data.items(): + formatted_data[group_name] = [] + for item in data: + name = item.pop("name") + formatted_item = {"name": name, "overloads": {}, "extras": item.pop("extras")} + for k, v in item.items(): + # Check if v is a tuple and convert to list + if isinstance(v, tuple): + v = list(v) + # Check if v is a tensor and convert to list + if isinstance(v, torch.Tensor): + v = v.detach().cpu().numpy().tolist() + if k in ["scale", "clip", "history_length", "flatten_history_dim"]: + formatted_item["overloads"][k] = v + elif k in ["modifiers", "description", "units"]: + formatted_item["extras"][k] = v + else: + formatted_item[k] = v + formatted_data[group_name].append(formatted_item) + formatted_data = {k: v for k, v in formatted_data.items() if k in group_names_to_export} + return formatted_data + """ Operations. """ @@ -229,18 +308,23 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: 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: + for mod in self._group_obs_class_instances: mod.reset(env_ids=env_ids) # nothing to log here return {} - def compute(self) -> dict[str, torch.Tensor | dict[str, torch.Tensor]]: + def compute(self, update_history: bool = False) -> 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. + Args: + update_history: The boolean indicator without return obs should be appended to observation history. + Default to False, in which case calling compute_group does not modify history. This input is no-ops + if the group's history_length == 0. + 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 @@ -250,14 +334,14 @@ def compute(self) -> dict[str, torch.Tensor | dict[str, torch.Tensor]]: 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) + obs_buffer[group_name] = self.compute_group(group_name, update_history=update_history) # 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]: + def compute_group(self, group_name: str, update_history: bool = False) -> 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 @@ -280,6 +364,9 @@ def compute_group(self, group_name: str) -> torch.Tensor | dict[str, torch.Tenso 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. + update_history: The boolean indicator without return obs should be appended to observation group's history. + Default to False, in which case calling compute_group does not modify history. This input is no-ops + if the group's history_length == 0. Returns: Depending on the group's configuration, the tensors for individual observation terms are @@ -310,30 +397,66 @@ def compute_group(self, group_name: str) -> torch.Tensor | dict[str, torch.Tenso if term_cfg.modifiers is not None: for modifier in term_cfg.modifiers: obs = modifier.func(obs, **modifier.params) - if term_cfg.noise: + if isinstance(term_cfg.noise, noise.NoiseCfg): obs = term_cfg.noise.func(obs, term_cfg.noise) + elif isinstance(term_cfg.noise, noise.NoiseModelCfg) and term_cfg.noise.func is not None: + obs = term_cfg.noise.func(obs) 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 + circular_buffer = self._group_obs_term_history_buffer[group_name][term_name] + if update_history: + circular_buffer.append(obs) + elif circular_buffer._buffer is None: + # because circular buffer only exits after the simulation steps, + # this guards history buffer from corruption by external calls before simulation start + circular_buffer = CircularBuffer( + max_len=circular_buffer.max_length, + batch_size=circular_buffer.batch_size, + device=circular_buffer.device, ) + circular_buffer.append(obs) + + if term_cfg.flatten_history_dim: + group_obs[term_name] = circular_buffer.buffer.reshape(self._env.num_envs, -1) else: - group_obs[term_name] = self._group_obs_term_history_buffer[group_name][term_name].buffer + group_obs[term_name] = circular_buffer.buffer else: group_obs[term_name] = obs # concatenate all observations in the group together if self._group_obs_concatenate[group_name]: - return torch.cat(list(group_obs.values()), dim=-1) + # 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. """ @@ -347,10 +470,20 @@ def _prepare_terms(self): 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 + # create a list to store classes instances, e.g., for modifiers and noise models # 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() + self._group_obs_class_instances: list[modifiers.ModifierBase | noise.NoiseModel] = 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): @@ -373,18 +506,31 @@ def _prepare_terms(self): 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() + + # history buffers 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() + term_cfg_items = group_cfg.items() else: - group_cfg_items = group_cfg.__dict__.items() + term_cfg_items = group_cfg.__dict__.items() # iterate over all the terms in each group - for term_name, term_cfg in group_cfg_items: + for term_name, term_cfg in term_cfg_items: # skip non-obs settings - if term_name in ["enable_corruption", "concatenate_terms", "history_length", "flatten_history_dim"]: + 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: @@ -407,20 +553,9 @@ def _prepare_terms(self): # 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) - # 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:]) # if scale is set, check if single float or tuple if term_cfg.scale is not None: @@ -455,7 +590,7 @@ def _prepare_terms(self): 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) + self._group_obs_class_instances.append(mod_cfg.func) else: raise TypeError( f"Modifier configuration '{mod_cfg}' of observation term '{term_name}' is not of" @@ -485,6 +620,33 @@ def _prepare_terms(self): f" and optional parameters: {args_with_defaults}, but received: {term_params}." ) + # prepare noise model classes + if term_cfg.noise is not None and isinstance(term_cfg.noise, noise.NoiseModelCfg): + noise_model_cls = term_cfg.noise.class_type + if not issubclass(noise_model_cls, noise.NoiseModel): + raise TypeError( + f"Class type for observation term '{term_name}' NoiseModelCfg" + f" is not a subclass of 'NoiseModel'. Received: '{type(noise_model_cls)}'." + ) + # initialize func to be the noise model class instance + term_cfg.noise.func = noise_model_cls( + term_cfg.noise, num_envs=self._env.num_envs, device=self._env.device + ) + self._group_obs_class_instances.append(term_cfg.noise.func) + + # 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) diff --git a/source/isaaclab/isaaclab/managers/recorder_manager.py b/source/isaaclab/isaaclab/managers/recorder_manager.py index a4a6dd17ef45..51a3f3e83518 100644 --- a/source/isaaclab/isaaclab/managers/recorder_manager.py +++ b/source/isaaclab/isaaclab/managers/recorder_manager.py @@ -1,19 +1,19 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """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 +import torch +from prettytable import PrettyTable + from isaaclab.utils import configclass from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler @@ -51,6 +51,9 @@ class RecorderManagerBaseCfg: export_in_record_pre_reset: bool = True """Whether to export episodes in the record_pre_reset call.""" + export_in_close: bool = False + """Whether to export episodes in the close call.""" + class RecorderTerm(ManagerTermBase): """Base class for recorder terms. @@ -124,6 +127,26 @@ def record_post_step(self) -> tuple[str | None, torch.Tensor | dict | None]: """ return None, None + def record_post_physics_decimation_step(self) -> tuple[str | None, torch.Tensor | dict | None]: + """Record data after the physics step is executed in the decimation loop. + + 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 close(self, file_path: str): + """Finalize and "clean up" the recorder term. + + This can include tasks such as appending metadata (e.g. labels) to a file + and properly closing any associated file handles or resources. + + Args: + file_path: the absolute path to the file + """ + pass + class RecorderManager(ManagerBase): """Manager for recording data from recorder terms.""" @@ -194,15 +217,7 @@ def __str__(self) -> str: 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() + self.close() """ Properties. @@ -223,6 +238,8 @@ def exported_successful_episode_count(self, env_id=None) -> int: 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()) @@ -237,6 +254,8 @@ def exported_failed_episode_count(self, env_id=None) -> int: 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()) @@ -359,6 +378,16 @@ def record_post_step(self) -> None: key, value = term.record_post_step() self.add_to_episodes(key, value) + def record_post_physics_decimation_step(self) -> None: + """Trigger recorder terms for post-physics step functions in the decimation loop.""" + # 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_physics_decimation_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. @@ -381,8 +410,9 @@ def record_pre_reset(self, env_ids: Sequence[int] | None, force_export_or_skip=N # 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 "success" in self._env.termination_manager.active_terms: - success_results |= self._env.termination_manager.get_term("success")[env_ids] + 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): @@ -402,12 +432,33 @@ def record_post_reset(self, env_ids: Sequence[int] | None) -> None: 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: + def get_ep_meta(self) -> dict: + """Get the episode metadata.""" + if not hasattr(self._env.cfg, "get_ep_meta"): + # Add basic episode metadata + ep_meta = dict() + ep_meta["sim_args"] = { + "dt": self._env.cfg.sim.dt, + "decimation": self._env.cfg.decimation, + "render_interval": self._env.cfg.sim.render_interval, + "num_envs": self._env.cfg.scene.num_envs, + } + return ep_meta + + # Add custom episode metadata if available + ep_meta = self._env.cfg.get_ep_meta() + return ep_meta + + def export_episodes(self, env_ids: Sequence[int] | None = None, demo_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. + demo_ids: Custom identifiers for the exported episodes. + If provided, episodes will be named "demo_{demo_id}" in the dataset. + Should have the same length as env_ids if both are provided. + If None, uses the default sequential naming scheme. Defaults to None. """ # Do nothing if no active recorder terms are provided if len(self.active_terms) == 0: @@ -418,10 +469,31 @@ def export_episodes(self, env_ids: Sequence[int] | None = None) -> None: if isinstance(env_ids, torch.Tensor): env_ids = env_ids.tolist() + # Handle demo_ids processing + if demo_ids is not None: + if isinstance(demo_ids, torch.Tensor): + demo_ids = demo_ids.tolist() + if len(demo_ids) != len(env_ids): + raise ValueError(f"Length of demo_ids ({len(demo_ids)}) must match length of env_ids ({len(env_ids)})") + # Check for duplicate demo_ids + if len(set(demo_ids)) != len(demo_ids): + duplicates = [x for i, x in enumerate(demo_ids) if demo_ids.index(x) != i] + raise ValueError(f"demo_ids must be unique. Found duplicates: {list(set(duplicates))}") + # Export episode data through dataset exporter need_to_flush = False - for env_id in env_ids: + + if any(env_id in self._episodes and not self._episodes[env_id].is_empty() for env_id in env_ids): + ep_meta = self.get_ep_meta() + if self._dataset_file_handler is not None: + self._dataset_file_handler.add_env_args(ep_meta) + if self._failed_episode_dataset_file_handler is not None: + self._failed_episode_dataset_file_handler.add_env_args(ep_meta) + + for i, env_id in enumerate(env_ids): if env_id in self._episodes and not self._episodes[env_id].is_empty(): + self._episodes[env_id].pre_export() + episode_succeeded = self._episodes[env_id].success target_dataset_file_handler = None if (self.cfg.dataset_export_mode == DatasetExportMode.EXPORT_ALL) or ( @@ -434,7 +506,9 @@ def export_episodes(self, env_ids: Sequence[int] | None = None) -> None: 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]) + # Use corresponding demo_id if provided, otherwise None + current_demo_id = demo_ids[i] if demo_ids is not None else None + target_dataset_file_handler.write_episode(self._episodes[env_id], current_demo_id) need_to_flush = True # Update episode count if episode_succeeded: @@ -452,6 +526,22 @@ def export_episodes(self, env_ids: Sequence[int] | None = None) -> None: if self._failed_episode_dataset_file_handler is not None: self._failed_episode_dataset_file_handler.flush() + def close(self): + """Closes the recorder manager by exporting any remaining data to file as well as properly + closes the recorder terms. + """ + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + if self._dataset_file_handler is not None: + if self.cfg.export_in_close: + self.export_episodes() + self._dataset_file_handler.close() + if self._failed_episode_dataset_file_handler is not None: + self._failed_episode_dataset_file_handler.close() + for term in self._terms.values(): + term.close(os.path.join(self.cfg.dataset_export_dir_path, self.cfg.dataset_filename)) + """ Helper functions. """ @@ -471,6 +561,7 @@ def _prepare_terms(self): "dataset_export_dir_path", "dataset_export_mode", "export_in_record_pre_reset", + "export_in_close", ]: continue # check if term config is None diff --git a/source/isaaclab/isaaclab/managers/reward_manager.py b/source/isaaclab/isaaclab/managers/reward_manager.py index 7255d4835c44..d9c66a100a72 100644 --- a/source/isaaclab/isaaclab/managers/reward_manager.py +++ b/source/isaaclab/isaaclab/managers/reward_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,12 @@ from __future__ import annotations -import torch from collections.abc import Sequence -from prettytable import PrettyTable from typing import TYPE_CHECKING +import torch +from prettytable import PrettyTable + from .manager_base import ManagerBase, ManagerTermBase from .manager_term_cfg import RewardTermCfg @@ -140,9 +141,10 @@ def compute(self, dt: float) -> torch.Tensor: # reset computation self._reward_buf[:] = 0.0 # iterate over all the reward terms - for name, term_cfg in zip(self._term_names, self._term_cfgs): + 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 @@ -152,7 +154,7 @@ def compute(self, dt: float) -> torch.Tensor: self._episode_sums[name] += value # Update current reward for this step. - self._step_reward[:, self._term_names.index(name)] = value / dt + self._step_reward[:, term_idx] = value / dt return self._reward_buf diff --git a/source/isaaclab/isaaclab/managers/scene_entity_cfg.py b/source/isaaclab/isaaclab/managers/scene_entity_cfg.py index c089a1d1ca62..f3b01f7eee0d 100644 --- a/source/isaaclab/isaaclab/managers/scene_entity_cfg.py +++ b/source/isaaclab/isaaclab/managers/scene_entity_cfg.py @@ -1,16 +1,21 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Configuration terms for different managers.""" +from __future__ import annotations + from dataclasses import MISSING +from typing import TYPE_CHECKING -from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection -from isaaclab.scene import InteractiveScene from isaaclab.utils import configclass +if TYPE_CHECKING: + from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection + from isaaclab.scene import InteractiveScene + @configclass class SceneEntityCfg: @@ -105,7 +110,8 @@ class for more details. 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. + This attribute is only used when :attr:`joint_names`, :attr:`body_names`, or :attr:`object_collection_names` + are specified. """ @@ -124,7 +130,8 @@ def resolve(self, scene: InteractiveScene): 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. + 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(): diff --git a/source/isaaclab/isaaclab/managers/termination_manager.py b/source/isaaclab/isaaclab/managers/termination_manager.py index 7a2c379ce0cb..0a557df628a9 100644 --- a/source/isaaclab/isaaclab/managers/termination_manager.py +++ b/source/isaaclab/isaaclab/managers/termination_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,12 @@ from __future__ import annotations -import torch from collections.abc import Sequence -from prettytable import PrettyTable from typing import TYPE_CHECKING +import torch +from prettytable import PrettyTable + from .manager_base import ManagerBase, ManagerTermBase from .manager_term_cfg import TerminationTermCfg @@ -60,10 +61,11 @@ def __init__(self, cfg: object, env: ManagerBasedRLEnv): # call the base class constructor (this will parse the terms config) super().__init__(cfg, env) + self._term_name_to_term_idx = {name: i for i, name in enumerate(self._term_names)} # 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) + self._term_dones = torch.zeros((self.num_envs, len(self._term_names)), device=self.device, dtype=torch.bool) + # prepare extra info to store last episode done per termination term information + self._last_episode_dones = torch.zeros_like(self._term_dones) # 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) @@ -139,9 +141,10 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor] env_ids = slice(None) # add to episode dict extras = {} - for key in self._term_dones.keys(): + last_episode_done_stats = self._last_episode_dones.float().mean(dim=0) + for i, key in enumerate(self._term_names): # store information - extras["Episode_Termination/" + key] = torch.count_nonzero(self._term_dones[key][env_ids]).item() + extras["Episode_Termination/" + key] = last_episode_done_stats[i].item() # reset all the reward terms for term_cfg in self._class_term_cfgs: term_cfg.func.reset(env_ids=env_ids) @@ -161,7 +164,7 @@ def compute(self) -> torch.Tensor: 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): + for i, term_cfg in enumerate(self._term_cfgs): value = term_cfg.func(self._env, **term_cfg.params) # store timeout signal separately if term_cfg.time_out: @@ -169,12 +172,17 @@ def compute(self) -> torch.Tensor: else: self._terminated_buf |= value # add to episode dones - self._term_dones[name][:] = value + self._term_dones[:, i] = value + # update last-episode dones once per compute: for any env where a term fired, + # reflect exactly which term(s) fired this step and clear others + rows = self._term_dones.any(dim=1).nonzero(as_tuple=True)[0] + if rows.numel() > 0: + self._last_episode_dones[rows] = self._term_dones[rows] # 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. + """Returns the termination term value at current step with the specified name. Args: name: The name of the termination term. @@ -182,12 +190,13 @@ def get_term(self, name: str) -> torch.Tensor: Returns: The corresponding termination term value. Shape is (num_envs,). """ - return self._term_dones[name] + return self._term_dones[:, self._term_name_to_term_idx[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. + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term + recorded at current step. Args: env_idx: The specific environment to pull the active terms from. @@ -196,8 +205,8 @@ def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequenc The active terms. """ terms = [] - for key in self._term_dones.keys(): - terms.append((key, [self._term_dones[key][env_idx].float().cpu().item()])) + for i, key in enumerate(self._term_names): + terms.append((key, [self._term_dones[env_idx, i].float().cpu().item()])) return terms """ @@ -217,7 +226,7 @@ def set_term_cfg(self, term_name: str, cfg: TerminationTermCfg): 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 + self._term_cfgs[self._term_name_to_term_idx[term_name]] = cfg def get_term_cfg(self, term_name: str) -> TerminationTermCfg: """Gets the configuration for the specified term. @@ -234,7 +243,7 @@ def get_term_cfg(self, term_name: str) -> TerminationTermCfg: 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)] + return self._term_cfgs[self._term_name_to_term_idx[term_name]] """ Helper functions. diff --git a/source/isaaclab/isaaclab/markers/__init__.py b/source/isaaclab/isaaclab/markers/__init__.py index 47c839892db6..eb7b69761009 100644 --- a/source/isaaclab/isaaclab/markers/__init__.py +++ b/source/isaaclab/isaaclab/markers/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/markers/config/__init__.py b/source/isaaclab/isaaclab/markers/config/__init__.py index 220f1d9823d0..54d30051259a 100644 --- a/source/isaaclab/isaaclab/markers/config/__init__.py +++ b/source/isaaclab/isaaclab/markers/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -47,6 +47,15 @@ ) """Configuration for the deformable object's kinematic target marker.""" +VISUO_TACTILE_SENSOR_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "tacsl_pts": sim_utils.SphereCfg( + radius=0.0002, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ), + }, +) +"""Configuration for the visuo-tactile sensor marker.""" ## # Frames. @@ -57,7 +66,12 @@ "frame": sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd", scale=(0.5, 0.5, 0.5), - ) + ), + "connecting_line": sim_utils.CylinderCfg( + radius=0.002, + height=1.0, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 1.0, 0.0), roughness=1.0), + ), } ) """Configuration for the frame marker.""" @@ -112,6 +126,16 @@ ) """Configuration for the cuboid marker.""" +SPHERE_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "sphere": sim_utils.SphereCfg( + radius=0.05, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + } +) +"""Configuration for the sphere marker.""" + POSITION_GOAL_MARKER_CFG = VisualizationMarkersCfg( markers={ "target_far": sim_utils.SphereCfg( diff --git a/source/isaaclab/isaaclab/markers/visualization_markers.py b/source/isaaclab/isaaclab/markers/visualization_markers.py index dee5c0f3bcdf..bd27009c0ec3 100644 --- a/source/isaaclab/isaaclab/markers/visualization_markers.py +++ b/source/isaaclab/isaaclab/markers/visualization_markers.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -19,12 +19,12 @@ # needed to import for allowing type-hinting: np.ndarray | torch.Tensor | None from __future__ import annotations +import logging +from dataclasses import MISSING + 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 @@ -33,6 +33,9 @@ from isaaclab.utils.configclass import configclass from isaaclab.utils.math import convert_quat +# import logger +logger = logging.getLogger(__name__) + @configclass class VisualizationMarkersCfg: @@ -63,9 +66,9 @@ class VisualizationMarkers: 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 + marker prim is resolved using the key of the marker in the :attr:`VisualizationMarkersCfg.markers` + dictionary. The marker prototypes are created using the :meth:`isaaclab.sim.utils.prims.create_prim` + function, and 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 @@ -97,7 +100,7 @@ class VisualizationMarkers: 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. @@ -143,10 +146,10 @@ def __init__(self, cfg: VisualizationMarkersCfg): 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) + prim_path = sim_utils.get_next_free_prim_path(cfg.prim_path) # create a new prim - stage = stage_utils.get_current_stage() - self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + self.stage = sim_utils.get_current_stage() + self._instancer_manager = UsdGeom.PointInstancer.Define(self.stage, prim_path) # store inputs self.prim_path = prim_path self.cfg = cfg @@ -369,9 +372,7 @@ def _process_prototype_prim(self, prim: Usd.Prim): 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. + prim: The prim to check. """ # check if prim is valid if not prim.IsValid(): @@ -396,11 +397,10 @@ def _process_prototype_prim(self, prim: Usd.Prim): # 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"), + sim_utils.change_prim_property( + prop_path=f"{child_prim.GetPrimPath().pathString}.primvars:invisibleToSecondaryRays", value=True, - prev=None, + stage=prim.GetStage(), type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool, ) # add children to list diff --git a/source/isaaclab/isaaclab/scene/__init__.py b/source/isaaclab/isaaclab/scene/__init__.py index df8b37cdf482..4b614ea4bced 100644 --- a/source/isaaclab/isaaclab/scene/__init__.py +++ b/source/isaaclab/isaaclab/scene/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 1483c30cf915..46e5895687aa 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -1,16 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch +import logging from collections.abc import Sequence from typing import Any +import torch + import carb -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 @@ -24,12 +24,25 @@ RigidObjectCfg, RigidObjectCollection, RigidObjectCollectionCfg, + SurfaceGripper, + SurfaceGripperCfg, ) from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg +from isaaclab.sim import SimulationContext +from isaaclab.sim.utils.stage import get_current_stage, get_current_stage_id +from isaaclab.sim.views import XformPrimView from isaaclab.terrains import TerrainImporter, TerrainImporterCfg +from isaaclab.utils.version import get_isaac_sim_version + +# Note: This is a temporary import for the VisuoTactileSensorCfg class. +# It will be removed once the VisuoTactileSensor class is added to the core Isaac Lab framework. +from isaaclab_contrib.sensors.tacsl_sensor import VisuoTactileSensorCfg from .interactive_scene_cfg import InteractiveSceneCfg +# import logger +logger = logging.getLogger(__name__) + class InteractiveScene: """A scene that contains entities added to the simulation. @@ -64,9 +77,10 @@ class InteractiveScene: from isaaclab_assets.robots.anymal import ANYMAL_C_CFG + @configclass class MySceneCfg(InteractiveSceneCfg): - + # ANYmal-C robot spawned in each environment robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") Then the robot can be accessed from the scene as follows: @@ -117,29 +131,49 @@ def __init__(self, cfg: InteractiveSceneCfg): self._rigid_objects = dict() self._rigid_object_collections = dict() self._sensors = dict() + self._surface_grippers = dict() self._extras = dict() - # obtain the current stage - self.stage = omni.usd.get_context().get_stage() + # get stage handle + self.sim = SimulationContext.instance() + self.stage = get_current_stage() + self.stage_id = get_current_stage_id() # physics scene path self._physics_scene_path = None # prepare cloner for environment replication - self.cloner = GridCloner(spacing=self.cfg.env_spacing) + self.cloner = GridCloner(spacing=self.cfg.env_spacing, stage=self.stage) 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") - + # allocate env indices + self._ALL_INDICES = torch.arange(self.cfg.num_envs, dtype=torch.long, device=self.device) # 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 - ) + # check version of Isaac Sim to determine whether clone_in_fabric is valid + if get_isaac_sim_version().major < 5: + # 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 if self.device != "cpu" else False + ), # this won't do anything because we are not replicating physics + ) + else: + # 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 if self.device != "cpu" else False + ), # this won't do anything because we are not replicating physics + clone_in_fabric=self.cfg.clone_in_fabric, + ) 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 @@ -155,17 +189,29 @@ def __init__(self, cfg: InteractiveSceneCfg): # 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, - ) + # check version of Isaac Sim to determine whether clone_in_fabric is valid + if get_isaac_sim_version().major < 5: + 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 if self.device != "cpu" else False, + ) + else: + 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 if self.device != "cpu" else False, + clone_in_fabric=self.cfg.clone_in_fabric, + ) # 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: + # additionally, env_ids is only supported in GPU simulation + if (not self.cfg.replicate_physics and self.cfg.filter_collisions) or self.device == "cpu": self.filter_collisions(self._global_prim_paths) def clone_environments(self, copy_from_source: bool = False): @@ -181,28 +227,48 @@ def clone_environments(self, copy_from_source: bool = False): 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( + logger.warning( "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 - ) + # check version of Isaac Sim to determine whether clone_in_fabric is valid + if get_isaac_sim_version().major < 5: + # 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 if self.device != "cpu" else False + ), # this automatically filters collisions between environments + ) + else: + # 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 if self.device != "cpu" else False + ), # this automatically filters collisions between environments + clone_in_fabric=self.cfg.clone_in_fabric, + ) # 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." - ) + # additionally, env_ids is only supported in GPU simulation + if (not self.cfg.replicate_physics and self.cfg.filter_collisions) or self.device == "cpu": + # if scene is specified through cfg, this is already taken care of + if not self._is_scene_setup_from_cfg(): + logger.warning( + "Collision filtering can only be automatically enabled when replicate_physics=True and using GPU" + " simulation. 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: @@ -225,6 +291,10 @@ def filter_collisions(self, global_prim_paths: list[str] | None = None): # remove duplicates in paths global_prim_paths = list(set(global_prim_paths)) + # if "/World/collisions" already exists in the stage, we don't filter again + if self.stage.GetPrimAtPath("/World/collisions"): + return + # set global prim paths list if not previously defined if len(self._global_prim_paths) < 1: self._global_prim_paths += global_prim_paths @@ -258,7 +328,7 @@ def physics_scene_path(self) -> str: 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}") + logger.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.") @@ -337,11 +407,16 @@ def sensors(self) -> dict[str, SensorBase]: return self._sensors @property - def extras(self) -> dict[str, XFormPrim]: + def surface_grippers(self) -> dict[str, SurfaceGripper]: + """A dictionary of the surface grippers in the scene.""" + return self._surface_grippers + + @property + def extras(self) -> dict[str, XformPrimView]: """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. + The keys are the names of the miscellaneous objects, and the values are the + :class:`~isaaclab.sim.views.XformPrimView` instances 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. @@ -350,8 +425,6 @@ 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 - """ return self._extras @@ -381,6 +454,8 @@ def reset(self, env_ids: Sequence[int] | None = None): deformable_object.reset(env_ids) for rigid_object in self._rigid_objects.values(): rigid_object.reset(env_ids) + for surface_gripper in self._surface_grippers.values(): + surface_gripper.reset(env_ids) for rigid_object_collection in self._rigid_object_collections.values(): rigid_object_collection.reset(env_ids) # -- sensors @@ -396,6 +471,8 @@ def write_data_to_sim(self): deformable_object.write_data_to_sim() for rigid_object in self._rigid_objects.values(): rigid_object.write_data_to_sim() + for surface_gripper in self._surface_grippers.values(): + surface_gripper.write_data_to_sim() for rigid_object_collection in self._rigid_object_collections.values(): rigid_object_collection.write_data_to_sim() @@ -414,6 +491,8 @@ def update(self, dt: float) -> None: rigid_object.update(dt) for rigid_object_collection in self._rigid_object_collections.values(): rigid_object_collection.update(dt) + for surface_gripper in self._surface_grippers.values(): + surface_gripper.update(dt) # -- sensors for sensor in self._sensors.values(): sensor.update(dt, force_recompute=not self.cfg.lazy_sensor_update) @@ -439,7 +518,7 @@ def reset_to( """ # resolve env_ids if env_ids is None: - env_ids = slice(None) + env_ids = self._ALL_INDICES # articulations for asset_name, articulation in self._articulations.items(): asset_state = state["articulation"][asset_name] @@ -476,6 +555,10 @@ def reset_to( 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) + # surface grippers + for asset_name, surface_gripper in self._surface_grippers.items(): + asset_state = state["gripper"][asset_name] + surface_gripper.set_grippers_command(asset_state) # write data to simulation to make sure initial state is set # this propagates the joint targets to the simulation @@ -537,7 +620,7 @@ def get_state(self, is_relative: bool = False) -> dict[str, dict[str, dict[str, state["articulation"] = dict() for asset_name, articulation in self._articulations.items(): asset_state = dict() - asset_state["root_pose"] = articulation.data.root_state_w[:, :7].clone() + 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() @@ -557,11 +640,15 @@ def get_state(self, is_relative: bool = False) -> dict[str, dict[str, dict[str, 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_state_w[:, :7].clone() + 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 + # surface grippers + state["gripper"] = dict() + for asset_name, gripper in self._surface_grippers.items(): + state["gripper"][asset_name] = gripper.state.clone() return state """ @@ -581,6 +668,7 @@ def keys(self) -> list[str]: self._rigid_objects, self._rigid_object_collections, self._sensors, + self._surface_grippers, self._extras, ]: all_keys += list(asset_family.keys()) @@ -607,6 +695,7 @@ def __getitem__(self, key: str) -> Any: self._rigid_objects, self._rigid_object_collections, self._sensors, + self._surface_grippers, self._extras, ]: out = asset_family.get(key) @@ -621,7 +710,12 @@ def __getitem__(self, key: str) -> Any: Internal methods. """ - def _is_scene_setup_from_cfg(self): + 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() @@ -660,6 +754,9 @@ def _add_entities_from_cfg(self): 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, SurfaceGripperCfg): + # add surface grippers to scene + self._surface_grippers[asset_name] = asset_cfg.class_type(asset_cfg) elif isinstance(asset_cfg, SensorBaseCfg): # Update target frame path(s)' regex name space for FrameTransformer if isinstance(asset_cfg, FrameTransformerCfg): @@ -673,6 +770,18 @@ def _add_entities_from_cfg(self): 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 + elif isinstance(asset_cfg, VisuoTactileSensorCfg): + if hasattr(asset_cfg, "camera_cfg") and asset_cfg.camera_cfg is not None: + asset_cfg.camera_cfg.prim_path = asset_cfg.camera_cfg.prim_path.format( + ENV_REGEX_NS=self.env_regex_ns + ) + if ( + hasattr(asset_cfg, "contact_object_prim_path_expr") + and asset_cfg.contact_object_prim_path_expr is not None + ): + asset_cfg.contact_object_prim_path_expr = asset_cfg.contact_object_prim_path_expr.format( + ENV_REGEX_NS=self.env_regex_ns + ) self._sensors[asset_name] = asset_cfg.class_type(asset_cfg) elif isinstance(asset_cfg, AssetBaseCfg): @@ -686,7 +795,7 @@ def _add_entities_from_cfg(self): ) # 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) + self._extras[asset_name] = XformPrimView(asset_cfg.prim_path, device=self.device, stage=self.stage) else: raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") # store global collision paths diff --git a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py index 8afdec4b46ff..f4328324152c 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -33,9 +33,9 @@ class InteractiveSceneCfg: from isaaclab_assets.robots.anymal import ANYMAL_C_CFG + @configclass class MySceneCfg(InteractiveSceneCfg): - # terrain - flat terrain plane terrain = TerrainImporterCfg( prim_path="/World/ground", @@ -52,7 +52,7 @@ class MySceneCfg(InteractiveSceneCfg): 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, + ray_alignment="yaw", pattern_cfg=GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], @@ -107,5 +107,20 @@ class MySceneCfg(InteractiveSceneCfg): .. 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()``. + If :attr:`replicated_physics` is ``False`` and collision filtering is desired, make sure to call + ``scene.filter_collisions()``. + """ + + clone_in_fabric: bool = False + """Enable/disable cloning in fabric. Default is False. + + Omniverse Fabric is a more optimized method for performing cloning in scene creation. This reduces the time + taken to create the scene. However, it limits flexibility in accessing the stage through USD APIs and instead, + the stage must be accessed through USDRT. + + .. note:: + Cloning in fabric can only be enabled if :attr:`replicated_physics` is also enabled. + If :attr:`replicated_physics` is ``False``, cloning in Fabric will automatically + default to ``False``. + """ diff --git a/source/isaaclab/isaaclab/sensors/__init__.py b/source/isaaclab/isaaclab/sensors/__init__.py index 0f28b6f05734..f0a5719e5056 100644 --- a/source/isaaclab/isaaclab/sensors/__init__.py +++ b/source/isaaclab/isaaclab/sensors/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sensors/camera/__init__.py b/source/isaaclab/isaaclab/sensors/camera/__init__.py index d3c3e36c5ab6..f2318067b586 100644 --- a/source/isaaclab/isaaclab/sensors/camera/__init__.py +++ b/source/isaaclab/isaaclab/sensors/camera/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 048b307372c8..26fd0713c715 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,20 +6,22 @@ from __future__ import annotations import json -import numpy as np +import logging import re -import torch from collections.abc import Sequence from typing import TYPE_CHECKING, Any, Literal +import numpy as np +import torch +from packaging import version + import carb -import isaacsim.core.utils.stage as stage_utils -import omni.kit.commands import omni.usd -from isaacsim.core.prims import XFormPrim -from pxr import UsdGeom +from pxr import Sdf, UsdGeom import isaaclab.sim as sim_utils +import isaaclab.utils.sensors as sensor_utils +from isaaclab.sim.views import XformPrimView from isaaclab.utils import to_camel_case from isaaclab.utils.array import convert_to_torch from isaaclab.utils.math import ( @@ -27,6 +29,7 @@ create_rotation_matrix_from_view, quat_from_matrix, ) +from isaaclab.utils.version import get_isaac_sim_version from ..sensor_base import SensorBase from .camera_data import CameraData @@ -34,6 +37,9 @@ if TYPE_CHECKING: from .camera_cfg import CameraCfg +# import logger +logger = logging.getLogger(__name__) + class Camera(SensorBase): r"""The camera sensor for acquiring visual data. @@ -123,7 +129,7 @@ def __init__(self, cfg: CameraCfg): rot_offset = convert_camera_frame_orientation_convention( rot, origin=self.cfg.offset.convention, target="opengl" ) - rot_offset = rot_offset.squeeze(0).numpy() + 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 @@ -141,6 +147,20 @@ def __init__(self, cfg: CameraCfg): # 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 + # checks for Isaac Sim v4.5 as this issue exists there + if get_isaac_sim_version() == version.parse("4.5"): + if "semantic_segmentation" in self.cfg.data_types or "instance_segmentation_fast" in self.cfg.data_types: + logger.warning( + "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." + ) + with Sdf.ChangeBlock(): + for prim in self.stage.Traverse(): + prim.SetInstanceable(False) + def __del__(self): """Unsubscribes from callbacks and detach from the replicator registry.""" # unsubscribe callbacks @@ -204,11 +224,11 @@ def image_shape(self) -> tuple[int, int]: """ def set_intrinsic_matrices( - self, matrices: torch.Tensor, focal_length: float = 1.0, env_ids: Sequence[int] | None = None + 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 and focal length are used to set the following parameters to the USD camera: + 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. @@ -224,7 +244,8 @@ def set_intrinsic_matrices( 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. + 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 @@ -237,27 +258,11 @@ def set_intrinsic_matrices( matrices = np.asarray(matrices, dtype=float) # iterate over env_ids for i, intrinsic_matrix in zip(env_ids, matrices): - # extract parameters from matrix - f_x = intrinsic_matrix[0, 0] - c_x = intrinsic_matrix[0, 2] - f_y = intrinsic_matrix[1, 1] - c_y = intrinsic_matrix[1, 2] - # get viewport parameters height, width = self.image_shape - height, width = float(height), float(width) - # resolve parameters for usd camera - params = { - "focal_length": focal_length, - "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, - } - - # TODO: Adjust to handle aperture offsets once supported by omniverse - # Internal ticket from rendering team: OM-42611 - if params["horizontal_aperture_offset"] > 1e-4 or params["vertical_aperture_offset"] > 1e-4: - omni.log.warn("Camera aperture offsets are not supported by Omniverse. These parameters are ignored.") + + 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] @@ -346,7 +351,7 @@ def set_world_poses_from_view( if env_ids is None: env_ids = self._ALL_INDICES # get up axis of current stage - up_axis = stage_utils.get_stage_up_axis() + up_axis = UsdGeom.GetStageUpAxis(self.stage) # 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) @@ -398,9 +403,10 @@ def _initialize_impl(self): # 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() + # Create a view for the sensor with Fabric enabled for fast pose queries, otherwise position will be stale. + self._view = XformPrimView( + self.cfg.prim_path, device=self._device, stage=self.stage, sync_usd_on_fabric_write=True + ) # Check that sizes are correct if self._view.count != self._num_envs: raise RuntimeError( @@ -417,12 +423,10 @@ def _initialize_impl(self): 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) + for cam_prim in self._view.prims: + # Obtain the prim path + cam_prim_path = cam_prim.GetPath().pathString # 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.") @@ -493,7 +497,8 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): # Increment frame count self._frame[env_ids] += 1 # -- pose - self._update_poses(env_ids) + 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: @@ -581,18 +586,17 @@ def _update_intrinsic_matrices(self, env_ids: Sequence[int]): # 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() - vert_aperture = sensor_prim.GetVerticalApertureAttr().Get() - horiz_aperture_offset = sensor_prim.GetHorizontalApertureOffsetAttr().Get() - vert_aperture_offset = sensor_prim.GetVerticalApertureOffsetAttr().Get() + # get viewport parameters height, width = self.image_shape # extract intrinsic parameters f_x = (width * focal_length) / horiz_aperture - f_y = (height * focal_length) / vert_aperture - c_x = width * 0.5 + horiz_aperture_offset * f_x - c_y = height * 0.5 + vert_aperture_offset * f_y + 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 diff --git a/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py b/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py index fd5fd846ddec..8fd9f307d180 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -30,7 +30,8 @@ class OffsetCfg: 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. + - ``"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. @@ -73,6 +74,14 @@ class OffsetCfg: 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 ``"*:*"``. @@ -119,14 +128,16 @@ class OffsetCfg: """Dictionary mapping semantics to specific colours Eg. - ``` - { - "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), - } - ``` + + .. 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/source/isaaclab/isaaclab/sensors/camera/camera_data.py b/source/isaaclab/isaaclab/sensors/camera/camera_data.py index 8ba46cdfc73a..ec3288b04e92 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera_data.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera_data.py @@ -1,12 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch from dataclasses import dataclass from typing import Any +import torch + from isaaclab.utils.math import convert_camera_frame_orientation_convention diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py index 0c9d30eaf472..4b3676158e75 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,18 +7,17 @@ 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 numpy as np +import torch import warp as wp -from isaacsim.core.prims import XFormPrim -from isaacsim.core.version import get_version -from pxr import Sdf, UsdGeom +import carb +from pxr import UsdGeom + +from isaaclab.sim.views import XformPrimView from isaaclab.utils.warp.kernels import reshape_tiled_image from ..sensor_base import SensorBase @@ -82,32 +81,10 @@ def __init__(self, cfg: TiledCameraCfg): 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) - # 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 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 from callbacks @@ -174,8 +151,7 @@ def _initialize_impl(self): # 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() + self._view = XformPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) # Check that sizes are correct if self._view.count != self._num_envs: raise RuntimeError( @@ -188,23 +164,20 @@ def _initialize_impl(self): # 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: + cam_prim_paths = [] + for cam_prim in self._view.prims: # Get camera prim - cam_prim = stage.GetPrimAtPath(cam_prim_path) + cam_prim_path = cam_prim.GetPath().pathString # 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) + self._sensor_prims.append(UsdGeom.Camera(cam_prim)) + cam_prim_paths.append(cam_prim_path) # Create replicator tiled render product - rp = rep.create.render_product_tiled( - cameras=self._view.prim_paths, tile_resolution=(self.cfg.width, self.cfg.height) - ) + rp = rep.create.render_product_tiled(cameras=cam_prim_paths, tile_resolution=(self.cfg.width, self.cfg.height)) self._render_product_paths = [rp.path] # Define the annotators based on requested data types @@ -250,6 +223,10 @@ 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) @@ -262,7 +239,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) @@ -283,6 +262,11 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): if data_type == "motion_vectors": tiled_data_buffer = tiled_data_buffer[:, :, :2].contiguous() + # For normals, we only require the first three channels of the tiled buffer + # Note: Not doing this breaks the alignment of the data (check: https://github.com/isaac-sim/IsaacLab/issues/4239) + if data_type == "normals": + tiled_data_buffer = tiled_data_buffer[:, :, :3].contiguous() + wp.launch( kernel=reshape_tiled_image, dim=(self._view.count, self.cfg.height, self.cfg.width), @@ -304,9 +288,9 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): # 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 + 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" diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera_cfg.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera_cfg.py index a0ec14988505..2241a0648fd2 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera_cfg.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,11 +14,3 @@ class TiledCameraCfg(CameraCfg): """Configuration for a tiled rendering-based camera sensor.""" class_type: type = TiledCamera - - return_latest_camera_pose: bool = False - """Whether to return the latest camera pose when fetching the camera's data. Defaults to False. - - If True, the latest camera pose is returned 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. - """ diff --git a/source/isaaclab/isaaclab/sensors/camera/utils.py b/source/isaaclab/isaaclab/sensors/camera/utils.py index 64a349e050c1..f9db81551b4f 100644 --- a/source/isaaclab/isaaclab/sensors/camera/utils.py +++ b/source/isaaclab/isaaclab/sensors/camera/utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,10 +8,10 @@ # 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 numpy as np +import torch import warp as wp import isaaclab.utils.math as math_utils @@ -170,7 +170,8 @@ def create_pointcloud_from_rgbd( 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 ``np.array``/``wp.array``/``torch.tensor`` of shape (H, W, 3), then the corresponding channels + encode the 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). diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py b/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py index dba5c1da74a8..94b402d41a37 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py @@ -1,9 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""Sub-module for rigid contact sensor based on :class:`isaacsim.core.prims.RigidContactView`.""" +"""Sub-module for rigid contact sensor.""" from .contact_sensor import ContactSensor from .contact_sensor_cfg import ContactSensorCfg diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index a88a0868247a..2a85a2661f6a 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,12 +8,14 @@ from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + 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 @@ -57,7 +59,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 @@ -148,17 +150,22 @@ def reset(self, env_ids: Sequence[int] | None = 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 + self._data.force_matrix_w_history[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 + # reset contact positions + if self.cfg.track_contact_points: + self._data.contact_pos_w[env_ids, :] = torch.nan + # reset friction forces + if self.cfg.track_friction_forces: + self._data.friction_forces_w[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. @@ -249,9 +256,8 @@ def compute_first_air(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Tensor: def _initialize_impl(self): super()._initialize_impl() - # create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") + # 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 @@ -278,7 +284,9 @@ def _initialize_impl(self): # 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 + body_names_glob, + filter_patterns=filter_prim_paths_glob, + max_contact_data_count=self.cfg.max_contact_data_count_per_prim * len(body_names) * self._num_envs, ) # resolve the true count of bodies self._num_bodies = self.body_physx_view.count // self._num_envs @@ -304,6 +312,35 @@ def _initialize_impl(self): 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) + + # check if filter paths are valid + if self.cfg.track_contact_points or self.cfg.track_friction_forces: + if len(self.cfg.filter_prim_paths_expr) == 0: + raise ValueError( + "The 'filter_prim_paths_expr' is empty. Please specify a valid filter pattern to track" + f" {'contact points' if self.cfg.track_contact_points else 'friction forces'}." + ) + if self.cfg.max_contact_data_count_per_prim < 1: + raise ValueError( + f"The 'max_contact_data_count_per_prim' is {self.cfg.max_contact_data_count_per_prim}. " + "Please set it to a value greater than 0 to track" + f" {'contact points' if self.cfg.track_contact_points else 'friction forces'}." + ) + + # -- position of contact points + if self.cfg.track_contact_points: + self._data.contact_pos_w = torch.full( + (self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3), + torch.nan, + device=self._device, + ) + # -- friction forces at contact points + if self.cfg.track_friction_forces: + self._data.friction_forces_w = torch.full( + (self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3), + 0.0, + 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) @@ -311,11 +348,18 @@ def _initialize_impl(self): 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) + # force matrix history: (num_envs, history_length, 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 ) + if self.cfg.history_length > 0: + self._data.force_matrix_w_history = torch.zeros( + self._num_envs, self.cfg.history_length, self._num_bodies, num_filters, 3, device=self._device + ) + else: + self._data.force_matrix_w_history = self._data.force_matrix_w.unsqueeze(1) def _update_buffers_impl(self, env_ids: Sequence[int]): """Fills the buffers of the sensor data.""" @@ -330,7 +374,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): 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] = self._data.net_forces_w_history[env_ids].roll(1, dims=1) self._data.net_forces_w_history[env_ids, 0] = self._data.net_forces_w[env_ids] # obtain the contact force matrix @@ -341,6 +385,9 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): 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] + if self.cfg.history_length > 0: + self._data.force_matrix_w_history[env_ids] = self._data.force_matrix_w_history[env_ids].roll(1, dims=1) + self._data.force_matrix_w_history[env_ids, 0] = self._data.force_matrix_w[env_ids] # obtain the pose of the sensor origin if self.cfg.track_pose: @@ -348,6 +395,24 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): 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 contact points + if self.cfg.track_contact_points: + _, buffer_contact_points, _, _, buffer_count, buffer_start_indices = ( + self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) + ) + self._data.contact_pos_w[env_ids] = self._unpack_contact_buffer_data( + buffer_contact_points, buffer_count, buffer_start_indices + )[env_ids] + + # obtain friction forces + if self.cfg.track_friction_forces: + friction_forces, _, buffer_count, buffer_start_indices = self.contact_physx_view.get_friction_data( + dt=self._sim_physics_dt + ) + self._data.friction_forces_w[env_ids] = self._unpack_contact_buffer_data( + friction_forces, buffer_count, buffer_start_indices, avg=False, default=0.0 + )[env_ids] + # obtain the air time if self.cfg.track_air_time: # -- time elapsed since last update @@ -378,11 +443,63 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): is_contact, self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 ) + def _unpack_contact_buffer_data( + self, + contact_data: torch.Tensor, + buffer_count: torch.Tensor, + buffer_start_indices: torch.Tensor, + avg: bool = True, + default: float = float("nan"), + ) -> torch.Tensor: + """ + Unpacks and aggregates contact data for each (env, body, filter) group. + + This function vectorizes the following nested loop: + + for i in range(self._num_bodies * self._num_envs): + for j in range(self.contact_physx_view.filter_count): + start_index_ij = buffer_start_indices[i, j] + count_ij = buffer_count[i, j] + self._contact_position_aggregate_buffer[i, j, :] = torch.mean( + contact_data[start_index_ij : (start_index_ij + count_ij), :], dim=0 + ) + + For more details, see the `RigidContactView.get_contact_data() documentation `_. + + Args: + contact_data: Flat tensor of contact data, shape (N_envs * N_bodies, 3). + buffer_count: Number of contact points per (env, body, filter), shape (N_envs * N_bodies, N_filters). + buffer_start_indices: Start indices for each (env, body, filter), shape (N_envs * N_bodies, N_filters). + avg: If True, average the contact data for each group; if False, sum the data. Defaults to True. + default: Default value to use for groups with zero contacts. Defaults to NaN. + + Returns: + Aggregated contact data, shape (N_envs, N_bodies, N_filters, 3). + """ + counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) + n_rows, total = counts.numel(), int(counts.sum()) + agg = torch.full((n_rows, 3), default, device=self._device, dtype=contact_data.dtype) + if total > 0: + row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) + + block_starts = counts.cumsum(0) - counts + deltas = torch.arange(row_ids.numel(), device=counts.device) - block_starts.repeat_interleave(counts) + flat_idx = starts[row_ids] + deltas + + pts = contact_data.index_select(0, flat_idx) + agg = agg.zero_().index_add_(0, row_ids, pts) + agg = agg / counts.clamp_min(1).unsqueeze(-1) if avg else agg + agg[counts == 0] = default + + return agg.view(self._num_envs * self.num_bodies, -1, 3).view( + self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 + ) + 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 + # create markers if necessary for the first time if not hasattr(self, "contact_visualizer"): self.contact_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) # set their visibility to true @@ -418,6 +535,5 @@ def _invalidate_initialize_callback(self, event): # call parent super()._invalidate_initialize_callback(event) # set all existing views to None to invalidate them - self._physics_sim_view = None self._body_physx_view = None self._contact_physx_view = None diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py index e9dde3a0599d..c811a7ca63d1 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -20,6 +20,25 @@ class ContactSensorCfg(SensorBaseCfg): track_pose: bool = False """Whether to track the pose of the sensor's origin. Defaults to False.""" + track_contact_points: bool = False + """Whether to track the contact point locations. Defaults to False.""" + + track_friction_forces: bool = False + """Whether to track the friction forces at the contact points. Defaults to False.""" + + max_contact_data_count_per_prim: int = 4 + """The maximum number of contacts across all batches of the sensor to keep track of. Default is 4. + + This parameter sets the total maximum counts of the simulation across all bodies and environments. The total number + of contacts allowed is max_contact_data_count_per_prim*num_envs*num_sensor_bodies. + + .. note:: + + If the environment is very contact rich it is suggested to increase this parameter to avoid out of bounds memory + errors and loss of contact data leading to inaccurate measurements. + + """ + track_air_time: bool = False """Whether to track the air/contact time of the bodies (time between contacts). Defaults to False.""" @@ -49,6 +68,7 @@ class ContactSensorCfg(SensorBaseCfg): 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. + If track_contact_points is true, then filter_prim_paths_expr cannot be an empty list! """ visualizer_cfg: VisualizationMarkersCfg = CONTACT_SENSOR_MARKER_CFG.replace(prim_path="/Visuals/ContactSensor") diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py index cd78ff992383..fd6c15ebe960 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,9 +6,10 @@ # needed to import for allowing type-hinting: torch.Tensor | None from __future__ import annotations -import torch from dataclasses import dataclass +import torch + @dataclass class ContactSensorData: @@ -21,6 +22,44 @@ class ContactSensorData: Note: If the :attr:`ContactSensorCfg.track_pose` is False, then this quantity is None. + + """ + + contact_pos_w: torch.Tensor | None = None + """Average of the positions of contact points between sensor body and filter prim 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. + + Collision pairs not in contact will result in NaN. + + Note: + + * If the :attr:`ContactSensorCfg.track_contact_points` is False, then this quantity is None. + * If the :attr:`ContactSensorCfg.track_contact_points` is True, a ValueError will be raised if: + + * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. + * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. + will not be calculated. + + """ + + friction_forces_w: torch.Tensor | None = None + """Sum of the friction forces between sensor body and filter prim 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. + + Collision pairs not in contact will result in NaN. + + Note: + + * If the :attr:`ContactSensorCfg.track_friction_forces` is False, then this quantity is None. + * If the :attr:`ContactSensorCfg.track_friction_forces` is True, a ValueError will be raised if: + + * The :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. + * The :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. + """ quat_w: torch.Tensor | None = None @@ -59,7 +98,19 @@ class ContactSensorData: """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. + and M is the number of filtered bodies. + + Note: + If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is None. + """ + + force_matrix_w_history: torch.Tensor | None = None + """The normal contact forces filtered between the sensor bodies and filtered bodies in world frame. + + Shape is (N, T, B, M, 3), where N is the number of sensors, T is the configured history length, + B is number of bodies in each sensor and M is the number of filtered bodies. + + In the history dimension, the first index is the most recent and the last index is the oldest. Note: If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is None. diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/__init__.py b/source/isaaclab/isaaclab/sensors/frame_transformer/__init__.py index a15c2e842d20..d5db853e8cc2 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/__init__.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index 277066964394..ed83392b3aa7 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py @@ -1,22 +1,31 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +import logging 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 torch + +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 isaaclab.utils.math import ( + combine_frame_transforms, + convert_quat, + is_identity_pose, + normalize, + quat_from_angle_axis, + subtract_frame_transforms, +) from ..sensor_base import SensorBase from .frame_transformer_data import FrameTransformerData @@ -24,6 +33,9 @@ if TYPE_CHECKING: from .frame_transformer_cfg import FrameTransformerCfg +# import logger +logger = logging.getLogger(__name__) + class FrameTransformer(SensorBase): """A sensor for reporting frame transforms. @@ -83,6 +95,26 @@ def data(self) -> FrameTransformerData: # 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 """ @@ -94,6 +126,18 @@ def reset(self, env_ids: Sequence[int] | None = 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. """ @@ -109,16 +153,17 @@ def _initialize_impl(self): 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}") + logger.debug(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}") + logger.debug(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 + # 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]] = {} @@ -158,10 +203,10 @@ def _initialize_impl(self): " 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 + # Get the name of the body: use relative prim path for unique identification + body_name = self._get_relative_body_path(matching_prim_path) + # Use leaf name of prim path if frame name isn't specified by user + frame_name = frame if frame is not None else matching_prim_path.rsplit("/", 1)[-1] # Keep track of which frames are associated with which bodies if body_name in body_names_to_frames: @@ -189,12 +234,12 @@ def _initialize_impl(self): target_offsets[frame_name] = {"pos": offset_pos, "quat": offset_quat} if not self._apply_target_frame_offset: - omni.log.info( + logger.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( + logger.info( f"Offsets application needed from '{self.cfg.prim_path}' to the following target frames:" f" {non_identity_offset_frames}" ) @@ -205,9 +250,8 @@ def _initialize_impl(self): body_names_regex = [tracked_prim_path.replace("env_0", "env_*") for tracked_prim_path in tracked_prim_paths] - # Create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") + # 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) @@ -230,8 +274,9 @@ def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: 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 + # 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( @@ -245,15 +290,16 @@ def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: ] 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 + # 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] + # -- target frames: use relative prim path for unique identification + self._target_frame_body_names = [self._get_relative_body_path(prim_path) for prim_path in sorted_prim_paths] - # -- source frame - self._source_frame_body_name = self.cfg.prim_path.split("/")[-1] + # -- source frame: use relative prim path for unique identification + self._source_frame_body_name = self._get_relative_body_path(self.cfg.prim_path) 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 @@ -264,7 +310,8 @@ def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: 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 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: @@ -390,6 +437,7 @@ def _set_debug_vis_impl(self, debug_vis: bool): if debug_vis: if not hasattr(self, "frame_visualizer"): self.frame_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + # set their visibility to true self.frame_visualizer.set_visibility(True) else: @@ -397,9 +445,33 @@ def _set_debug_vis_impl(self, debug_vis: bool): self.frame_visualizer.set_visibility(False) def _debug_vis_callback(self, event): - # Update the visualized markers - if self.frame_visualizer is not None: - self.frame_visualizer.visualize(self._data.target_pos_w.view(-1, 3), self._data.target_quat_w.view(-1, 4)) + # Get the all frames pose + frames_pos = torch.cat([self._data.source_pos_w, self._data.target_pos_w.view(-1, 3)], dim=0) + frames_quat = torch.cat([self._data.source_quat_w, self._data.target_quat_w.view(-1, 4)], dim=0) + + # Get the all connecting lines between frames pose + lines_pos, lines_quat, lines_length = self._get_connecting_lines( + start_pos=self._data.source_pos_w.repeat_interleave(self._data.target_pos_w.size(1), dim=0), + end_pos=self._data.target_pos_w.view(-1, 3), + ) + + # Initialize default (identity) scales and marker indices for all markers (frames + lines) + marker_scales = torch.ones(frames_pos.size(0) + lines_pos.size(0), 3) + marker_indices = torch.zeros(marker_scales.size(0)) + + # Set the z-scale of line markers to represent their actual length + marker_scales[-lines_length.size(0) :, -1] = lines_length + + # Assign marker config index 1 to line markers + marker_indices[-lines_length.size(0) :] = 1 + + # Update the frame and the connecting line visualizer + self.frame_visualizer.visualize( + translations=torch.cat((frames_pos, lines_pos), dim=0), + orientations=torch.cat((frames_quat, lines_quat), dim=0), + scales=marker_scales, + marker_indices=marker_indices, + ) """ Internal simulation callbacks. @@ -410,5 +482,79 @@ def _invalidate_initialize_callback(self, event): # call parent super()._invalidate_initialize_callback(event) # set all existing views to None to invalidate them - self._physics_sim_view = None self._frame_physx_view = None + + """ + Internal helpers. + """ + + def _get_connecting_lines( + self, start_pos: torch.Tensor, end_pos: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Draws connecting lines between frames. + + Given start and end points, this function computes the positions (mid-point), orientations, + and lengths of the connecting lines. + + Args: + start_pos: The start positions of the connecting lines. Shape is (N, 3). + end_pos: The end positions of the connecting lines. Shape is (N, 3). + + Returns: + A tuple containing: + - The positions of each connecting line. Shape is (N, 3). + - The orientations of each connecting line in quaternion. Shape is (N, 4). + - The lengths of each connecting line. Shape is (N,). + """ + direction = end_pos - start_pos + lengths = torch.norm(direction, dim=-1) + positions = (start_pos + end_pos) / 2 + + # Get default direction (along z-axis) + default_direction = torch.tensor([0.0, 0.0, 1.0], device=self.device).expand(start_pos.size(0), -1) + + # Normalize direction vector + direction_norm = normalize(direction) + + # Calculate rotation from default direction to target direction + rotation_axis = torch.linalg.cross(default_direction, direction_norm) + rotation_axis_norm = torch.norm(rotation_axis, dim=-1) + + # Handle case where vectors are parallel + mask = rotation_axis_norm > 1e-6 + rotation_axis = torch.where( + mask.unsqueeze(-1), + normalize(rotation_axis), + torch.tensor([1.0, 0.0, 0.0], device=self.device).expand(start_pos.size(0), -1), + ) + + # Calculate rotation angle + cos_angle = torch.sum(default_direction * direction_norm, dim=-1) + cos_angle = torch.clamp(cos_angle, -1.0, 1.0) + angle = torch.acos(cos_angle) + orientations = quat_from_angle_axis(angle, rotation_axis) + + return positions, orientations, lengths + + @staticmethod + def _get_relative_body_path(prim_path: str) -> str: + """Extract a normalized body path from a prim path. + + Removes the environment instance segment `/envs/env_/` to normalize paths + across multiple environments, while preserving the `/envs/` prefix to + distinguish environment-scoped paths from non-environment paths. + + Examples: + - '/World/envs/env_0/Robot/torso' -> '/World/envs/Robot/torso' + - '/World/envs/env_123/Robot/left_hand' -> '/World/envs/Robot/left_hand' + - '/World/Robot' -> '/World/Robot' + - '/World/Robot_2/left_hand' -> '/World/Robot_2/left_hand' + + Args: + prim_path: The full prim path. + + Returns: + The prim path with `/envs/env_/` removed, preserving `/envs/`. + """ + pattern = re.compile(r"/envs/env_[^/]+/") + return pattern.sub("/envs/", prim_path) diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_cfg.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_cfg.py index 65095257e78b..ca9b528aa1d9 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_cfg.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -33,11 +33,14 @@ class FrameCfg: 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 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). + 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 diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_data.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_data.py index b0819960ab8b..942ddbd5144b 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_data.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer_data.py @@ -1,11 +1,12 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch from dataclasses import dataclass +import torch + @dataclass class FrameTransformerData: diff --git a/source/isaaclab/isaaclab/sensors/imu/__init__.py b/source/isaaclab/isaaclab/sensors/imu/__init__.py index d28447e541bf..7501e41cf49d 100644 --- a/source/isaaclab/isaaclab/sensors/imu/__init__.py +++ b/source/isaaclab/isaaclab/sensors/imu/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sensors/imu/imu.py b/source/isaaclab/isaaclab/sensors/imu/imu.py index 788b3ce45399..e092b39502ee 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu.py @@ -1,17 +1,17 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING -import isaacsim.core.utils.stage as stage_utils -import omni.physics.tensors.impl.api as physx -from pxr import UsdPhysics +import torch + +from isaacsim.core.simulation_manager import SimulationManager +from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils @@ -27,10 +27,13 @@ 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. + The sensor can be attached to any prim path with a rigid ancestor in its tree and produces body-frame + linear acceleration and angular velocity, along with world-frame pose and body-frame linear and angular + accelerations/velocities. + + If the provided path is not a rigid body, the closest rigid-body ancestor is used for simulation queries. + The fixed transform from that ancestor to the target prim is computed once during initialization and + composed with the configured sensor offset. .. note:: @@ -40,10 +43,13 @@ class Imu(SensorBase): .. 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. + The user can configure the sensor offset in the configuration file. The offset is applied relative to the + rigid source prim. If the target prim is not a rigid body, the offset is composed with the fixed transform + from the rigid ancestor to the target prim. The offset is applied in the body frame of the rigid source prim. + The offset is defined as a position vector and a quaternion rotation, which + are applied in the order: position, then rotation. The position is applied as a translation + in the body frame of the rigid source prim, and the rotation is applied as a rotation + in the body frame of the rigid source prim. """ @@ -61,6 +67,9 @@ def __init__(self, cfg: ImuCfg): # Create empty variables for storing output data self._data = ImuData() + # Internal: expression used to build the rigid body view (may be different from cfg.prim_path) + self._rigid_parent_expr: str | None = None + def __str__(self) -> str: """Returns: A string containing information about the instance.""" return ( @@ -96,11 +105,17 @@ def reset(self, env_ids: Sequence[int] | None = None): if env_ids is None: env_ids = slice(None) # reset accumulative data buffers + self._data.pos_w[env_ids] = 0.0 self._data.quat_w[env_ids] = 0.0 + self._data.quat_w[env_ids, 0] = 1.0 + self._data.projected_gravity_b[env_ids] = 0.0 + self._data.projected_gravity_b[env_ids, 2] = -1.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 + self._prev_lin_vel_w[env_ids] = 0.0 + self._prev_ang_vel_w[env_ids] = 0.0 def update(self, dt: float, force_recompute: bool = False): # save timestamp @@ -115,68 +130,107 @@ def update(self, dt: float, force_recompute: bool = False): 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 + - If the target prim path is a rigid body, build the view directly on it. + - Otherwise find the closest rigid-body ancestor, cache the fixed transform from that ancestor + to the target prim, and build the view on the ancestor expression. """ # Initialize parent class super()._initialize_impl() - # create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") + # 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(".*", "*")) + + # Find the first matching ancestor prim that implements rigid body API + ancestor_prim = sim_utils.get_first_matching_ancestor_prim( + prim.GetPath(), predicate=lambda _prim: _prim.HasAPI(UsdPhysics.RigidBodyAPI) + ) + if ancestor_prim is None: + raise RuntimeError(f"Failed to find a rigid body ancestor prim at path expression: {self.cfg.prim_path}") + # Convert ancestor prim path to expression + if ancestor_prim == prim: + self._rigid_parent_expr = self.cfg.prim_path + fixed_pos_b, fixed_quat_b = None, None else: - raise RuntimeError(f"Failed to find a RigidBodyAPI for the prim paths: {self.cfg.prim_path}") + # Convert ancestor prim path to expression + relative_path = prim.GetPath().MakeRelativePath(ancestor_prim.GetPath()).pathString + self._rigid_parent_expr = self.cfg.prim_path.replace(relative_path, "") + # Resolve the relative pose between the target prim and the ancestor prim + fixed_pos_b, fixed_quat_b = sim_utils.resolve_prim_pose(prim, ancestor_prim) + + # Create the rigid body view on the ancestor + self._view = self._physics_sim_view.create_rigid_body_view(self._rigid_parent_expr.replace(".*", "*")) + + # Get world gravity + gravity = self._physics_sim_view.get_gravity() + gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self.device) + gravity_dir = math_utils.normalize(gravity_dir.unsqueeze(0)).squeeze(0) + self.GRAVITY_VEC_W = gravity_dir.repeat(self.num_instances, 1) # Create internal buffers self._initialize_buffers_impl() + # Compose the configured offset with the fixed ancestor->target transform (done once) + # new_offset = fixed * cfg.offset + # where composition is: p = p_fixed + R_fixed * p_cfg, q = q_fixed * q_cfg + if fixed_pos_b is not None and fixed_quat_b is not None: + # Broadcast fixed transform across instances + fixed_p = torch.tensor(fixed_pos_b, device=self._device).repeat(self._view.count, 1) + fixed_q = torch.tensor(fixed_quat_b, device=self._device).repeat(self._view.count, 1) + + cfg_p = self._offset_pos_b.clone() + cfg_q = self._offset_quat_b.clone() + + composed_p = fixed_p + math_utils.quat_apply(fixed_q, cfg_p) + composed_q = math_utils.quat_mul(fixed_q, cfg_q) + + self._offset_pos_b = composed_p + self._offset_quat_b = composed_q + 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 + # world pose of the rigid source (ancestor) from the PhysX view 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") + quat_w = quat_w.roll(1, dims=-1) - # store the poses - self._data.pos_w[env_ids] = pos_w + math_utils.quat_rotate(quat_w, self._offset_pos_b[env_ids]) + # sensor pose in world: apply composed offset + 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 of rigid source (body frame) com_pos_b = self._view.get_coms().to(self.device).split([3, 4], dim=-1)[0] - # obtain the velocities of the link COM + # Velocities at rigid source 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 + + # If sensor offset or COM != link origin, account for angular velocity contribution lin_vel_w += torch.linalg.cross( - ang_vel_w, math_utils.quat_rotate(quat_w, self._offset_pos_b[env_ids] - com_pos_b[env_ids]), dim=-1 + ang_vel_w, math_utils.quat_apply(quat_w, self._offset_pos_b[env_ids] - com_pos_b[env_ids]), dim=-1 ) - # numerical derivative + # numerical derivative (world frame) 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_rotate_inverse(self._data.quat_w[env_ids], lin_vel_w) - self._data.ang_vel_b[env_ids] = math_utils.quat_rotate_inverse(self._data.quat_w[env_ids], ang_vel_w) + + # batch rotate world->body using current sensor orientation + dynamics_data = torch.stack((lin_vel_w, ang_vel_w, lin_acc_w, ang_acc_w, self.GRAVITY_VEC_W[env_ids]), dim=0) + dynamics_data_rot = math_utils.quat_apply_inverse(self._data.quat_w[env_ids].repeat(5, 1), dynamics_data).chunk( + 5, dim=0 + ) + # store the velocities. + self._data.lin_vel_b[env_ids] = dynamics_data_rot[0] + self._data.ang_vel_b[env_ids] = dynamics_data_rot[1] # store the accelerations - self._data.lin_acc_b[env_ids] = math_utils.quat_rotate_inverse(self._data.quat_w[env_ids], lin_acc_w) - self._data.ang_acc_b[env_ids] = math_utils.quat_rotate_inverse(self._data.quat_w[env_ids], ang_acc_w) + self._data.lin_acc_b[env_ids] = dynamics_data_rot[2] + self._data.ang_acc_b[env_ids] = dynamics_data_rot[3] + # store projected gravity + self._data.projected_gravity_b[env_ids] = dynamics_data_rot[4] self._prev_lin_vel_w[env_ids] = lin_vel_w self._prev_ang_vel_w[env_ids] = ang_vel_w @@ -187,6 +241,7 @@ def _initialize_buffers_impl(self): 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.projected_gravity_b = torch.zeros(self._view.count, 3, device=self._device) 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) @@ -194,7 +249,8 @@ def _initialize_buffers_impl(self): 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 + # store sensor offset (applied relative to rigid source). + # This may be composed later with a fixed ancestor->target transform. 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 @@ -206,7 +262,7 @@ 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 + # create markers if necessary for the first time if not hasattr(self, "acceleration_visualizer"): self.acceleration_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) # set their visibility to true @@ -228,12 +284,12 @@ def _debug_vis_callback(self, event): 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() + up_axis = UsdGeom.GetStageUpAxis(self.stage) # 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_rotate(self._data.quat_w, self._data.lin_acc_b), + self._data.pos_w + math_utils.quat_apply(self._data.quat_w, self._data.lin_acc_b), up_axis=up_axis, device=self._device, ) diff --git a/source/isaaclab/isaaclab/sensors/imu/imu_cfg.py b/source/isaaclab/isaaclab/sensors/imu/imu_cfg.py index e8797dd0481c..06aeca5fa95b 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu_cfg.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sensors/imu/imu_data.py b/source/isaaclab/isaaclab/sensors/imu/imu_data.py index 7c56a3808aea..dd06e09a8b79 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu_data.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu_data.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from dataclasses import dataclass +import torch + @dataclass class ImuData: @@ -25,6 +26,12 @@ class ImuData: Shape is (N, 4), where ``N`` is the number of environments. """ + projected_gravity_b: torch.Tensor = None + """Gravity direction unit vector projected on the imu frame. + + Shape is (N,3), 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. diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/__init__.py b/source/isaaclab/isaaclab/sensors/ray_caster/__init__.py index 2eb3a0be5e1b..06f479ed2ee8 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/__init__.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/__init__.py @@ -1,11 +1,27 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""Sub-module for Warp-based ray-cast sensor.""" +"""Sub-module for Warp-based ray-cast sensor. + +The sub-module contains two implementations of the ray-cast sensor: + +- :class:`isaaclab.sensors.ray_caster.RayCaster`: A basic ray-cast sensor that can be used to ray-cast against a single mesh. +- :class:`isaaclab.sensors.ray_caster.MultiMeshRayCaster`: A multi-mesh ray-cast sensor that can be used to ray-cast against + multiple meshes. For these meshes, it tracks their transformations and updates the warp meshes accordingly. + +Corresponding camera implementations are also provided for each of the sensor implementations. Internally, they perform +the same ray-casting operations as the sensor implementations, but return the results as images. +""" from . import patterns +from .multi_mesh_ray_caster import MultiMeshRayCaster +from .multi_mesh_ray_caster_camera import MultiMeshRayCasterCamera +from .multi_mesh_ray_caster_camera_cfg import MultiMeshRayCasterCameraCfg +from .multi_mesh_ray_caster_camera_data import MultiMeshRayCasterCameraData +from .multi_mesh_ray_caster_cfg import MultiMeshRayCasterCfg +from .multi_mesh_ray_caster_data import MultiMeshRayCasterData from .ray_caster import RayCaster from .ray_caster_camera import RayCasterCamera from .ray_caster_camera_cfg import RayCasterCameraCfg diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py new file mode 100644 index 000000000000..39be0d7ca0d8 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py @@ -0,0 +1,427 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +import re +from collections.abc import Sequence +from typing import TYPE_CHECKING, ClassVar + +import numpy as np +import torch +import trimesh +import warp as wp + +import omni.physics.tensors.impl.api as physx + +import isaaclab.sim as sim_utils +from isaaclab.sim.views import XformPrimView +from isaaclab.utils.math import matrix_from_quat, quat_mul +from isaaclab.utils.mesh import PRIMITIVE_MESH_TYPES, create_trimesh_from_geom_mesh, create_trimesh_from_geom_shape +from isaaclab.utils.warp import convert_to_warp_mesh, raycast_dynamic_meshes + +from .multi_mesh_ray_caster_data import MultiMeshRayCasterData +from .ray_cast_utils import obtain_world_pose_from_view +from .ray_caster import RayCaster + +if TYPE_CHECKING: + from .multi_mesh_ray_caster_cfg import MultiMeshRayCasterCfg + +# import logger +logger = logging.getLogger(__name__) + + +class MultiMeshRayCaster(RayCaster): + """A multi-mesh 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 :attr:`meshes` list. The ray-caster then ray-casts against + these warp meshes using the ray pattern provided in the configuration. + + Compared to the default RayCaster, the MultiMeshRayCaster provides additional functionality and flexibility as + an extension of the default RayCaster with the following enhancements: + + - Raycasting against multiple target types : Supports primitive shapes (spheres, cubes, etc.) as well as arbitrary + meshes. + - Dynamic mesh tracking : Keeps track of specified meshes, enabling raycasting against moving parts + (e.g., robot links, articulated bodies, or dynamic obstacles). + - Memory-efficient caching : Avoids redundant memory usage by reusing mesh data across environments. + + Example usage to raycast against the visual meshes of a robot (e.g. ANYmal): + + .. code-block:: python + + ray_caster_cfg = MultiMeshRayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot", + mesh_prim_paths=[ + "/World/Ground", + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/LF_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/RF_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/LH_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/RH_.*/visuals"), + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="{ENV_REGEX_NS}/Robot/base/visuals"), + ], + ray_alignment="world", + pattern_cfg=patterns.GridPatternCfg(resolution=0.02, size=(2.5, 2.5), direction=(0, 0, -1)), + ) + + """ + + cfg: MultiMeshRayCasterCfg + """The configuration parameters.""" + + mesh_offsets: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} + + mesh_views: ClassVar[dict[str, XformPrimView | physx.ArticulationView | physx.RigidBodyView]] = {} + """A dictionary to store mesh views for raycasting, shared across all instances. + + The keys correspond to the prim path for the mesh views, and values are the corresponding view objects. + """ + + def __init__(self, cfg: MultiMeshRayCasterCfg): + """Initializes the ray-caster object. + + Args: + cfg: The configuration parameters. + """ + # Initialize base class + super().__init__(cfg) + + # Create empty variables for storing output data + self._num_meshes_per_env: dict[str, int] = {} + """Keeps track of the number of meshes per env for each ray_cast target. + Since we allow regex indexing (e.g. env_*/object_*) they can differ + """ + + self._raycast_targets_cfg: list[MultiMeshRayCasterCfg.RaycastTargetCfg] = [] + for target in self.cfg.mesh_prim_paths: + # Legacy support for string targets. Treat them as global targets. + if isinstance(target, str): + self._raycast_targets_cfg.append(cfg.RaycastTargetCfg(prim_expr=target, track_mesh_transforms=False)) + else: + self._raycast_targets_cfg.append(target) + + # Resolve regex namespace if set + for cfg in self._raycast_targets_cfg: + cfg.prim_expr = cfg.prim_expr.format(ENV_REGEX_NS="/World/envs/env_.*") + + # overwrite the data class + self._data = MultiMeshRayCasterData() + + 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 : {self._num_envs} x {sum(self._num_meshes_per_env.values())} \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 data(self) -> MultiMeshRayCasterData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + """ + Implementation. + """ + + def _initialize_warp_meshes(self): + """Parse mesh prim expressions, build (or reuse) Warp meshes, and cache per-env mesh IDs. + + High-level steps (per target expression): + + 1. Resolve matching prims by regex/path expression. + 2. Collect supported mesh child prims; merge into a single mesh if configured. + 3. Deduplicate identical vertex buffers (exact match) to avoid uploading duplicates to Warp. + 4. Partition mesh IDs per environment or mark as globally shared. + 5. Optionally create physics views (articulation / rigid body / fallback XForm) and cache local offsets. + + Exceptions: + Raises a RuntimeError if: + + - No prims match the provided expression. + - No supported mesh prims are found under a matched prim. + - Multiple mesh prims are found but merging is disabled. + + """ + multi_mesh_ids: dict[str, list[list[int]]] = {} + for target_cfg in self._raycast_targets_cfg: + # target prim path to ray cast against + target_prim_path = target_cfg.prim_expr + # # check if mesh already casted into warp mesh and skip if so. + if target_prim_path in multi_mesh_ids: + logger.warning( + f"Mesh at target prim path '{target_prim_path}' already exists in the mesh cache. Duplicate entries" + " in `mesh_prim_paths`? This mesh will be skipped." + ) + continue + + # find all matching prim paths to provided expression of the target + target_prims = sim_utils.find_matching_prims(target_prim_path) + if len(target_prims) == 0: + raise RuntimeError(f"Failed to find a prim at path expression: {target_prim_path}") + + # If only one prim is found, treat it as a global prim. + # Either it's a single global object (e.g. ground) or we are only using one env. + is_global_prim = len(target_prims) == 1 + + loaded_vertices: list[np.ndarray | None] = [] + wp_mesh_ids = [] + + for target_prim in target_prims: + # Reuse previously parsed shared mesh instance if possible. + if target_cfg.is_shared and len(wp_mesh_ids) > 0: + # Verify if this mesh has already been registered in an earlier environment. + # Note, this check may fail, if the prim path is not following the env_.* pattern + # Which (worst case) leads to parsing the mesh and skipping registering it at a later stage + curr_prim_base_path = re.sub(r"env_\d+", "env_0", str(target_prim.GetPath())) # + if curr_prim_base_path in MultiMeshRayCaster.meshes: + MultiMeshRayCaster.meshes[str(target_prim.GetPath())] = MultiMeshRayCaster.meshes[ + curr_prim_base_path + ] + # Reuse mesh imported by another ray-cast sensor (global cache). + if str(target_prim.GetPath()) in MultiMeshRayCaster.meshes: + wp_mesh_ids.append(MultiMeshRayCaster.meshes[str(target_prim.GetPath())].id) + loaded_vertices.append(None) + continue + + mesh_prims = sim_utils.get_all_matching_child_prims( + target_prim.GetPath(), lambda prim: prim.GetTypeName() in PRIMITIVE_MESH_TYPES + ["Mesh"] + ) + if len(mesh_prims) == 0: + warn_msg = ( + f"No mesh prims found at path: {target_prim.GetPath()} with supported types:" + f" {PRIMITIVE_MESH_TYPES + ['Mesh']}" + " Skipping this target." + ) + for prim in sim_utils.get_all_matching_child_prims(target_prim.GetPath(), lambda prim: True): + warn_msg += f"\n - Available prim '{prim.GetPath()}' of type '{prim.GetTypeName()}'" + logger.warning(warn_msg) + continue + + trimesh_meshes = [] + + for mesh_prim in mesh_prims: + # check if valid + if mesh_prim is None or not mesh_prim.IsValid(): + raise RuntimeError(f"Invalid mesh prim path: {target_prim}") + + if mesh_prim.GetTypeName() == "Mesh": + mesh = create_trimesh_from_geom_mesh(mesh_prim) + else: + mesh = create_trimesh_from_geom_shape(mesh_prim) + scale = sim_utils.resolve_prim_scale(mesh_prim) + mesh.apply_scale(scale) + + relative_pos, relative_quat = sim_utils.resolve_prim_pose(mesh_prim, target_prim) + relative_pos = torch.tensor(relative_pos, dtype=torch.float32) + relative_quat = torch.tensor(relative_quat, dtype=torch.float32) + + rotation = matrix_from_quat(relative_quat) + transform = np.eye(4) + transform[:3, :3] = rotation.numpy() + transform[:3, 3] = relative_pos.numpy() + mesh.apply_transform(transform) + + # add to list of parsed meshes + trimesh_meshes.append(mesh) + + if len(trimesh_meshes) == 1: + trimesh_mesh = trimesh_meshes[0] + elif target_cfg.merge_prim_meshes: + # combine all trimesh meshes into a single mesh + trimesh_mesh = trimesh.util.concatenate(trimesh_meshes) + else: + raise RuntimeError( + f"Multiple mesh prims found at path: {target_prim.GetPath()} but merging is disabled. Please" + " enable `merge_prim_meshes` in the configuration or specify each mesh separately." + ) + + # check if the mesh is already registered, if so only reference the mesh + registered_idx = _registered_points_idx(trimesh_mesh.vertices, loaded_vertices) + if registered_idx != -1 and self.cfg.reference_meshes: + logger.info("Found a duplicate mesh, only reference the mesh.") + # Found a duplicate mesh, only reference the mesh. + loaded_vertices.append(None) + wp_mesh_ids.append(wp_mesh_ids[registered_idx]) + else: + loaded_vertices.append(trimesh_mesh.vertices) + wp_mesh = convert_to_warp_mesh(trimesh_mesh.vertices, trimesh_mesh.faces, device=self.device) + MultiMeshRayCaster.meshes[str(target_prim.GetPath())] = wp_mesh + wp_mesh_ids.append(wp_mesh.id) + + # print info + if registered_idx != -1: + logger.info(f"Found duplicate mesh for mesh prims under path '{target_prim.GetPath()}'.") + else: + logger.info( + f"Read '{len(mesh_prims)}' mesh prims under path '{target_prim.GetPath()}' with" + f" {len(trimesh_mesh.vertices)} vertices and {len(trimesh_mesh.faces)} faces." + ) + + if is_global_prim: + # reference the mesh for each environment to ray cast against + multi_mesh_ids[target_prim_path] = [wp_mesh_ids] * self._num_envs + self._num_meshes_per_env[target_prim_path] = len(wp_mesh_ids) + else: + # split up the meshes for each environment. Little bit ugly, since + # the current order is interleaved (env1_obj1, env1_obj2, env2_obj1, env2_obj2, ...) + multi_mesh_ids[target_prim_path] = [] + mesh_idx = 0 + n_meshes_per_env = len(wp_mesh_ids) // self._num_envs + self._num_meshes_per_env[target_prim_path] = n_meshes_per_env + for _ in range(self._num_envs): + multi_mesh_ids[target_prim_path].append(wp_mesh_ids[mesh_idx : mesh_idx + n_meshes_per_env]) + mesh_idx += n_meshes_per_env + + if target_cfg.track_mesh_transforms: + MultiMeshRayCaster.mesh_views[target_prim_path], MultiMeshRayCaster.mesh_offsets[target_prim_path] = ( + self._obtain_trackable_prim_view(target_prim_path) + ) + + # throw an error if no meshes are found + if all([target_cfg.prim_expr not in multi_mesh_ids for target_cfg in self._raycast_targets_cfg]): + raise RuntimeError( + f"No meshes found for ray-casting! Please check the mesh prim paths: {self.cfg.mesh_prim_paths}" + ) + + total_n_meshes_per_env = sum(self._num_meshes_per_env.values()) + self._mesh_positions_w = torch.zeros(self._num_envs, total_n_meshes_per_env, 3, device=self.device) + self._mesh_orientations_w = torch.zeros(self._num_envs, total_n_meshes_per_env, 4, device=self.device) + + # Update the mesh positions and rotations + mesh_idx = 0 + for target_cfg in self._raycast_targets_cfg: + n_meshes = self._num_meshes_per_env[target_cfg.prim_expr] + + # update position of the target meshes + pos_w, ori_w = [], [] + for prim in sim_utils.find_matching_prims(target_cfg.prim_expr): + translation, quat = sim_utils.resolve_prim_pose(prim) + pos_w.append(translation) + ori_w.append(quat) + pos_w = torch.tensor(pos_w, device=self.device, dtype=torch.float32).view(-1, n_meshes, 3) + ori_w = torch.tensor(ori_w, device=self.device, dtype=torch.float32).view(-1, n_meshes, 4) + + self._mesh_positions_w[:, mesh_idx : mesh_idx + n_meshes] = pos_w + self._mesh_orientations_w[:, mesh_idx : mesh_idx + n_meshes] = ori_w + mesh_idx += n_meshes + + # flatten the list of meshes that are included in mesh_prim_paths of the specific ray caster + multi_mesh_ids_flattened = [] + for env_idx in range(self._num_envs): + meshes_in_env = [] + for target_cfg in self._raycast_targets_cfg: + meshes_in_env.extend(multi_mesh_ids[target_cfg.prim_expr][env_idx]) + multi_mesh_ids_flattened.append(meshes_in_env) + + self._mesh_views = [ + self.mesh_views[target_cfg.prim_expr] if target_cfg.track_mesh_transforms else None + for target_cfg in self._raycast_targets_cfg + ] + + # save a warp array with mesh ids that is passed to the raycast function + self._mesh_ids_wp = wp.array2d(multi_mesh_ids_flattened, dtype=wp.uint64, device=self.device) + + def _initialize_rays_impl(self): + super()._initialize_rays_impl() + if self.cfg.update_mesh_ids: + self._data.ray_mesh_ids = torch.zeros( + self._num_envs, self.num_rays, 1, device=self.device, dtype=torch.int16 + ) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data. + + Args: + env_ids: The environment ids to update. + """ + + self._update_ray_infos(env_ids) + + # Update the mesh positions and rotations + mesh_idx = 0 + for view, target_cfg in zip(self._mesh_views, self._raycast_targets_cfg): + if not target_cfg.track_mesh_transforms: + mesh_idx += self._num_meshes_per_env[target_cfg.prim_expr] + continue + + # update position of the target meshes + pos_w, ori_w = obtain_world_pose_from_view(view, None) + pos_w = pos_w.squeeze(0) if len(pos_w.shape) == 3 else pos_w + ori_w = ori_w.squeeze(0) if len(ori_w.shape) == 3 else ori_w + + if target_cfg.prim_expr in MultiMeshRayCaster.mesh_offsets: + pos_offset, ori_offset = MultiMeshRayCaster.mesh_offsets[target_cfg.prim_expr] + pos_w -= pos_offset + ori_w = quat_mul(ori_offset.expand(ori_w.shape[0], -1), ori_w) + + count = view.count + if count != 1: # Mesh is not global, i.e. we have different meshes for each env + count = count // self._num_envs + pos_w = pos_w.view(self._num_envs, count, 3) + ori_w = ori_w.view(self._num_envs, count, 4) + + self._mesh_positions_w[:, mesh_idx : mesh_idx + count] = pos_w + self._mesh_orientations_w[:, mesh_idx : mesh_idx + count] = ori_w + mesh_idx += count + + self._data.ray_hits_w[env_ids], _, _, _, mesh_ids = raycast_dynamic_meshes( + self._ray_starts_w[env_ids], + self._ray_directions_w[env_ids], + mesh_ids_wp=self._mesh_ids_wp, # list with shape num_envs x num_meshes_per_env + max_dist=self.cfg.max_distance, + mesh_positions_w=self._mesh_positions_w[env_ids], + mesh_orientations_w=self._mesh_orientations_w[env_ids], + return_mesh_id=self.cfg.update_mesh_ids, + ) + + if self.cfg.update_mesh_ids: + self._data.ray_mesh_ids[env_ids] = mesh_ids + + def __del__(self): + super().__del__() + if RayCaster._instance_count == 0: + MultiMeshRayCaster.mesh_offsets.clear() + MultiMeshRayCaster.mesh_views.clear() + + +""" +Helper functions +""" + + +def _registered_points_idx(points: np.ndarray, registered_points: list[np.ndarray | None]) -> int: + """Check if the points are already registered in the list of registered points. + + Args: + points: The points to check. + registered_points: The list of registered points. + + Returns: + The index of the registered points if found, otherwise -1. + """ + for idx, reg_points in enumerate(registered_points): + if reg_points is None: + continue + if reg_points.shape == points.shape and (reg_points == points).all(): + return idx + return -1 diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera.py new file mode 100644 index 000000000000..970860fa50ae --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera.py @@ -0,0 +1,221 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.utils.warp import raycast_dynamic_meshes + +from .multi_mesh_ray_caster import MultiMeshRayCaster +from .multi_mesh_ray_caster_camera_data import MultiMeshRayCasterCameraData +from .ray_cast_utils import obtain_world_pose_from_view +from .ray_caster_camera import RayCasterCamera + +if TYPE_CHECKING: + from .multi_mesh_ray_caster_camera_cfg import MultiMeshRayCasterCameraCfg + + +class MultiMeshRayCasterCamera(RayCasterCamera, MultiMeshRayCaster): + """A multi-mesh 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. + """ + + cfg: MultiMeshRayCasterCameraCfg + """The configuration parameters.""" + + def __init__(self, cfg: MultiMeshRayCasterCameraCfg): + """Initializes the camera object. + + Args: + cfg: The configuration parameters. + + Raises: + ValueError: If the provided data types are not supported by the ray-caster camera. + """ + self._check_supported_data_types(cfg) + # initialize base class + MultiMeshRayCaster.__init__(self, cfg) + # create empty variables for storing output data + self._data = MultiMeshRayCasterCameraData() + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Multi-Mesh 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(MultiMeshRayCaster.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}" + ) + + """ + Implementation. + """ + + def _initialize_warp_meshes(self): + MultiMeshRayCaster._initialize_warp_meshes(self) + + def _create_buffers(self): + super()._create_buffers() + self._data.image_mesh_ids = torch.zeros( + self._num_envs, *self.image_shape, 1, device=self.device, dtype=torch.int16 + ) + + 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) + + self._data.quat_w = torch.zeros(self._view.count, 4, device=self.device) + self._data.pos_w = torch.zeros(self._view.count, 3, device=self.device) + + self._ray_starts_w = torch.zeros(self._view.count, self.num_rays, 3, device=self.device) + self._ray_directions_w = torch.zeros(self._view.count, self.num_rays, 3, device=self.device) + + def _update_ray_infos(self, env_ids: Sequence[int]): + """Updates the ray information buffers.""" + + # compute poses from current view + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids) + pos_w, quat_w = math_utils.combine_frame_transforms( + pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] + ) + # update the data + self._data.pos_w[env_ids] = pos_w + self._data.quat_w_world[env_ids] = quat_w + self._data.quat_w_ros[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]) + + self._ray_starts_w[env_ids] = ray_starts_w + self._ray_directions_w[env_ids] = ray_directions_w + + def _update_buffers_impl(self, env_ids: Sequence[int] | torch.Tensor | None): + """Fills the buffers of the sensor data.""" + self._update_ray_infos(env_ids) + + # increment frame count + if env_ids is None: + env_ids = torch.arange(self._num_envs, device=self.device) + elif not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, device=self.device) + + self._frame[env_ids] += 1 + + # Update the mesh positions and rotations + mesh_idx = 0 + for view, target_cfg in zip(self._mesh_views, self._raycast_targets_cfg): + if not target_cfg.track_mesh_transforms: + mesh_idx += self._num_meshes_per_env[target_cfg.prim_expr] + continue + + # update position of the target meshes + pos_w, ori_w = obtain_world_pose_from_view(view, None) + pos_w = pos_w.squeeze(0) if len(pos_w.shape) == 3 else pos_w + ori_w = ori_w.squeeze(0) if len(ori_w.shape) == 3 else ori_w + + if target_cfg.prim_expr in MultiMeshRayCaster.mesh_offsets: + pos_offset, ori_offset = MultiMeshRayCaster.mesh_offsets[target_cfg.prim_expr] + pos_w -= pos_offset + ori_w = math_utils.quat_mul(ori_offset.expand(ori_w.shape[0], -1), ori_w) + + count = view.count + if count != 1: # Mesh is not global, i.e. we have different meshes for each env + count = count // self._num_envs + pos_w = pos_w.view(self._num_envs, count, 3) + ori_w = ori_w.view(self._num_envs, count, 4) + + self._mesh_positions_w[:, mesh_idx : mesh_idx + count] = pos_w + self._mesh_orientations_w[:, mesh_idx : mesh_idx + count] = ori_w + mesh_idx += count + + # ray cast and store the hits + self.ray_hits_w[env_ids], ray_depth, ray_normal, _, ray_mesh_ids = raycast_dynamic_meshes( + self._ray_starts_w[env_ids], + self._ray_directions_w[env_ids], + mesh_ids_wp=self._mesh_ids_wp, # list with shape num_envs x num_meshes_per_env + max_dist=self.cfg.max_distance, + mesh_positions_w=self._mesh_positions_w[env_ids], + mesh_orientations_w=self._mesh_orientations_w[env_ids], + 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, + return_mesh_id=self.cfg.update_mesh_ids, + ) + + # 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(self._data.quat_w_world[env_ids]).repeat(1, self.num_rays), + (ray_depth[:, :, None] * self._ray_directions_w[env_ids]), + ) + )[:, :, 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) + + if self.cfg.update_mesh_ids: + self._data.image_mesh_ids[env_ids] = ray_mesh_ids.view(-1, *self.image_shape, 1) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera_cfg.py new file mode 100644 index 000000000000..45df51ce6d80 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera_cfg.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the ray-cast camera sensor.""" + +import logging + +from isaaclab.utils import configclass + +from .multi_mesh_ray_caster_camera import MultiMeshRayCasterCamera +from .multi_mesh_ray_caster_cfg import MultiMeshRayCasterCfg +from .ray_caster_camera_cfg import RayCasterCameraCfg + +# import logger +logger = logging.getLogger(__name__) + + +@configclass +class MultiMeshRayCasterCameraCfg(RayCasterCameraCfg, MultiMeshRayCasterCfg): + """Configuration for the multi-mesh ray-cast camera sensor.""" + + class_type: type = MultiMeshRayCasterCamera + + def __post_init__(self): + super().__post_init__() + + # Camera only supports 'base' ray alignment. Ensure this is set correctly. + if self.ray_alignment != "base": + logger.warning( + "Ray alignment for MultiMeshRayCasterCameraCfg only supports 'base' alignment. Overriding from" + f"'{self.ray_alignment}' to 'base'." + ) + self.ray_alignment = "base" diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera_data.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera_data.py new file mode 100644 index 000000000000..d2f26abdbf47 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera_data.py @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Data container for the multi-mesh ray-cast camera sensor.""" + +import torch + +from isaaclab.sensors.camera import CameraData + +from .ray_caster_data import RayCasterData + + +class MultiMeshRayCasterCameraData(CameraData, RayCasterData): + """Data container for the multi-mesh ray-cast sensor.""" + + image_mesh_ids: torch.Tensor = None + """The mesh ids of the image pixels. + + Shape is (N, H, W, 1), where N is the number of sensors, H and W are the height and width of the image, + and 1 is the number of mesh ids per pixel. + """ diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_cfg.py new file mode 100644 index 000000000000..f5393920162a --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_cfg.py @@ -0,0 +1,71 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Configuration for the ray-cast sensor.""" + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from .multi_mesh_ray_caster import MultiMeshRayCaster +from .ray_caster_cfg import RayCasterCfg + + +@configclass +class MultiMeshRayCasterCfg(RayCasterCfg): + """Configuration for the multi-mesh ray-cast sensor.""" + + @configclass + class RaycastTargetCfg: + """Configuration for different ray-cast targets.""" + + prim_expr: str = MISSING + """The regex to specify the target prim to ray cast against.""" + + is_shared: bool = False + """Whether the target prim is assumed to be the same mesh across all environments. Defaults to False. + + If True, only the first mesh is read and then reused for all environments, rather than re-parsed. + This provides a startup performance boost when there are many environments that all use the same asset. + + .. note:: + If :attr:`MultiMeshRayCasterCfg.reference_meshes` is False, this flag has no effect. + """ + + merge_prim_meshes: bool = True + """Whether to merge the parsed meshes for a prim that contains multiple meshes. Defaults to True. + + This will create a new mesh that combines all meshes in the parsed prim. The raycast hits mesh IDs + will then refer to the single merged mesh. + """ + + track_mesh_transforms: bool = True + """Whether the mesh transformations should be tracked. Defaults to True. + + .. note:: + Not tracking the mesh transformations is recommended when the meshes are static to increase performance. + """ + + class_type: type = MultiMeshRayCaster + + mesh_prim_paths: list[str | RaycastTargetCfg] = MISSING + """The list of mesh primitive paths to ray cast against. + + If an entry is a string, it is internally converted to :class:`RaycastTargetCfg` with + :attr:`~RaycastTargetCfg.track_mesh_transforms` disabled. These settings ensure backwards compatibility + with the default raycaster. + """ + + update_mesh_ids: bool = False + """Whether to update the mesh ids of the ray hits in the :attr:`data` container.""" + + reference_meshes: bool = True + """Whether to reference duplicated meshes instead of loading each one separately into memory. + Defaults to True. + + When enabled, the raycaster parses all meshes in all environments, but reuses references + for duplicates instead of storing multiple copies. This reduces memory footprint. + """ diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_data.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_data.py new file mode 100644 index 000000000000..b9ae187591be --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_data.py @@ -0,0 +1,22 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Data container for the multi-mesh ray-cast sensor.""" + +import torch + +from .ray_caster_data import RayCasterData + + +class MultiMeshRayCasterData(RayCasterData): + """Data container for the multi-mesh ray-cast sensor.""" + + ray_mesh_ids: torch.Tensor = None + """The mesh ids of the ray hits. + + Shape is (N, B, 1), where N is the number of sensors, B is the number of rays + in the scan pattern per sensor. + """ diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/__init__.py b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/__init__.py index 1ab92d411633..d43f5437ce01 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/__init__.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py index d680c28ad32c..cae68833c4f2 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,9 +6,10 @@ from __future__ import annotations import math -import torch from typing import TYPE_CHECKING +import torch + if TYPE_CHECKING: from . import patterns_cfg @@ -109,7 +110,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. @@ -153,7 +154,9 @@ def lidar_pattern(cfg: patterns_cfg.LidarPatternCfg, device: str) -> tuple[torch up_to = None # Horizontal angles - num_horizontal_angles = math.ceil((cfg.horizontal_fov_range[1] - cfg.horizontal_fov_range[0]) / cfg.horizontal_res) + num_horizontal_angles = ( + math.ceil((cfg.horizontal_fov_range[1] - cfg.horizontal_fov_range[0]) / cfg.horizontal_res) + 1 + ) horizontal_angles = torch.linspace(cfg.horizontal_fov_range[0], cfg.horizontal_fov_range[1], num_horizontal_angles)[ :up_to ] diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns_cfg.py index 5a3b33acb7c1..f50ba272b708 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,12 @@ from __future__ import annotations -import torch from collections.abc import Callable, Sequence from dataclasses import MISSING from typing import Literal +import torch + from isaaclab.utils import configclass from . import patterns @@ -137,8 +138,8 @@ def from_intrinsic_matrix( 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. + 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. diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py new file mode 100644 index 000000000000..543276e8ea2a --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utility functions for ray-cast sensors.""" + +from __future__ import annotations + +import torch + +import omni.physics.tensors.impl.api as physx + +from isaaclab.sim.views import XformPrimView +from isaaclab.utils.math import convert_quat + + +def obtain_world_pose_from_view( + physx_view: XformPrimView | physx.ArticulationView | physx.RigidBodyView, + env_ids: torch.Tensor, + clone: bool = False, +) -> tuple[torch.Tensor, torch.Tensor]: + """Get the world poses of the prim referenced by the prim view. + + Args: + physx_view: The prim view to get the world poses from. + env_ids: The environment ids of the prims to get the world poses for. + clone: Whether to clone the returned tensors (default: False). + + Returns: + A tuple containing the world positions and orientations of the prims. + Orientation is in (w, x, y, z) format. + + Raises: + NotImplementedError: If the prim view is not of the supported type. + """ + if isinstance(physx_view, XformPrimView): + pos_w, quat_w = physx_view.get_world_poses(env_ids) + elif isinstance(physx_view, physx.ArticulationView): + pos_w, quat_w = physx_view.get_root_transforms()[env_ids].split([3, 4], dim=-1) + quat_w = convert_quat(quat_w, to="wxyz") + elif isinstance(physx_view, physx.RigidBodyView): + pos_w, quat_w = physx_view.get_transforms()[env_ids].split([3, 4], dim=-1) + quat_w = convert_quat(quat_w, to="wxyz") + else: + raise NotImplementedError(f"Cannot get world poses for prim view of type '{type(physx_view)}'.") + + if clone: + return pos_w.clone(), quat_w.clone() + else: + return pos_w, quat_w diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 4e39b04e16b1..e6735a9f4819 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -1,34 +1,41 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import numpy as np +import logging import re -import torch from collections.abc import Sequence -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, ClassVar -import omni.log -import omni.physics.tensors.impl.api as physx +import numpy as np +import torch import warp as wp -from isaacsim.core.prims import XFormPrim + +import omni +from isaacsim.core.simulation_manager import SimulationManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils from isaaclab.markers import VisualizationMarkers +from isaaclab.sim.views import XformPrimView from isaaclab.terrains.trimesh.utils import make_plane -from isaaclab.utils.math import convert_quat, quat_apply, quat_apply_yaw +from isaaclab.utils.math import quat_apply, quat_apply_yaw from isaaclab.utils.warp import convert_to_warp_mesh, raycast_mesh from ..sensor_base import SensorBase +from .ray_cast_utils import obtain_world_pose_from_view from .ray_caster_data import RayCasterData if TYPE_CHECKING: from .ray_caster_cfg import RayCasterCfg +# import logger +logger = logging.getLogger(__name__) + class RayCaster(SensorBase): """A ray-casting sensor. @@ -49,12 +56,21 @@ class RayCaster(SensorBase): cfg: RayCasterCfg """The configuration parameters.""" + # Class variables to share meshes across instances + meshes: ClassVar[dict[str, wp.Mesh]] = {} + """A dictionary to store warp meshes for raycasting, shared across all instances. + + The keys correspond to the prim path for the meshes, and values are the corresponding warp Mesh objects.""" + _instance_count: ClassVar[int] = 0 + """A counter to track the number of RayCaster instances, used to manage class variable lifecycle.""" + def __init__(self, cfg: RayCasterCfg): """Initializes the ray-caster object. Args: cfg: The configuration parameters. """ + RayCaster._instance_count += 1 # 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]". @@ -62,15 +78,13 @@ def __init__(self, cfg: RayCasterCfg): 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}." + f"Invalid prim path for the ray-caster sensor: {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.""" @@ -78,7 +92,7 @@ def __str__(self) -> str: 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 meshes : {len(RayCaster.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}" @@ -109,8 +123,18 @@ def reset(self, env_ids: Sequence[int] | None = None): # resolve None if env_ids is None: env_ids = slice(None) + num_envs_ids = self._view.count + else: + num_envs_ids = len(env_ids) # resample the drift - self.drift[env_ids] = self.drift[env_ids].uniform_(*self.cfg.drift_range) + r = torch.empty(num_envs_ids, 3, device=self.device) + self.drift[env_ids] = r.uniform_(*self.cfg.drift_range) + # resample the height drift + range_list = [self.cfg.ray_cast_drift_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device=self.device) + self.ray_cast_drift[env_ids] = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (num_envs_ids, 3), device=self.device + ) """ Implementation. @@ -118,30 +142,17 @@ def reset(self, env_ids: Sequence[int] | None = None): def _initialize_impl(self): super()._initialize_impl() - # create simulation view - self._physics_sim_view = physx.create_simulation_view(self._backend) - self._physics_sim_view.set_subspace_roots("/") - # 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 + # obtain global simulation view + + self._physics_sim_view = SimulationManager.get_physics_sim_view() 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}") + available_prims = ",".join([str(p.GetPath()) for p in sim_utils.get_current_stage().Traverse()]) + raise RuntimeError( + f"Failed to find a prim at path expression: {self.cfg.prim_path}. Available prims: {available_prims}" + ) + + self._view, self._offset = self._obtain_trackable_prim_view(self.cfg.prim_path) # load the meshes by parsing the stage self._initialize_warp_meshes() @@ -157,6 +168,10 @@ def _initialize_warp_meshes(self): # read prims to ray-cast for mesh_prim_path in self.cfg.mesh_prim_paths: + # check if mesh already casted into warp mesh + if mesh_prim_path in RayCaster.meshes: + continue + # 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( @@ -181,19 +196,19 @@ def _initialize_warp_meshes(self): indices = np.asarray(mesh_prim.GetFaceVertexIndicesAttr().Get()) wp_mesh = convert_to_warp_mesh(points, indices, device=self.device) # print info - omni.log.info( + logger.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()}.") + logger.info(f"Created infinite plane mesh prim: {mesh_prim.GetPath()}.") # add the warp mesh to the list - self.meshes[mesh_prim_path] = wp_mesh + RayCaster.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]): + if all([mesh_prim_path not in RayCaster.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}" ) @@ -212,53 +227,86 @@ def _initialize_rays_impl(self): 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) + self.ray_cast_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 + 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) + self._ray_starts_w = torch.zeros(self._view.count, self.num_rays, 3, device=self.device) + self._ray_directions_w = torch.zeros(self._view.count, self.num_rays, 3, device=self.device) + + def _update_ray_infos(self, env_ids: Sequence[int]): + """Updates the ray information buffers.""" + + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids) + pos_w, quat_w = math_utils.combine_frame_transforms( + pos_w, quat_w, self._offset[0][env_ids], self._offset[1][env_ids] + ) + # apply drift to ray starting position in world frame 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 + # check if user provided attach_yaw_only flag + if self.cfg.attach_yaw_only is not None: + msg = ( + "Raycaster attribute 'attach_yaw_only' property will be deprecated in a future release." + " Please use the parameter 'ray_alignment' instead." + ) + # set ray alignment to yaw + if self.cfg.attach_yaw_only: + self.cfg.ray_alignment = "yaw" + msg += " Setting ray_alignment to 'yaw'." + else: + self.cfg.ray_alignment = "base" + msg += " Setting ray_alignment to 'base'." + # log the warning + logger.warning(msg) # ray cast based on the sensor poses - if self.cfg.attach_yaw_only: + if self.cfg.ray_alignment == "world": + # apply horizontal drift to ray starting position in ray caster frame + pos_w[:, 0:2] += self.ray_cast_drift[env_ids, 0:2] + # no rotation is considered and directions are not rotated + ray_starts_w = self.ray_starts[env_ids] + ray_starts_w += pos_w.unsqueeze(1) + ray_directions_w = self.ray_directions[env_ids] + elif self.cfg.ray_alignment == "yaw": + # apply horizontal drift to ray starting position in ray caster frame + pos_w[:, 0:2] += quat_apply_yaw(quat_w, self.ray_cast_drift[env_ids])[:, 0:2] # 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: + elif self.cfg.ray_alignment == "base": + # apply horizontal drift to ray starting position in ray caster frame + pos_w[:, 0:2] += quat_apply(quat_w, self.ray_cast_drift[env_ids])[:, 0:2] # 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]) + else: + raise RuntimeError(f"Unsupported ray_alignment type: {self.cfg.ray_alignment}.") + + self._ray_starts_w[env_ids] = ray_starts_w + self._ray_directions_w[env_ids] = ray_directions_w + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data.""" + self._update_ray_infos(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, + self._ray_starts_w[env_ids], + self._ray_directions_w[env_ids], max_dist=self.cfg.max_distance, - mesh=self.meshes[self.cfg.mesh_prim_paths[0]], + mesh=RayCaster.meshes[self.cfg.mesh_prim_paths[0]], )[0] + # apply vertical drift to ray starting position in ray caster frame + self._data.ray_hits_w[env_ids, :, 2] += self.ray_cast_drift[env_ids, 2].unsqueeze(-1) + def _set_debug_vis_impl(self, debug_vis: bool): # set visibility of markers # note: parent only deals with callbacks. not their visibility @@ -272,12 +320,97 @@ def _set_debug_vis_impl(self, debug_vis: bool): self.ray_visualizer.set_visibility(False) def _debug_vis_callback(self, event): + if self._data.ray_hits_w is None: + return # 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 Helpers. + """ + + def _obtain_trackable_prim_view( + self, target_prim_path: str + ) -> tuple[XformPrimView | any, tuple[torch.Tensor, torch.Tensor]]: + """Obtain a prim view that can be used to track the pose of the parget prim. + + The target prim path is a regex expression that matches one or more mesh prims. While we can track its + pose directly using XFormPrim, this is not efficient and can be slow. Instead, we create a prim view + using the physics simulation view, which provides a more efficient way to track the pose of the mesh prims. + + The function additionally resolves the relative pose between the mesh and its corresponding physics prim. + This is especially useful if the mesh is not directly parented to the physics prim. + + Args: + target_prim_path: The target prim path to obtain the prim view for. + + Returns: + A tuple containing: + + - An XFormPrim or a physics prim view (ArticulationView or RigidBodyView). + - A tuple containing the positions and orientations of the mesh prims in the physics prim frame. + + """ + + mesh_prim = sim_utils.find_first_matching_prim(target_prim_path) + current_prim = mesh_prim + current_path_expr = target_prim_path + + prim_view = None + + while prim_view is None: + # TODO: Need to handle the case where API is present but it is disabled + if current_prim.HasAPI(UsdPhysics.ArticulationRootAPI): + prim_view = self._physics_sim_view.create_articulation_view(current_path_expr.replace(".*", "*")) + logger.info(f"Created articulation view for mesh prim at path: {target_prim_path}") + break + + # TODO: Need to handle the case where API is present but it is disabled + if current_prim.HasAPI(UsdPhysics.RigidBodyAPI): + prim_view = self._physics_sim_view.create_rigid_body_view(current_path_expr.replace(".*", "*")) + logger.info(f"Created rigid body view for mesh prim at path: {target_prim_path}") + break + + new_root_prim = current_prim.GetParent() + current_path_expr = current_path_expr.rsplit("/", 1)[0] + if not new_root_prim.IsValid(): + prim_view = XformPrimView(target_prim_path, device=self._device, stage=self.stage) + current_path_expr = target_prim_path + logger.warning( + f"The prim at path {target_prim_path} which is used for raycasting is not a physics prim." + " Defaulting to XFormPrim. \n The pose of the mesh will most likely not" + " be updated correctly when running in headless mode and position lookups will be much slower. \n" + " If possible, ensure that the mesh or its parent is a physics prim (rigid body or articulation)." + ) + break + + # switch the current prim to the parent prim + current_prim = new_root_prim + + # obtain the relative transforms between target prim and the view prims + mesh_prims = sim_utils.find_matching_prims(target_prim_path) + view_prims = sim_utils.find_matching_prims(current_path_expr) + if len(mesh_prims) != len(view_prims): + raise RuntimeError( + f"The number of mesh prims ({len(mesh_prims)}) does not match the number of physics prims" + f" ({len(view_prims)})Please specify the correct mesh and physics prim paths more" + " specifically in your target expressions." + ) + positions = [] + quaternions = [] + for mesh_prim, view_prim in zip(mesh_prims, view_prims): + pos, orientation = sim_utils.resolve_prim_pose(mesh_prim, view_prim) + positions.append(torch.tensor(pos, dtype=torch.float32, device=self.device)) + quaternions.append(torch.tensor(orientation, dtype=torch.float32, device=self.device)) + + positions = torch.stack(positions).to(device=self.device, dtype=torch.float32) + quaternions = torch.stack(quaternions).to(device=self.device, dtype=torch.float32) + + return prim_view, (positions, quaternions) + """ Internal simulation callbacks. """ @@ -287,5 +420,9 @@ def _invalidate_initialize_callback(self, event): # call parent super()._invalidate_initialize_callback(event) # set all existing views to None to invalidate them - self._physics_sim_view = None self._view = None + + def __del__(self): + RayCaster._instance_count -= 1 + if RayCaster._instance_count == 0: + RayCaster.meshes.clear() diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py index ebaca00deca1..e930d3df1837 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py @@ -1,27 +1,31 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch +import logging 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 torch + +from pxr import UsdGeom import isaaclab.utils.math as math_utils from isaaclab.sensors.camera import CameraData from isaaclab.utils.warp import raycast_mesh +from .ray_cast_utils import obtain_world_pose_from_view from .ray_caster import RayCaster if TYPE_CHECKING: from .ray_caster_camera_cfg import RayCasterCameraCfg +# import logger +logger = logging.getLogger(__name__) + class RayCasterCamera(RayCaster): """A ray-casting camera sensor. @@ -86,7 +90,7 @@ def __str__(self) -> str: 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 meshes : {len(RayCaster.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" @@ -143,11 +147,14 @@ 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) + if env_ids is None or isinstance(env_ids, slice): + env_ids = self._ALL_INDICES # 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) + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + pos_w, quat_w = math_utils.combine_frame_transforms( + pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] + ) self._data.pos_w[env_ids] = pos_w self._data.quat_w_world[env_ids] = quat_w # Reset the frame count @@ -184,11 +191,11 @@ def set_world_poses( RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. """ # resolve env_ids - if env_ids is None: + if env_ids is None or isinstance(env_ids, slice): env_ids = self._ALL_INDICES # get current positions - pos_w, quat_w = self._compute_view_world_poses(env_ids) + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids) if positions is not None: # transform to camera frame pos_offset_world_frame = positions - pos_w @@ -201,7 +208,10 @@ def set_world_poses( 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) + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + pos_w, quat_w = math_utils.combine_frame_transforms( + pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] + ) self._data.pos_w[env_ids] = pos_w self._data.quat_w_world[env_ids] = quat_w @@ -220,7 +230,7 @@ def set_world_poses_from_view( 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() + up_axis = UsdGeom.GetStageUpAxis(self.stage) # 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) @@ -260,7 +270,10 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): self._frame[env_ids] += 1 # compute poses from current view - pos_w, quat_w = self._compute_camera_world_poses(env_ids) + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + pos_w, quat_w = math_utils.combine_frame_transforms( + pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] + ) # update the data self._data.pos_w[env_ids] = pos_w self._data.quat_w_world[env_ids] = quat_w @@ -280,7 +293,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): 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]], + mesh=RayCaster.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"]] @@ -345,6 +358,7 @@ def _create_buffers(self): """Create buffers for storing data.""" # prepare drift self.drift = torch.zeros(self._view.count, 3, device=self.device) + self.ray_cast_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) @@ -394,37 +408,49 @@ def _compute_intrinsic_matrices(self): 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. + .. deprecated v2.3.1: + This function will be removed in a future release in favor of implementation + :meth:`obtain_world_pose_from_view`. + 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() + # deprecation + logger.warning( + "The function '_compute_view_world_poses' will be deprecated in favor of the util method" + " 'obtain_world_pose_from_view'. Please use 'obtain_world_pose_from_view' instead...." + ) + + return obtain_world_pose_from_view(self._view, env_ids, clone=True) 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. + .. deprecated v2.3.1: + This function will be removed in a future release. Instead, use the code block below: + + .. code-block:: python + + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + pos_w, quat_w = math_utils.combine_frame_transforms( + pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] + ) + 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 + # deprecation + logger.warning( + "The function '_compute_camera_world_poses' will be deprecated in favor of the combination of methods" + " 'obtain_world_pose_from_view' and 'math_utils.combine_frame_transforms'. Please use" + " 'obtain_world_pose_from_view' and 'math_utils.combine_frame_transforms' instead...." + ) + + # get the pose of the view the camera is attached to + pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + return math_utils.combine_frame_transforms(pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids]) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py index d6fc22e8221e..604c586adcc7 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -32,7 +32,8 @@ class OffsetCfg: 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. + - ``"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. @@ -60,4 +61,4 @@ class OffsetCfg: def __post_init__(self): # for cameras, this quantity should be False always. - self.attach_yaw_only = False + self.ray_alignment = "base" diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index 5b150b606a04..dbdebfad3a5e 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -1,12 +1,12 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Configuration for the ray-cast sensor.""" - from dataclasses import MISSING +from typing import Literal from isaaclab.markers import VisualizationMarkersCfg from isaaclab.markers.config import RAY_CASTER_MARKER_CFG @@ -43,10 +43,32 @@ class OffsetCfg: 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 + attach_yaw_only: bool | None = None """Whether the rays' starting positions and directions only track the yaw orientation. + Defaults to None, which doesn't raise a warning of deprecated usage. This is useful for ray-casting height maps, where only yaw rotation is needed. + + .. deprecated:: 2.1.1 + + This attribute is deprecated and will be removed in the future. Please use + :attr:`ray_alignment` instead. + + To get the same behavior as setting this parameter to ``True`` or ``False``, set + :attr:`ray_alignment` to ``"yaw"`` or "base" respectively. + + """ + + ray_alignment: Literal["base", "yaw", "world"] = "base" + """Specify in what frame the rays are projected onto the ground. Default is "base". + + The options are: + + * ``base`` if the rays' starting positions and directions track the full root position and orientation. + * ``yaw`` if the rays' starting positions and directions track root position and only yaw component of + the orientation. This is useful for ray-casting height maps. + * ``world`` if rays' starting positions and directions are always fixed. This is useful in combination + with a mapping package on the robot and querying ray-casts in a global frame. """ pattern_cfg: PatternBaseCfg = MISSING @@ -56,7 +78,14 @@ class OffsetCfg: """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). + """The range of drift (in meters) to add to the ray starting positions (xyz) in world frame. Defaults to (0.0, 0.0). + + For floating base robots, this is useful for simulating drift in the robot's pose estimation. + """ + + ray_cast_drift_range: dict[str, tuple[float, float]] = {"x": (0.0, 0.0), "y": (0.0, 0.0), "z": (0.0, 0.0)} + """The range of drift (in meters) to add to the projected ray points in local projection frame. Defaults to + a dictionary with zero drift for each x, y and z axis. For floating base robots, this is useful for simulating drift in the robot's pose estimation. """ diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_data.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_data.py index 291ec7ea6376..d63e085e752f 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_data.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_data.py @@ -1,11 +1,12 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch from dataclasses import dataclass +import torch + @dataclass class RayCasterData: diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index 1c005f8caff0..4ece160bbe5a 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,17 +11,22 @@ from __future__ import annotations +import builtins import inspect -import torch +import re import weakref from abc import ABC, abstractmethod from collections.abc import Sequence from typing import TYPE_CHECKING, Any +import torch + import omni.kit.app import omni.timeline +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager import isaaclab.sim as sim_utils +from isaaclab.sim.utils.stage import get_current_stage if TYPE_CHECKING: from .sensor_base_cfg import SensorBaseCfg @@ -56,21 +61,12 @@ def __init__(self, cfg: SensorBaseCfg): self._is_initialized = False # flag for whether the sensor is in visualization mode self._is_visualizing = False + # get stage handle + self.stage = get_current_stage() + + # register various callback functions + self._register_callbacks() - # 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, - ) # 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 @@ -79,16 +75,7 @@ def __init__(self, cfg: SensorBaseCfg): def __del__(self): """Unsubscribe from the callbacks.""" # clear physics events handles - 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 + self._clear_callbacks() """ Properties @@ -230,6 +217,11 @@ def _initialize_impl(self): # Timestamp from last update self._timestamp_last_update = torch.zeros_like(self._timestamp) + # Initialize debug visualization handle + if self._debug_vis_handle is None: + # set initial state of debug visualization + self.set_debug_vis(self.cfg.debug_vis) + @abstractmethod def _update_buffers_impl(self, env_ids: Sequence[int]): """Fills the sensor data for provided environment ids. @@ -262,6 +254,43 @@ def _debug_vis_callback(self, event): Internal simulation callbacks. """ + def _register_callbacks(self): + """Registers the timeline and prim deletion callbacks.""" + + # register simulator callbacks (with weakref safety to avoid crashes on deletion) + def safe_callback(callback_name, event, obj_ref): + """Safely invoke a callback on a weakly-referenced object, ignoring ReferenceError if deleted.""" + try: + obj = obj_ref + getattr(obj, callback_name)(event) + except ReferenceError: + # Object has been deleted; ignore. + pass + + # note: use weakref on callbacks to ensure that this object can be deleted when its destructor is called. + # add callbacks for stage play/stop + obj_ref = weakref.proxy(self) + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + + # the order is set to 10 which is arbitrary but should be lower priority than the default order of 0 + # register timeline PLAY event callback (lower priority with order=10) + self._initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj_ref=obj_ref: safe_callback("_initialize_callback", event, obj_ref), + order=10, + ) + # register timeline STOP event callback (lower priority with order=10) + self._invalidate_initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event, obj_ref=obj_ref: safe_callback("_invalidate_initialize_callback", event, obj_ref), + order=10, + ) + # register prim deletion callback + self._prim_deletion_callback_id = SimulationManager.register_callback( + lambda event, obj_ref=obj_ref: safe_callback("_on_prim_deletion", event, obj_ref), + event=IsaacEvents.PRIM_DELETION, + ) + def _initialize_callback(self, event): """Initializes the scene elements. @@ -270,12 +299,53 @@ def _initialize_callback(self, event): called whenever the simulator "plays" from a "stop" state. """ if not self._is_initialized: - self._initialize_impl() + 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. diff --git a/source/isaaclab/isaaclab/sensors/sensor_base_cfg.py b/source/isaaclab/isaaclab/sensors/sensor_base_cfg.py index a0c56dd280d3..85875b2e4996 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base_cfg.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/__init__.py b/source/isaaclab/isaaclab/sim/__init__.py index 1f261ba736f4..1dc920f4e100 100644 --- a/source/isaaclab/isaaclab/sim/__init__.py +++ b/source/isaaclab/isaaclab/sim/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -32,3 +32,4 @@ from .simulation_context import SimulationContext, build_simulation_context # noqa: F401, F403 from .spawners import * # noqa: F401, F403 from .utils import * # noqa: F401, F403 +from .views import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/sim/converters/__init__.py b/source/isaaclab/isaaclab/sim/converters/__init__.py index e21866653cb9..7503c53bdd85 100644 --- a/source/isaaclab/isaaclab/sim/converters/__init__.py +++ b/source/isaaclab/isaaclab/sim/converters/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/converters/asset_converter_base.py b/source/isaaclab/isaaclab/sim/converters/asset_converter_base.py index d96d540a9ffb..11c200422391 100644 --- a/source/isaaclab/isaaclab/sim/converters/asset_converter_base.py +++ b/source/isaaclab/isaaclab/sim/converters/asset_converter_base.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -40,8 +40,8 @@ class AssetConverterBase(abc.ABC): .. 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. + :obj:`AssetConverterBaseCfg.usd_file_name` are not considered as modifications in the configuration instance + that trigger the USD file re-generation. """ diff --git a/source/isaaclab/isaaclab/sim/converters/asset_converter_base_cfg.py b/source/isaaclab/isaaclab/sim/converters/asset_converter_base_cfg.py index 7741ddd92d61..79bb8d17d41c 100644 --- a/source/isaaclab/isaaclab/sim/converters/asset_converter_base_cfg.py +++ b/source/isaaclab/isaaclab/sim/converters/asset_converter_base_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py index fb6adbaef3dc..4a79a908bab1 100644 --- a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py @@ -1,21 +1,24 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import asyncio +import logging 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 +from isaaclab.sim.utils import delete_prim, export_prim_to_file + +# import logger +logger = logging.getLogger(__name__) class MeshConverter(AssetConverterBase): @@ -29,7 +32,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: @@ -87,7 +90,7 @@ def _convert_asset(self, cfg: MeshConverterCfg): # 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( + logger.warning( 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." ) @@ -122,14 +125,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 @@ -169,7 +173,7 @@ def _convert_asset(self, cfg: MeshConverterCfg): ) # 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) + delete_prim(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) @@ -216,7 +220,6 @@ async def _convert_mesh_to_usd(in_file: str, out_file: str, load_materials: bool enable_extension("omni.kit.asset_converter") import omni.kit.asset_converter - import omni.usd # Create converter context converter_context = omni.kit.asset_converter.AssetConverterContext() diff --git a/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py b/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py index c7f603e309f2..3d231ac1efee 100644 --- a/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py +++ b/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -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 19dd3452bf7d..17808f59b948 100644 --- a/source/isaaclab/isaaclab/sim/converters/mjcf_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/mjcf_converter.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,14 +6,16 @@ from __future__ import annotations import os +from typing import TYPE_CHECKING -import isaacsim import omni.kit.commands -import omni.usd from .asset_converter_base import AssetConverterBase from .mjcf_converter_cfg import MjcfConverterCfg +if TYPE_CHECKING: + import isaacsim.asset.importer.mjcf + class MjcfConverter(AssetConverterBase): """Converter for a MJCF description file to a USD file. @@ -31,7 +33,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 @@ -65,7 +67,7 @@ def _convert_asset(self, cfg: MjcfConverterCfg): prim_path=f"/{file_basename}", ) - def _get_mjcf_import_config(self) -> isaacsim.asset.importer.mjcf.ImportConfig: + def _get_mjcf_import_config(self) -> isaacsim.asset.importer.mjcf._mjcf.ImportConfig: """Returns the import configuration for MJCF to USD conversion. Returns: diff --git a/source/isaaclab/isaaclab/sim/converters/mjcf_converter_cfg.py b/source/isaaclab/isaaclab/sim/converters/mjcf_converter_cfg.py index d61694086d6e..7cbd83e3e9f2 100644 --- a/source/isaaclab/isaaclab/sim/converters/mjcf_converter_cfg.py +++ b/source/isaaclab/isaaclab/sim/converters/mjcf_converter_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/converters/urdf_converter.py b/source/isaaclab/isaaclab/sim/converters/urdf_converter.py index 401654102c78..ba80f520355e 100644 --- a/source/isaaclab/isaaclab/sim/converters/urdf_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/urdf_converter.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,16 +7,21 @@ import math import re +from typing import TYPE_CHECKING + +from packaging.version import Version -import isaacsim import omni.kit.app import omni.kit.commands -import omni.usd -from isaacsim.core.utils.extensions import enable_extension + +from isaaclab.utils.version import get_isaac_sim_version from .asset_converter_base import AssetConverterBase from .urdf_converter_cfg import UrdfConverterCfg +if TYPE_CHECKING: + import isaacsim.asset.importer.urdf + class UrdfConverter(AssetConverterBase): """Converter for a URDF description file to a USD file. @@ -32,9 +37,19 @@ class UrdfConverter(AssetConverterBase): .. 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``. - .. _isaacsim.asset.importer.urdf: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/ext_isaacsim_asset_importer_urdf.html + .. note:: + In Isaac Sim 5.1, the URDF importer changed the default behavior of merging fixed joints. + Links connected through ``fixed_joint`` elements are no longer merged when their URDF link + entries specify mass and inertia, even if ``merge-joint`` is set to True. The new behavior + treats links with mass/inertia as full bodies rather than zero-mass reference frames. + + To maintain backwards compatibility, **this converter pins to an older version of the + URDF importer extension** (version 2.4.31) that still merges fixed joints by default. + This allows existing URDFs to work as expected without modification. + + .. _isaacsim.asset.importer.urdf: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/ext_isaacsim_asset_importer_urdf.html """ cfg: UrdfConverterCfg @@ -46,9 +61,13 @@ def __init__(self, cfg: UrdfConverterCfg): 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") + # switch to older version of the URDF importer extension + if get_isaac_sim_version() >= Version("5.1"): + manager = omni.kit.app.get_app().get_extension_manager() + if not manager.is_extension_enabled("isaacsim.asset.importer.urdf-2.4.31"): + manager.set_extension_enabled_immediate("isaacsim.asset.importer.urdf-2.4.31", True) + + # acquire the URDF interface from isaacsim.asset.importer.urdf._urdf import acquire_urdf_interface self._urdf_interface = acquire_urdf_interface() @@ -95,7 +114,7 @@ def _convert_asset(self, cfg: UrdfConverterCfg): Helper methods. """ - def _get_urdf_import_config(self) -> isaacsim.asset.importer.urdf.ImportConfig: + def _get_urdf_import_config(self) -> isaacsim.asset.importer.urdf._urdf.ImportConfig: """Create and fill URDF ImportConfig with desired settings Returns: @@ -121,6 +140,7 @@ def _get_urdf_import_config(self) -> isaacsim.asset.importer.urdf.ImportConfig: 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) + import_config.set_merge_fixed_ignore_inertia(self.cfg.merge_fixed_joints) # -- physics settings # create fix joint for base link import_config.set_fix_base(self.cfg.fix_base) diff --git a/source/isaaclab/isaaclab/sim/converters/urdf_converter_cfg.py b/source/isaaclab/isaaclab/sim/converters/urdf_converter_cfg.py index f345eb046d12..c04ede2400ae 100644 --- a/source/isaaclab/isaaclab/sim/converters/urdf_converter_cfg.py +++ b/source/isaaclab/isaaclab/sim/converters/urdf_converter_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -97,7 +97,9 @@ class NaturalFrequencyGainsCfg: """ 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.""" + """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.""" diff --git a/source/isaaclab/isaaclab/sim/schemas/__init__.py b/source/isaaclab/isaaclab/sim/schemas/__init__.py index 20f02aa9e69b..c8402fdb13c8 100644 --- a/source/isaaclab/isaaclab/sim/schemas/__init__.py +++ b/source/isaaclab/isaaclab/sim/schemas/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -33,11 +33,15 @@ """ from .schemas import ( + MESH_APPROXIMATION_TOKENS, + 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,14 +49,79 @@ 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", + "MESH_APPROXIMATION_TOKENS", +] diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas.py b/source/isaaclab/isaaclab/sim/schemas/schemas.py index fe8ced7e79eb..91eb3b2cd6a9 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,14 +6,17 @@ # needed to import for allowing type-hinting: Usd.Stage | None from __future__ import annotations +import logging import math +from collections.abc import Callable +from typing import Any -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 isaaclab.sim.utils.stage import get_current_stage + from ..utils import ( apply_nested, find_global_fixed_joint_prim, @@ -22,6 +25,46 @@ ) from . import schemas_cfg +# import logger +logger = logging.getLogger(__name__) + + +""" +Constants. +""" + +# Mapping from string names to USD/PhysX tokens for mesh collision approximation +# Refer to omniverse documentation +# https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/dev_guide/rigid_bodies_articulations/collision.html#mesh-geometry-colliders +# for available tokens. +MESH_APPROXIMATION_TOKENS = { + "boundingCube": UsdPhysics.Tokens.boundingCube, + "boundingSphere": UsdPhysics.Tokens.boundingSphere, + "convexDecomposition": UsdPhysics.Tokens.convexDecomposition, + "convexHull": UsdPhysics.Tokens.convexHull, + "none": UsdPhysics.Tokens.none, + "meshSimplification": UsdPhysics.Tokens.meshSimplification, + "sdf": PhysxSchema.Tokens.sdf, +} + + +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, +] + + """ Articulation root properties. """ @@ -44,9 +87,10 @@ def define_articulation_root_properties( ValueError: When the prim path is not valid. TypeError: When the prim already has conflicting API schemas. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get articulation USD prim prim = stage.GetPrimAtPath(prim_path) # check if prim path is valid @@ -102,9 +146,10 @@ def modify_articulation_root_properties( Raises: NotImplementedError: When the root prim is not a rigid body and a fixed joint is to be created. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get articulation USD prim articulation_prim = stage.GetPrimAtPath(prim_path) # check if prim has articulation applied on it @@ -133,12 +178,12 @@ def modify_articulation_root_properties( # 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( + logger.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}'.") + logger.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 @@ -204,9 +249,10 @@ def define_rigid_body_properties( ValueError: When the prim path is not valid. TypeError: When the prim already has conflicting API schemas. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim prim = stage.GetPrimAtPath(prim_path) # check if prim path is valid @@ -250,9 +296,10 @@ def modify_rigid_body_properties( Returns: True if the properties were successfully set, False otherwise. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get rigid-body USD prim rigid_body_prim = stage.GetPrimAtPath(prim_path) # check if prim has rigid-body applied on it @@ -299,9 +346,10 @@ def define_collision_properties( Raises: ValueError: When the prim path is not valid. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim prim = stage.GetPrimAtPath(prim_path) # check if prim path is valid @@ -343,9 +391,10 @@ def modify_collision_properties( Returns: True if the properties were successfully set, False otherwise. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim collider_prim = stage.GetPrimAtPath(prim_path) # check if prim has collision applied on it @@ -390,9 +439,10 @@ def define_mass_properties(prim_path: str, cfg: schemas_cfg.MassPropertiesCfg, s Raises: ValueError: When the prim path is not valid. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim prim = stage.GetPrimAtPath(prim_path) # check if prim path is valid @@ -435,9 +485,10 @@ def modify_mass_properties(prim_path: str, cfg: schemas_cfg.MassPropertiesCfg, s Returns: True if the properties were successfully set, False otherwise. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim rigid_prim = stage.GetPrimAtPath(prim_path) # check if prim has mass API applied on it @@ -478,9 +529,10 @@ def activate_contact_sensors(prim_path: str, threshold: float = 0.0, stage: Usd. ValueError: If the input prim path is not valid. ValueError: If there are no rigid bodies under the prim path. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get prim prim: Usd.Prim = stage.GetPrimAtPath(prim_path) # check if prim is valid @@ -502,10 +554,10 @@ def activate_contact_sensors(prim_path: str, threshold: float = 0.0, stage: Usd. 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()}'") + logger.debug(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()}'") + logger.debug(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) @@ -564,9 +616,10 @@ def modify_joint_drive_properties( Raises: ValueError: If the input prim path is not valid. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim prim = stage.GetPrimAtPath(prim_path) # check if prim path is valid @@ -666,9 +719,10 @@ def modify_fixed_tendon_properties( Raises: ValueError: If the input prim path is not valid. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim tendon_prim = stage.GetPrimAtPath(prim_path) # check if prim has fixed tendon applied on it @@ -694,6 +748,77 @@ def modify_fixed_tendon_properties( return True +""" +Spatial tendon properties. +""" + + +@apply_nested +def modify_spatial_tendon_properties( + prim_path: str, cfg: schemas_cfg.SpatialTendonPropertiesCfg, stage: Usd.Stage | None = None +) -> bool: + """Modify PhysX parameters for a spatial tendon attachment prim. + + A `spatial 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. + + .. _spatial tendon: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxArticulationSpatialTendon.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 + .. _PhysxTendonAttachmentRootAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_attachment_root_a_p_i.html + .. _PhysxTendonAttachmentLeafAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_attachment_leaf_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 = get_current_stage() + # get USD prim + tendon_prim = stage.GetPrimAtPath(prim_path) + # check if prim has spatial tendon applied on it + has_spatial_tendon = tendon_prim.HasAPI(PhysxSchema.PhysxTendonAttachmentRootAPI) or tendon_prim.HasAPI( + PhysxSchema.PhysxTendonAttachmentLeafAPI + ) + if not has_spatial_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 spatial tendon schema + # retrieve the USD tendon api + if "PhysxTendonAttachmentRootAPI" in schema_name: + instance_name = schema_name.split(":")[-1] + physx_tendon_spatial_api = PhysxSchema.PhysxTendonAttachmentRootAPI(tendon_prim, instance_name) + elif "PhysxTendonAttachmentLeafAPI" in schema_name: + instance_name = schema_name.split(":")[-1] + physx_tendon_spatial_api = PhysxSchema.PhysxTendonAttachmentLeafAPI(tendon_prim, instance_name) + else: + continue + # 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_spatial_api, attr_name, value, camel_case=True) + # success + return True + + """ Deformable body properties. """ @@ -721,9 +846,10 @@ def define_deformable_body_properties( ValueError: When the prim path is not valid. ValueError: When the prim has no mesh or multiple meshes. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim prim = stage.GetPrimAtPath(prim_path) # check if prim path is valid @@ -796,9 +922,9 @@ def modify_deformable_body_properties( Returns: True if the properties were successfully set, False otherwise. """ - # obtain stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() # get deformable-body USD prim deformable_body_prim = stage.GetPrimAtPath(prim_path) @@ -851,3 +977,150 @@ def modify_deformable_body_properties( # success return True + + +""" +Collision mesh properties. +""" + + +def extract_mesh_collision_api_and_attrs( + cfg: schemas_cfg.MeshCollisionPropertiesCfg, +) -> tuple[Callable, dict[str, Any]]: + """Extract the mesh collision API function and custom attributes from the configuration. + + Args: + cfg: The configuration for the mesh collision properties. + + Returns: + A tuple containing the API function to use and a dictionary of custom attributes. + + Raises: + ValueError: When neither USD nor PhysX API can be determined to be used. + """ + # 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", "mesh_approximation_name"] + } + + 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!") + + if use_usd_api: + # Use USD API for corresponding attributes + # For mesh collision approximation attribute, we set it explicitly in `modify_mesh_collision_properties`` + api_func = cfg.usd_func + elif use_phsyx_api: + api_func = cfg.physx_func + else: + raise ValueError("Either USD or PhysX API should be used for modifying mesh collision attributes!") + + 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 +) -> bool: + """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. + Returns: + True if the properties were successfully set, False otherwise. + Raises: + ValueError: When the mesh approximation name is invalid. + """ + # obtain stage + if stage is None: + stage = get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + + # we need MeshCollisionAPI to set mesh collision approximation attribute + if not UsdPhysics.MeshCollisionAPI(prim): + UsdPhysics.MeshCollisionAPI.Apply(prim) + # convert mesh approximation string to token + approximation_name = cfg.mesh_approximation_name + if approximation_name not in MESH_APPROXIMATION_TOKENS: + raise ValueError( + f"Invalid mesh approximation name: '{approximation_name}'. " + f"Valid options are: {list(MESH_APPROXIMATION_TOKENS.keys())}" + ) + approximation_token = MESH_APPROXIMATION_TOKENS[approximation_name] + safe_set_attribute_on_usd_schema( + UsdPhysics.MeshCollisionAPI(prim), "Approximation", approximation_token, camel_case=False + ) + + 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 08fdfae434d0..446d7faa105d 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py @@ -1,10 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from dataclasses import MISSING from typing import Literal +from pxr import PhysxSchema, UsdPhysics + from isaaclab.utils import configclass @@ -264,6 +267,37 @@ class FixedTendonPropertiesCfg: """Spring rest length of the tendon.""" +@configclass +class SpatialTendonPropertiesCfg: + """Properties to define spatial tendons of an articulation. + + See :meth:`modify_spatial_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. + """ + + @configclass class DeformableBodyPropertiesCfg: """Properties to apply to a deformable body. @@ -395,3 +429,251 @@ 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 + """USD API function for modifying mesh collision properties. + Refer to + `original USD Documentation `_ + for more information. + """ + + physx_func: callable = MISSING + """PhysX API function for modifying mesh collision properties. + Refer to + `original PhysX Documentation `_ + for more information. + """ + + mesh_approximation_name: str = "none" + """Name of mesh collision approximation method. Default: "none". + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + +@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 + """ + + mesh_approximation_name: str = "boundingCube" + """Name of mesh collision approximation method. Default: "boundingCube". + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + +@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 + """ + + mesh_approximation_name: str = "boundingSphere" + """Name of mesh collision approximation method. Default: "boundingSphere". + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + +@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 + """ + + mesh_approximation_name: str = "convexDecomposition" + """Name of mesh collision approximation method. Default: "convexDecomposition". + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + 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 + """ + + mesh_approximation_name: str = "convexHull" + """Name of mesh collision approximation method. Default: "convexHull". + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + 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 + """ + + mesh_approximation_name: str = "none" + """Name of mesh collision approximation method. Default: "none" (uses triangle mesh). + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + 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 + """ + + mesh_approximation_name: str = "meshSimplification" + """Name of mesh collision approximation method. Default: "meshSimplification". + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + 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 + """ + + mesh_approximation_name: str = "sdf" + """Name of mesh collision approximation method. Default: "sdf". + Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. + """ + + 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 2b6153c44bf6..06ed4826af68 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,7 +9,7 @@ configuring the environment instances, viewer settings, and simulation parameters. """ -from typing import Literal +from typing import Any, Literal from isaaclab.utils import configclass @@ -40,7 +40,27 @@ class PhysxCfg: Available solvers: * :obj:`0`: PGS (Projective Gauss-Seidel) - * :obj:`1`: TGS (Truncated Gauss-Seidel) + * :obj:`1`: TGS (Temporal Gauss-Seidel) + """ + + 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. + """ min_position_iteration_count: int = 1 @@ -87,8 +107,31 @@ class PhysxCfg: """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_stabilization: bool = False + """Enable/disable additional stabilization pass in solver. Default is False. + + .. note:: + + We recommend setting this flag to true only when the simulation step size is large + (i.e., less than 30 Hz or more than 0.0333 seconds). + + .. warning:: + + Enabling this flag may lead to incorrect contact forces report from the contact sensor. + """ + + enable_external_forces_every_iteration: bool = False + """Enable/disable external forces every position iteration in the TGS solver. Default is False. + + When using the TGS solver (:attr:`solver_type` is 1), this flag allows enabling external forces every solver + position iteration. This can help improve the accuracy of velocity updates. Consider enabling this flag if + the velocities generated by the simulation are noisy. Increasing the number of velocity iterations, together + with this flag, can help improve the accuracy of velocity updates. + + .. note:: + + This flag is ignored when using the PGS solver (:attr:`solver_type` is 0). + """ enable_enhanced_determinism: bool = False """Enable/disable improved determinism at the expense of performance. Defaults to False. @@ -156,9 +199,14 @@ class PhysxCfg: 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. + 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``: Setting used when running the simulation with the GUI enabled. + * ``apps/isaaclab.python.headless.rendering.kit``: Setting used when running the simulation in headless mode. + + Setting any value here will override the defaults of the experience files. For more information, see the `Omniverse RTX Renderer documentation`_. @@ -166,78 +214,134 @@ class RenderCfg: """ enable_translucency: bool | None = None - """Enables translucency for specular transmissive surfaces such as glass at the cost of some performance. Default is False. + """Enables translucency for specular transmissive surfaces such as glass at the cost of some performance. + Default is False. - Set variable: /rtx/translucency/enabled + This is set by the 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 + This is set by the 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 + This is set by the 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 + - **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. + + This is set by the 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. + """"Enables the use of DLSS-G. Default is False. - Set variable: /rtx-transient/dlssg/enabled + 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. + + .. note:: + + This feature requires an Ada Lovelace architecture GPU. Enabling this feature also enables additional + thread-related activities, which can hurt performance. + + This is set by the 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 + The DL denoiser can help improve the quality of renders, but comes at a cost of performance. + + This is set by the 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. + """For DLSS anti-aliasing, selects the performance/quality tradeoff mode. Default is 0. - Set variable: /rtx/post/dlss/execMode + Valid values are: + + * 0 (Performance) + * 1 (Balanced) + * 2 (Quality) + * 3 (Auto) + + This is set by the variable: ``/rtx/post/dlss/execMode``. """ enable_direct_lighting: bool | None = None - """Enable direct light contributions from lights. + """Enable direct light contributions from lights. Default is False. - Set variable: /rtx/directLighting/enabled + This is set by the 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. + """Defines the Direct Lighting samples per pixel. Default is 1. - Set variable: /rtx/directLighting/sampledLighting/samplesPerPixel""" + A higher value increases the direct lighting quality at the cost of performance. + + This is set by the 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. + """Enables shadows at the cost of performance. Defaults to True. - Set variable: /rtx/shadows/enabled + When disabled, lights will not cast shadows. + + This is set by the 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 + 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. + + The keys of the dictionary 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"] | None = None + """The rendering mode. + + This behaves the same as the passing the CLI arg ``--rendering_mode`` to an executable script. """ @@ -298,6 +402,11 @@ class SimulationCfg: 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`. + + When using GPU simulation, it is required to enable Fabric to visualize updates in the renderer. + Transform updates are propagated to the renderer through Fabric. If Fabric is disabled with GPU simulation, + the renderer will not be able to render any updates in the simulation, although simulation will still be + running under the hood. """ physx: PhysxCfg = PhysxCfg() @@ -314,3 +423,22 @@ class SimulationCfg: render: RenderCfg = RenderCfg() """Render settings. Default is RenderCfg().""" + + create_stage_in_memory: bool = False + """If stage is first created in memory. Default is False. + + Creating the stage in memory can reduce start-up time. + """ + + logging_level: Literal["DEBUG", "INFO", "WARNING", "ERROR", "CRITICAL"] = "WARNING" + """The logging level. Default is "WARNING".""" + + save_logs_to_file: bool = True + """Save logs to a file. Default is True.""" + + log_dir: str | None = None + """The directory to save the logs to. Default is None. + + If :attr:`save_logs_to_file` is True, the logs will be saved to the directory specified by :attr:`log_dir`. + If None, the logs will be saved to the temp directory. + """ diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 1cd058a03846..d022c2d32a75 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -1,32 +1,46 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import builtins import enum -import numpy as np -import sys -import torch +import glob +import logging +import os +import re +import time import traceback import weakref from collections.abc import Iterator from contextlib import contextmanager +from datetime import datetime from typing import Any +import flatdict +import numpy as np +import toml +import torch + import carb -import isaacsim.core.utils.stage as stage_utils -import omni.log 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.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, UsdUtils + +import isaaclab.sim as sim_utils +from isaaclab.utils.logger import configure_logging +from isaaclab.utils.version import get_isaac_sim_version from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg from .utils import bind_physics_material +# import logger +logger = logging.getLogger(__name__) + class SimulationContext(_SimulationContext): """A class to control simulation-related events such as physics stepping and rendering. @@ -86,8 +100,8 @@ class RenderMode(enum.IntEnum): 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_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. @@ -118,82 +132,55 @@ def __init__(self, cfg: SimulationCfg | None = None): cfg.validate() self.cfg = cfg # check that simulation is running - if stage_utils.get_current_stage() is None: + if sim_utils.get_current_stage() is None: raise RuntimeError("The stage has not been created. Did you run the simulator?") - # set flags for simulator + # setup logger + self.logger = configure_logging( + logging_level=self.cfg.logging_level, + save_logs_to_file=self.cfg.save_logs_to_file, + log_dir=self.cfg.log_dir, + ) + + # create stage in memory if requested + if self.cfg.create_stage_in_memory: + self._initial_stage = sim_utils.create_new_stage_in_memory() + else: + self._initial_stage = omni.usd.get_context().get_stage() + # cache stage if it is not already cached + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(self._initial_stage).ToLongInt() + if stage_id < 0: + stage_cache.Insert(self._initial_stage) + # acquire settings interface - carb_settings_iface = carb.settings.get_settings() - # enable hydra scene-graph instancing - # note: this allows rendering of instanceable assets on the GUI - carb_settings_iface.set_bool("/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 - carb_settings_iface.set_bool("/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. - carb_settings_iface.set_bool("/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 - carb_settings_iface.set_bool("/physics/collisionConeCustomGeometry", False) - carb_settings_iface.set_bool("/physics/collisionCylinderCustomGeometry", False) - # hide the Simulation Settings window - carb_settings_iface.set_bool("/physics/autoPopupSimulationOutputWindow", False) + 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 = carb_settings_iface.get("/app/window/enabled") + self._local_gui = self.carb_settings.get("/app/window/enabled") # read flag for whether livestreaming GUI is enabled - self._livestream_gui = carb_settings_iface.get("/app/livestream/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 flags anim recording config and init timestamps + self._setup_anim_recording() # 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(carb_settings_iface.get("/isaaclab/render/offscreen")) + 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(carb_settings_iface.get("/isaaclab/render/active_viewport")) + 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 + self._has_gui = self._local_gui or self._livestream_gui or self._xr_gui # apply render settings from render config - if self.cfg.render.enable_translucency is not None: - carb_settings_iface.set_bool("/rtx/translucency/enabled", self.cfg.render.enable_translucency) - if self.cfg.render.enable_reflections is not None: - carb_settings_iface.set_bool("/rtx/reflections/enabled", self.cfg.render.enable_reflections) - if self.cfg.render.enable_global_illumination is not None: - carb_settings_iface.set_bool("/rtx/indirectDiffuse/enabled", self.cfg.render.enable_global_illumination) - if self.cfg.render.enable_dlssg is not None: - carb_settings_iface.set_bool("/rtx-transient/dlssg/enabled", self.cfg.render.enable_dlssg) - if self.cfg.render.enable_dl_denoiser is not None: - carb_settings_iface.set_bool("/rtx-transient/dldenoiser/enabled", self.cfg.render.enable_dl_denoiser) - if self.cfg.render.dlss_mode is not None: - carb_settings_iface.set_int("/rtx/post/dlss/execMode", self.cfg.render.dlss_mode) - if self.cfg.render.enable_direct_lighting is not None: - carb_settings_iface.set_bool("/rtx/directLighting/enabled", self.cfg.render.enable_direct_lighting) - if self.cfg.render.samples_per_pixel is not None: - carb_settings_iface.set_int( - "/rtx/directLighting/sampledLighting/samplesPerPixel", self.cfg.render.samples_per_pixel - ) - if self.cfg.render.enable_shadows is not None: - carb_settings_iface.set_bool("/rtx/shadows/enabled", self.cfg.render.enable_shadows) - if self.cfg.render.enable_ambient_occlusion is not None: - carb_settings_iface.set_bool("/rtx/ambientOcclusion/enabled", self.cfg.render.enable_ambient_occlusion) - # set denoiser mode - try: - import omni.replicator.core as rep - - rep.settings.set_render_rtx_realtime(antialiasing=self.cfg.render.antialiasing_mode) - except Exception: - pass + self._apply_render_settings_from_cfg() # store the default render mode if not self._has_gui and not self._offscreen_render: @@ -246,9 +233,6 @@ def __init__(self, cfg: SimulationCfg | None = None): # 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. @@ -256,17 +240,21 @@ def __init__(self, cfg: SimulationCfg | None = None): # 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_callback(*args), + 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() @@ -274,16 +262,65 @@ def __init__(self, cfg: SimulationCfg | None = None): if "physx" in sim_params: physx_params = sim_params.pop("physx") sim_params.update(physx_params) + + # add warning about enabling stabilization for large step sizes + if not self.cfg.physx.enable_stabilization and (self.cfg.dt > 0.0333): + self.logger.warning( + "Large simulation step size (> 0.0333 seconds) is not recommended without enabling stabilization." + " Consider setting the `enable_stabilization` flag to True in the PhysxCfg, or reducing the" + " 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 - 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, - ) + if get_isaac_sim_version().major < 5: + # stage arg is not supported before isaac sim 5.0 + 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, + ) + else: + 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, + stage=self._initial_stage, + ) + + """ + Properties - Override. + """ + + @property + def device(self) -> str: + """Device used by the simulation. + + 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. @@ -327,15 +364,26 @@ def is_fabric_enabled(self) -> bool: 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). + * Major version: This is the year of the release (e.g. 2022). + * Minor version: This is the half-year of the release (e.g. 1 or 2). + * Patch version: This is the patch number of the release (e.g. 0). + + .. attention:: + This function is deprecated and will be removed in the future. + We recommend using :func:`isaaclab.utils.version.get_isaac_sim_version` + instead of this function. + + Returns: + A tuple containing the major, minor, and patch versions. + + Example: + >>> sim = SimulationContext() + >>> sim.get_version() + (2022, 1, 0) """ - return int(self._isaacsim_version[2]), int(self._isaacsim_version[3]), int(self._isaacsim_version[4]) + return get_isaac_sim_version().major, get_isaac_sim_version().minor, get_isaac_sim_version().micro """ Operations - New utilities. @@ -382,7 +430,7 @@ def set_render_mode(self, mode: RenderMode): """ # check if mode change is possible -- not possible when no GUI is available if not self._has_gui: - omni.log.warn( + self.logger.warning( f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." ) return @@ -424,7 +472,19 @@ def set_setting(self, name: str, value: Any): name: The name of the setting. value: The value of the setting. """ - self._settings.set(name, value) + # Route through typed setters for correctness and consistency for common scalar types. + if isinstance(value, bool): + self.carb_settings.set_bool(name, value) + elif isinstance(value, int): + self.carb_settings.set_int(name, value) + elif isinstance(value, float): + self.carb_settings.set_float(name, value) + elif isinstance(value, str): + self.carb_settings.set_string(name, value) + elif isinstance(value, (list, tuple)): + self.carb_settings.set(name, value) + else: + raise ValueError(f"Unsupported value type for setting '{name}': {type(value)}") def get_setting(self, name: str) -> Any: """Read the simulation setting using the Carbonite SDK. @@ -435,22 +495,31 @@ def get_setting(self, name: str) -> Any: Returns: The value of the setting. """ - return self._settings.get(name) + return self.carb_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) + def get_initial_stage(self) -> Usd.Stage: + """Returns stage handle used during scene creation. + + Returns: + The stage used during scene creation. + """ + return self._initial_stage """ 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() @@ -459,6 +528,15 @@ def reset(self, soft: bool = False): if not soft: for _ in range(2): self.render() + self._disable_app_control_on_stop_handle = False + + 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) def step(self, render: bool = True): """Steps the simulation. @@ -470,6 +548,19 @@ def step(self, render: bool = True): 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 + + # update anim recording if needed + if self._anim_recording_enabled: + is_anim_recording_finished = self._update_anim_recording() + if is_anim_recording_finished: + logger.warning("[INFO][SimulationContext]: Animation recording finished. Closing app.") + self._app.shutdown() + # 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 @@ -487,6 +578,10 @@ def step(self, render: bool = True): # 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. @@ -499,6 +594,11 @@ def render(self, mode: RenderMode | None = None): 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) @@ -526,6 +626,10 @@ def render(self, mode: RenderMode | None = None): 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) """ @@ -543,17 +647,18 @@ async def reset_async(self, soft: bool = False): 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 + with sim_utils.use_stage(self.get_initial_stage()): + # 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) @@ -578,6 +683,131 @@ 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 + self.carb_settings.set_bool("/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 + self.carb_settings.set_bool("/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"): + self.logger.warning( + "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. + self.carb_settings.set_bool("/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 + self.carb_settings.set_bool("/physics/collisionConeCustomGeometry", False) + self.carb_settings.set_bool("/physics/collisionCylinderCustomGeometry", False) + # hide the Simulation Settings window + self.carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) + self.carb_settings.set_bool("/physics/visualizationSimulationOutput", False) + # set fabric enabled flag + self.carb_settings.set_bool("/physics/fabricEnabled", self.cfg.use_fabric) + + def _apply_render_settings_from_cfg(self): # noqa: C901 + """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 = self.carb_settings.get("/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 get_isaac_sim_version().major < 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 + self.set_setting(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] + self.set_setting(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 self.get_setting(key) is None: + raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") + self.set_setting(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 self.carb_settings.get("/rtx/rendermode").lower() == "raytracedlighting": + self.carb_settings.set_string("/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 @@ -594,6 +824,24 @@ 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 + ) + # -- Enable external forces every iteration, helps improve the accuracy of velocity updates. + + if self.cfg.physx.solver_type == 1: + if not self.cfg.physx.enable_external_forces_every_iteration: + logger.warning( + "The `enable_external_forces_every_iteration` parameter in the PhysxCfg is set to False. If you are" + " experiencing noisy velocities, consider enabling this flag. You may need to slightly increase the" + " number of velocity iterations (setting it to 1 or 2 rather than 0), together with this flag, to" + " improve the accuracy of velocity updates." + ) + physx_scene_api.CreateEnableExternalForcesEveryIterationAttr( + self.cfg.physx.enable_external_forces_every_iteration + ) # -- Gravity # note: Isaac sim only takes the "up-axis" as the gravity direction. But physics allows any direction so we @@ -619,7 +867,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 @@ -642,11 +890,124 @@ def _load_fabric_interface(self): # Needed for backward compatibility with older Isaac Sim versions self._update_fabric = self._fabric_iface.update + def _update_anim_recording(self): + """Tracks anim recording timestamps and triggers finish animation recording if the total time has elapsed.""" + if self._anim_recording_started_timestamp is None: + self._anim_recording_started_timestamp = time.time() + + if self._anim_recording_started_timestamp is not None: + anim_recording_total_time = time.time() - self._anim_recording_started_timestamp + if anim_recording_total_time > self._anim_recording_stop_time: + self._finish_anim_recording() + return True + return False + + def _setup_anim_recording(self): + """Sets up anim recording settings and initializes the recording.""" + + self._anim_recording_enabled = bool(self.carb_settings.get("/isaaclab/anim_recording/enabled")) + if not self._anim_recording_enabled: + return + + # Import omni.physx.pvd.bindings here since it is not available by default + from omni.physxpvd.bindings import _physxPvd + + # Init anim recording settings + self._anim_recording_start_time = self.carb_settings.get("/isaaclab/anim_recording/start_time") + self._anim_recording_stop_time = self.carb_settings.get("/isaaclab/anim_recording/stop_time") + self._anim_recording_first_step_timestamp = None + self._anim_recording_started_timestamp = None + + # Make output path relative to repo path + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + self._anim_recording_timestamp = datetime.now().strftime("%Y_%m_%d_%H%M%S") + self._anim_recording_output_dir = ( + os.path.join(repo_path, "anim_recordings", self._anim_recording_timestamp).replace("\\", "/").rstrip("/") + + "/" + ) + os.makedirs(self._anim_recording_output_dir, exist_ok=True) + + # Acquire physx pvd interface and set output directory + self._physxPvdInterface = _physxPvd.acquire_physx_pvd_interface() + + # Set carb settings for the output path and enabling pvd recording + self.carb_settings.set_string( + "/persistent/physics/omniPvdOvdRecordingDirectory", self._anim_recording_output_dir + ) + self.carb_settings.set_bool("/physics/omniPvdOutputEnabled", True) + + def _update_usda_start_time(self, file_path, start_time): + """Updates the start time of the USDA baked anim recordingfile.""" + + # Read the USDA file + with open(file_path) as file: + content = file.read() + + # Extract the timeCodesPerSecond value + time_code_match = re.search(r"timeCodesPerSecond\s*=\s*(\d+)", content) + if not time_code_match: + raise ValueError("timeCodesPerSecond not found in the file.") + time_codes_per_second = int(time_code_match.group(1)) + + # Compute the new start time code + new_start_time_code = int(start_time * time_codes_per_second) + + # Replace the startTimeCode in the file + content = re.sub(r"startTimeCode\s*=\s*\d+", f"startTimeCode = {new_start_time_code}", content) + + # Write the updated content back to the file + with open(file_path, "w") as file: + file.write(content) + + def _finish_anim_recording(self): + """Finishes the animation recording and outputs the baked animation recording.""" + + logger.warning( + "[INFO][SimulationContext]: Finishing animation recording. Stage must be saved. Might take a few minutes." + ) + + # Detaching the stage will also close it and force the serialization of the OVD file + physx = omni.physx.get_physx_simulation_interface() + physx.detach_stage() + + # Save stage to disk + stage_path = os.path.join(self._anim_recording_output_dir, "stage_simulation.usdc") + sim_utils.save_stage(stage_path, save_and_reload_in_place=False) + + # Find the latest ovd file not named tmp.ovd + ovd_files = [ + f for f in glob.glob(os.path.join(self._anim_recording_output_dir, "*.ovd")) if not f.endswith("tmp.ovd") + ] + input_ovd_path = max(ovd_files, key=os.path.getctime) + + # Invoke pvd interface to create recording + stage_filename = "baked_animation_recording.usda" + result = self._physxPvdInterface.ovd_to_usd_over_with_layer_creation( + input_ovd_path, + stage_path, + self._anim_recording_output_dir, + stage_filename, + self._anim_recording_start_time, + self._anim_recording_stop_time, + True, # True: ASCII layers / False : USDC layers + False, # True: verify over layer + ) + + # Workaround for manually setting the truncated start time in the baked animation recording + self._update_usda_start_time( + os.path.join(self._anim_recording_output_dir, stage_filename), self._anim_recording_start_time + ) + + # Disable recording + self.carb_settings.set_bool("/physics/omniPvdOutputEnabled", False) + + return result + """ Callbacks. """ - def _app_control_on_stop_callback(self, event: carb.events.IEvent): + 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 @@ -663,67 +1024,10 @@ def _app_control_on_stop_callback(self, event: carb.events.IEvent): 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. """ - # check if the simulation is stopped - if event.type == int(omni.timeline.TimelineEventType.STOP): - # keep running the simulator when configured to not shutdown the app - if self._has_gui and sys.exc_info()[0] is None: - omni.log.warn( - "Simulation is stopped. The app will keep running with physics disabled." - " Press Ctrl+C or close the window to exit the app." - ) - while self.app.is_running(): - self.render() - - # Note: For the following code: - # The method is an exact copy of the implementation in the `isaacsim.simulation_app.SimulationApp` class. - # We need to remove this method once the SimulationApp class becomes a singleton. - - # make sure that any replicator workflows finish rendering/writing - try: - import omni.replicator.core as rep - - rep_status = rep.orchestrator.get_status() - if rep_status not in [rep.orchestrator.Status.STOPPED, rep.orchestrator.Status.STOPPING]: - rep.orchestrator.stop() - if rep_status != rep.orchestrator.Status.STOPPED: - rep.orchestrator.wait_until_complete() - - # Disable capture on play to avoid replicator engaging on any new timeline events - rep.orchestrator.set_capture_on_play(False) - except Exception: - pass - - # clear the instance and all callbacks - # note: clearing callbacks is important to prevent memory leaks - self.clear_all_callbacks() - - # workaround for exit issues, clean the stage first: - if omni.usd.get_context().can_close_stage(): - omni.usd.get_context().close_stage() - - # print logging information - print("[INFO]: Simulation is stopped. Shutting down the app.") - - # Cleanup any running tracy instances so data is not lost - try: - profiler_tracy = carb.profiler.acquire_profiler_interface(plugin_name="carb.profiler-tracy.plugin") - if profiler_tracy: - profiler_tracy.set_capture_mask(0) - profiler_tracy.end(0) - profiler_tracy.shutdown() - except RuntimeError: - # Tracy plugin was not loaded, so profiler never started - skip checks. - pass - - # Disable logging before shutdown to keep the log clean - # Warnings at this point don't matter as the python process is about to be terminated - logging = carb.logging.acquire_logging() - logging.set_level_threshold(carb.logging.LEVEL_ERROR) - - # App shutdown is disabled to prevent crashes on shutdown. Terminating carb is faster - self._app.shutdown() - self._framework.unload_all_plugins() - sys.exit(0) + if not self._disable_app_control_on_stop_handle: + while not omni.timeline.get_timeline_interface().is_playing(): + self.render() + return @contextmanager @@ -743,20 +1047,20 @@ def build_simulation_context( 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. + 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 + # Design the scene - # Play the simulation - sim.reset() - while sim.is_playing(): - sim.step() + # Play the simulation + sim.reset() + while sim.is_playing(): + sim.step() Args: create_new_stage: Whether to create a new stage. Defaults to True. @@ -775,7 +1079,7 @@ def build_simulation_context( """ try: if create_new_stage: - stage_utils.create_new_stage() + sim_utils.create_new_stage() if sim_cfg is None: # Construct one and overwrite the dt, gravity, and device @@ -812,7 +1116,7 @@ def build_simulation_context( yield sim except Exception: - omni.log.error(traceback.format_exc()) + sim.logger.error(traceback.format_exc()) raise finally: if not sim.has_gui(): @@ -822,3 +1126,8 @@ def build_simulation_context( # 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/source/isaaclab/isaaclab/sim/spawners/__init__.py b/source/isaaclab/isaaclab/sim/spawners/__init__.py index fd85dcbab490..916141906e1e 100644 --- a/source/isaaclab/isaaclab/sim/spawners/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py index d867f0382aec..a95ac491b0a8 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -13,5 +13,11 @@ """ -from .from_files import spawn_from_urdf, spawn_from_usd, spawn_ground_plane -from .from_files_cfg import GroundPlaneCfg, UrdfFileCfg, UsdFileCfg +from .from_files import ( + spawn_from_mjcf, + spawn_from_urdf, + spawn_from_usd, + spawn_from_usd_with_compliant_contact_material, + spawn_ground_plane, +) +from .from_files_cfg import GroundPlaneCfg, MjcfFileCfg, UrdfFileCfg, UsdFileCfg, UsdFileWithCompliantContactCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index 2a6b8f359c1b..242829d777e8 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -1,24 +1,38 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +import logging 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, Usd from isaaclab.sim import converters, schemas -from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone, select_usd_variants +from isaaclab.sim.spawners.materials import RigidBodyMaterialCfg +from isaaclab.sim.utils import ( + add_labels, + bind_physics_material, + bind_visual_material, + change_prim_property, + clone, + create_prim, + get_current_stage, + get_first_matching_child_prim, + select_usd_variants, + set_prim_visibility, +) +from isaaclab.utils.assets import check_usd_path_with_timeout if TYPE_CHECKING: from . import from_files_cfg +# import logger +logger = logging.getLogger(__name__) + @clone def spawn_from_usd( @@ -26,6 +40,7 @@ def spawn_from_usd( cfg: from_files_cfg.UsdFileCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Spawn an asset from a USD file and override the settings with the given config. @@ -49,6 +64,7 @@ def spawn_from_usd( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The prim of the spawned asset. @@ -66,6 +82,7 @@ def spawn_from_urdf( cfg: from_files_cfg.UrdfFileCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Spawn an asset from a URDF file and override the settings with the given config. @@ -89,6 +106,7 @@ def spawn_from_urdf( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The prim of the spawned asset. @@ -102,11 +120,54 @@ def spawn_from_urdf( return _spawn_from_usd_file(prim_path, urdf_loader.usd_path, cfg, translation, orientation) +@clone +def spawn_from_mjcf( + prim_path: str, + cfg: from_files_cfg.MjcfFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Spawn an asset from a MJCF file and override the settings with the given config. + + It uses the :class:`MjcfConverter` class to create a USD file from MJCF. 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 MJCF file does not exist at the given path. + """ + # mjcf loader to convert mjcf to usd + mjcf_loader = converters.MjcfConverter(cfg) + # spawn asset from the generated usd file + return _spawn_from_usd_file(prim_path, mjcf_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, + **kwargs, ) -> Usd.Prim: """Spawns a ground plane into the scene. @@ -125,6 +186,7 @@ def spawn_ground_plane( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The prim of the spawned asset. @@ -132,9 +194,12 @@ def spawn_ground_plane( Raises: ValueError: If the prim path already exists. """ + # Obtain current stage + stage = get_current_stage() + # 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) + if not stage.GetPrimAtPath(prim_path).IsValid(): + create_prim(prim_path, usd_path=cfg.usd_path, translation=translation, orientation=orientation, stage=stage) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") @@ -142,39 +207,57 @@ def spawn_ground_plane( 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" - ) + collision_prim = get_first_matching_child_prim( + prim_path, + predicate=lambda _prim: _prim.GetTypeName() == "Plane", + stage=stage, ) - bind_physics_material(collision_prim_path, f"{prim_path}/physicsMaterial") - + if collision_prim is None: + raise ValueError(f"No collision prim found at path: '{prim_path}'.") + # bind physics material to the collision prim + collision_prim_path = str(collision_prim.GetPath()) + bind_physics_material(collision_prim_path, f"{prim_path}/physicsMaterial", stage=stage) + + # Obtain environment prim + environment_prim = stage.GetPrimAtPath(f"{prim_path}/Environment") # 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"): + if environment_prim.IsValid(): # 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) + environment_prim.GetAttribute("xformOp:scale").Set(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), + change_prim_property( + prop_path=f"{prim_path}/Looks/theGrid/Shader.inputs:diffuse_tint", value=Gf.Vec3f(*cfg.color), - prev=None, + stage=stage, 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"]) + omni.kit.commands.execute("ToggleVisibilitySelectedPrims", selected_paths=[f"{prim_path}/SphereLight"], stage=stage) + + prim = stage.GetPrimAtPath(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(" ", "_") + # add labels to the prim + add_labels(prim, labels=[semantic_value_sanitized], instance_name=semantic_type_sanitized) + + # Apply visibility + set_prim_visibility(prim, cfg.visible) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return prim """ @@ -188,6 +271,7 @@ def _spawn_from_usd_file( cfg: from_files_cfg.FileCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Spawn an asset from a USD file and override the settings with the given config. @@ -204,6 +288,7 @@ def _spawn_from_usd_file( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The prim of the spawned asset. @@ -211,22 +296,31 @@ def _spawn_from_usd_file( 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}'.") + # check if usd path exists with periodic logging until timeout + if not check_usd_path_with_timeout(usd_path): + if "4.5" in usd_path: + usd_5_0_path = usd_path.replace("http", "https").replace("/4.5", "/5.0") + if not check_usd_path_with_timeout(usd_5_0_path): + raise FileNotFoundError(f"USD file not found at path at either: '{usd_path}' or '{usd_5_0_path}'.") + usd_path = usd_5_0_path + else: + raise FileNotFoundError(f"USD file not found at path at: '{usd_path}'.") + + # Obtain current stage + stage = get_current_stage() # spawn asset if it doesn't exist. - if not prim_utils.is_prim_path_valid(prim_path): + if not stage.GetPrimAtPath(prim_path).IsValid(): # add prim as reference to stage - prim_utils.create_prim( + create_prim( prim_path, usd_path=usd_path, translation=translation, orientation=orientation, scale=cfg.scale, + stage=stage, ) else: - omni.log.warn(f"A prim already exists at prim path: '{prim_path}'.") + logger.warning(f"A prim already exists at prim path: '{prim_path}'.") # modify variants if hasattr(cfg, "variants") and cfg.variants is not None: @@ -248,6 +342,8 @@ def _spawn_from_usd_file( # modify tendon properties if cfg.fixed_tendons_props is not None: schemas.modify_fixed_tendon_properties(prim_path, cfg.fixed_tendons_props) + if cfg.spatial_tendons_props is not None: + schemas.modify_spatial_tendon_properties(prim_path, cfg.spatial_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. @@ -267,7 +363,81 @@ def _spawn_from_usd_file( # create material cfg.visual_material.func(material_path, cfg.visual_material) # apply material - bind_visual_material(prim_path, material_path) + bind_visual_material(prim_path, material_path, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) + + +@clone +def spawn_from_usd_with_compliant_contact_material( + prim_path: str, + cfg: from_files_cfg.UsdFileWithCompliantContactCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Spawn an asset from a USD file and apply physics material to specified prims. + + This function extends the :meth:`spawn_from_usd` function by allowing application of compliant contact + physics materials to specified prims within the spawned asset. This is useful for configuring + contact behavior of specific parts within the asset. + + 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 containing the USD file path and physics material settings. + 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. + + Returns: + The prim of the spawned asset with the physics material applied to the specified prims. + + Raises: + FileNotFoundError: If the USD file does not exist at the given path. + """ + + prim = _spawn_from_usd_file(prim_path, cfg.usd_path, cfg, translation, orientation) + stiff = cfg.compliant_contact_stiffness + damp = cfg.compliant_contact_damping + if cfg.physics_material_prim_path is None: + logger.warning("No physics material prim path specified. Skipping physics material application.") + return prim + + if isinstance(cfg.physics_material_prim_path, str): + prim_paths = [cfg.physics_material_prim_path] + else: + prim_paths = cfg.physics_material_prim_path + + if stiff is not None or damp is not None: + material_kwargs = {} + if stiff is not None: + material_kwargs["compliant_contact_stiffness"] = stiff + if damp is not None: + material_kwargs["compliant_contact_damping"] = damp + material_cfg = RigidBodyMaterialCfg(**material_kwargs) + + for path in prim_paths: + if not path.startswith("/"): + rigid_body_prim_path = f"{prim_path}/{path}" + else: + rigid_body_prim_path = path + + material_path = f"{rigid_body_prim_path}/compliant_material" + + # spawn physics material + material_cfg.func(material_path, material_cfg) + + bind_physics_material( + rigid_body_prim_path, + material_path, + ) + logger.info( + f"Applied physics material to prim: {rigid_body_prim_path} with compliance stiffness: {stiff} and" + f" compliance damping: {damp}." + ) + + return prim diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py index 6556140b4f05..ad9f55859040 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -39,9 +39,12 @@ class FileCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): articulation_props: schemas.ArticulationRootPropertiesCfg | None = None """Properties to apply to the articulation root.""" - fixed_tendons_props: schemas.FixedTendonsPropertiesCfg | None = None + fixed_tendons_props: schemas.FixedTendonPropertiesCfg | None = None """Properties to apply to the fixed tendons (if any).""" + spatial_tendons_props: schemas.SpatialTendonPropertiesCfg | None = None + """Properties to apply to the spatial tendons (if any).""" + joint_drive_props: schemas.JointDrivePropertiesCfg | None = None """Properties to apply to a joint. @@ -101,9 +104,9 @@ class UsdFileCfg(FileCfg): 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. + 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. """ @@ -129,11 +132,68 @@ class UrdfFileCfg(FileCfg, converters.UrdfConverterCfg): func: Callable = from_files.spawn_from_urdf +@configclass +class MjcfFileCfg(FileCfg, converters.MjcfConverterCfg): + """MJCF file to spawn asset from. + + It uses the :class:`MjcfConverter` class to create a USD file from MJCF 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_mjcf` 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_mjcf + + """ Spawning ground plane. """ +@configclass +class UsdFileWithCompliantContactCfg(UsdFileCfg): + """Configuration for spawning a USD asset with compliant contact physics material. + + This class extends :class:`UsdFileCfg` to support applying compliant contact properties + (stiffness and damping) to specific prims in the spawned asset. It uses the + :meth:`spawn_from_usd_with_compliant_contact_material` function to perform the spawning and + material application. + """ + + func: Callable = from_files.spawn_from_usd_with_compliant_contact_material + + compliant_contact_stiffness: float | None = None + """Stiffness of the compliant contact. Defaults to None. + + This parameter is the same as + :attr:`~isaaclab.sim.spawners.materials.RigidBodyMaterialCfg.compliant_contact_stiffness`. + """ + + compliant_contact_damping: float | None = None + """Damping of the compliant contact. Defaults to None. + + This parameter is the same as + :attr:`isaaclab.sim.spawners.materials.RigidBodyMaterialCfg.compliant_contact_damping`. + """ + + physics_material_prim_path: str | list[str] | None = None + """Path to the prim or prims to apply the physics material to. Defaults to None, in which case the + physics material is not applied. + + If the path is relative, then it will be relative to the prim's path. + If None, then the physics material will not be applied. + """ + + @configclass class GroundPlaneCfg(SpawnerCfg): """Create a ground plane prim. diff --git a/source/isaaclab/isaaclab/sim/spawners/lights/__init__.py b/source/isaaclab/isaaclab/sim/spawners/lights/__init__.py index 7fb677efcea0..df0c638f58f1 100644 --- a/source/isaaclab/isaaclab/sim/spawners/lights/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/lights/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py index 59a1cf1125fe..9b0106c6ecdd 100644 --- a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py +++ b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,10 +7,9 @@ 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 +from isaaclab.sim.utils import clone, create_prim, get_current_stage, safe_set_attribute_on_usd_prim if TYPE_CHECKING: from . import lights_cfg @@ -22,6 +21,7 @@ def spawn_light( cfg: lights_cfg.LightCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a light prim at the specified prim path with the specified configuration. @@ -39,15 +39,20 @@ def spawn_light( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Raises: ValueError: When a prim already exists at the specified prim path. """ + # obtain stage handle + stage = get_current_stage() # check if prim already exists - if prim_utils.is_prim_path_valid(prim_path): + if stage.GetPrimAtPath(prim_path).IsValid(): 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) + prim = create_prim( + prim_path, prim_type=cfg.prim_type, translation=translation, orientation=orientation, stage=stage + ) # convert to dict cfg = cfg.to_dict() diff --git a/source/isaaclab/isaaclab/sim/spawners/lights/lights_cfg.py b/source/isaaclab/isaaclab/sim/spawners/lights/lights_cfg.py index 946ce0fc8f38..60060ea22e52 100644 --- a/source/isaaclab/isaaclab/sim/spawners/lights/lights_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/lights/lights_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -129,10 +129,12 @@ class DomeLightCfg(LightCfg): Valid values are: - * ``"automatic"``: Tries to determine the layout from the file itself. For example, Renderman texture files embed an explicit parameterization. + * ``"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. + * ``"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. """ diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py index 27f7183f7e9e..0b0e7851494f 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -31,8 +31,6 @@ Usage: .. code-block:: python - import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils # create a visual material @@ -49,7 +47,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.py b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py index 1b1ebaf950e5..29818d830951 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,10 @@ 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 +from isaaclab.sim.utils.stage import get_current_stage if TYPE_CHECKING: from . import physics_materials_cfg @@ -41,12 +40,15 @@ def spawn_rigid_body_material(prim_path: str, cfg: physics_materials_cfg.RigidBo Raises: ValueError: When a prim already exists at the specified prim path and is not a material. """ + # get stage handle + stage = get_current_stage() + # 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) + if not stage.GetPrimAtPath(prim_path).IsValid(): + _ = UsdShade.Material.Define(stage, prim_path) # obtain prim - prim = prim_utils.get_prim_at_path(prim_path) + prim = stage.GetPrimAtPath(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.") @@ -99,12 +101,15 @@ def spawn_deformable_body_material(prim_path: str, cfg: physics_materials_cfg.De .. _PxFEMSoftBodyMaterial: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/structPxFEMSoftBodyMaterialModel.html """ + # get stage handle + stage = get_current_stage() + # 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) + if not stage.GetPrimAtPath(prim_path).IsValid(): + _ = UsdShade.Material.Define(stage, prim_path) # obtain prim - prim = prim_utils.get_prim_at_path(prim_path) + prim = stage.GetPrimAtPath(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.") 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 d44504319859..ce05c2b9ea46 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -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 @@ -48,9 +44,6 @@ class RigidBodyMaterialCfg(PhysicsMaterialCfg): 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"`. @@ -92,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/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 1baa9d325e92..074d6ac0e432 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -1,22 +1,26 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +import logging from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils -import omni.kit.commands -from pxr import Usd +from omni.usd.commands import CreateMdlMaterialPrimCommand, CreateShaderPrimFromSdrCommand +from pxr import Usd, UsdShade from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim +from isaaclab.sim.utils.stage import get_current_stage from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR if TYPE_CHECKING: from . import visual_materials_cfg +# import logger +logger = logging.getLogger(__name__) + @clone def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfaceCfg) -> Usd.Prim: @@ -26,9 +30,9 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa 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. + The function calls the USD command `CreateShaderPrimFromSdrCommand`_ to create the prim. - .. _CreatePreviewSurfaceMaterialPrim: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreatePreviewSurfaceMaterialPrimCommand.html + .. _CreateShaderPrimFromSdrCommand: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreateShaderPrimFromSdrCommand.html .. note:: This function is decorated with :func:`clone` that resolves prim path into list of paths @@ -46,24 +50,55 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa Raises: ValueError: If a prim already exists at the given path. """ + # get stage handle + stage = get_current_stage() + # 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) + if not stage.GetPrimAtPath(prim_path).IsValid(): + # note: we don't use Omniverse's CreatePreviewSurfaceMaterialPrimCommand + # since it does not support USD stage as an argument. The created material + # in that case is always the one from USD Context which makes it difficult to + # handle scene creation on a custom stage. + material_prim = UsdShade.Material.Define(stage, prim_path) + if material_prim: + shader_prim = CreateShaderPrimFromSdrCommand( + parent_path=prim_path, + identifier="UsdPreviewSurface", + stage_or_context=stage, + name="Shader", + ).do() + # bind the shader graph to the material + if shader_prim: + surface_out = shader_prim.GetOutput("surface") + if surface_out: + material_prim.CreateSurfaceOutput().ConnectToSource(surface_out) + + displacement_out = shader_prim.GetOutput("displacement") + if displacement_out: + material_prim.CreateDisplacementOutput().ConnectToSource(displacement_out) + else: + raise ValueError(f"Failed to create preview surface shader at path: '{prim_path}'.") 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") + prim = stage.GetPrimAtPath(f"{prim_path}/Shader") + # check prim is valid + if not prim.IsValid(): + raise ValueError(f"Failed to create preview surface material at path: '{prim_path}'.") # apply properties - cfg = cfg.to_dict() + cfg = cfg.to_dict() # type: ignore 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: +def spawn_from_mdl_file( + prim_path: str, cfg: visual_materials_cfg.MdlFileCfg | visual_materials_cfg.GlassMdlCfg +) -> Usd.Prim: """Load a material from its MDL file and override the settings with the given config. NVIDIA's `Material Definition Language (MDL) `__ @@ -91,23 +126,29 @@ def spawn_from_mdl_file(prim_path: str, cfg: visual_materials_cfg.MdlMaterialCfg Raises: ValueError: If a prim already exists at the given path. """ + # get stage handle + stage = get_current_stage() + # spawn material if it doesn't exist. - if not prim_utils.is_prim_path_valid(prim_path): + if not stage.GetPrimAtPath(prim_path).IsValid(): # extract material name from path material_name = cfg.mdl_path.split("/")[-1].split(".")[0] - omni.kit.commands.execute( - "CreateMdlMaterialPrim", + CreateMdlMaterialPrimCommand( mtl_url=cfg.mdl_path.format(NVIDIA_NUCLEUS_DIR=NVIDIA_NUCLEUS_DIR), mtl_name=material_name, mtl_path=prim_path, + stage=stage, select_new_prim=False, - ) + ).do() 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") + prim = stage.GetPrimAtPath(f"{prim_path}/Shader") + # check prim is valid + if not prim.IsValid(): + raise ValueError(f"Failed to create MDL material at path: '{prim_path}'.") # apply properties - cfg = cfg.to_dict() + cfg = cfg.to_dict() # type: ignore del cfg["func"] del cfg["mdl_path"] for attr_name, attr_value in cfg.items(): diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials_cfg.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials_cfg.py index 578b869d8965..a0c2874b0e9a 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.py b/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.py index e00b0f910f4d..49836dc5cbd4 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py index 567530984584..066ca0bea188 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py @@ -1,20 +1,20 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +from typing import TYPE_CHECKING + 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 isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone, create_prim, get_current_stage from ..materials import DeformableBodyMaterialCfg, RigidBodyMaterialCfg @@ -28,6 +28,7 @@ def spawn_mesh_sphere( cfg: meshes_cfg.MeshSphereCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USD-Mesh sphere prim with the given attributes. @@ -44,6 +45,7 @@ def spawn_mesh_sphere( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -53,10 +55,13 @@ def spawn_mesh_sphere( """ # create a trimesh sphere sphere = trimesh.creation.uv_sphere(radius=cfg.radius) + + # obtain stage handle + stage = get_current_stage() # spawn the sphere as a mesh - _spawn_mesh_geom_from_mesh(prim_path, cfg, sphere, translation, orientation) + _spawn_mesh_geom_from_mesh(prim_path, cfg, sphere, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -65,6 +70,7 @@ def spawn_mesh_cuboid( cfg: meshes_cfg.MeshCuboidCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USD-Mesh cuboid prim with the given attributes. @@ -81,18 +87,23 @@ def spawn_mesh_cuboid( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. Raises: ValueError: If a prim already exists at the given path. - """ # create a trimesh box + """ + # create a trimesh box box = trimesh.creation.box(cfg.size) + + # obtain stage handle + stage = get_current_stage() # spawn the cuboid as a mesh - _spawn_mesh_geom_from_mesh(prim_path, cfg, box, translation, orientation, None) + _spawn_mesh_geom_from_mesh(prim_path, cfg, box, translation, orientation, None, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -101,6 +112,7 @@ def spawn_mesh_cylinder( cfg: meshes_cfg.MeshCylinderCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USD-Mesh cylinder prim with the given attributes. @@ -117,6 +129,7 @@ def spawn_mesh_cylinder( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -134,10 +147,13 @@ def spawn_mesh_cylinder( transform = None # create a trimesh cylinder cylinder = trimesh.creation.cylinder(radius=cfg.radius, height=cfg.height, transform=transform) + + # obtain stage handle + stage = get_current_stage() # spawn the cylinder as a mesh - _spawn_mesh_geom_from_mesh(prim_path, cfg, cylinder, translation, orientation) + _spawn_mesh_geom_from_mesh(prim_path, cfg, cylinder, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -146,6 +162,7 @@ def spawn_mesh_capsule( cfg: meshes_cfg.MeshCapsuleCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USD-Mesh capsule prim with the given attributes. @@ -162,6 +179,7 @@ def spawn_mesh_capsule( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -179,10 +197,13 @@ def spawn_mesh_capsule( transform = None # create a trimesh capsule capsule = trimesh.creation.capsule(radius=cfg.radius, height=cfg.height, transform=transform) + + # obtain stage handle + stage = get_current_stage() # spawn capsule if it doesn't exist. - _spawn_mesh_geom_from_mesh(prim_path, cfg, capsule, translation, orientation) + _spawn_mesh_geom_from_mesh(prim_path, cfg, capsule, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -191,6 +212,7 @@ def spawn_mesh_cone( cfg: meshes_cfg.MeshConeCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USD-Mesh cone prim with the given attributes. @@ -207,6 +229,7 @@ def spawn_mesh_cone( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -224,10 +247,13 @@ def spawn_mesh_cone( transform = None # create a trimesh cone cone = trimesh.creation.cone(radius=cfg.radius, height=cfg.height, transform=transform) + + # obtain stage handle + stage = get_current_stage() # spawn cone if it doesn't exist. - _spawn_mesh_geom_from_mesh(prim_path, cfg, cone, translation, orientation) + _spawn_mesh_geom_from_mesh(prim_path, cfg, cone, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) """ @@ -242,6 +268,8 @@ def _spawn_mesh_geom_from_mesh( translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, scale: tuple[float, float, float] | None = None, + stage: Usd.Stage | None = None, + **kwargs, ): """Create a `USDGeomMesh`_ prim from the given mesh. @@ -263,6 +291,8 @@ def _spawn_mesh_geom_from_mesh( 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. + stage: The stage to spawn the asset at. Defaults to None, in which case the current stage is used. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Raises: ValueError: If a prim already exists at the given path. @@ -273,12 +303,14 @@ def _spawn_mesh_geom_from_mesh( .. _USDGeomMesh: https://openusd.org/dev/api/class_usd_geom_mesh.html """ + # obtain stage handle + stage = stage if stage is not None else get_current_stage() + # 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) + if not stage.GetPrimAtPath(prim_path).IsValid(): + create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation, stage=stage) 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.") @@ -297,7 +329,7 @@ def _spawn_mesh_geom_from_mesh( mesh_prim_path = geom_prim_path + "/mesh" # create the mesh prim - mesh_prim = prim_utils.create_prim( + mesh_prim = create_prim( mesh_prim_path, prim_type="Mesh", scale=scale, @@ -307,6 +339,7 @@ def _spawn_mesh_geom_from_mesh( "faceVertexCounts": np.asarray([3] * len(mesh.faces)), "subdivisionScheme": "bilinear", }, + stage=stage, ) # note: in case of deformable objects, we need to apply the deformable properties to the mesh prim. @@ -314,9 +347,9 @@ def _spawn_mesh_geom_from_mesh( 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) + schemas.define_mass_properties(mesh_prim_path, cfg.mass_props, stage=stage) # apply deformable body properties - schemas.define_deformable_body_properties(mesh_prim_path, cfg.deformable_props) + schemas.define_deformable_body_properties(mesh_prim_path, cfg.deformable_props, stage=stage) elif cfg.collision_props is not None: # decide on type of collision approximation based on the mesh if cfg.__class__.__name__ == "MeshSphereCfg": @@ -331,7 +364,7 @@ def _spawn_mesh_geom_from_mesh( 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) + schemas.define_collision_properties(mesh_prim_path, cfg.collision_props, stage=stage) # apply visual material if cfg.visual_material is not None: @@ -342,7 +375,7 @@ def _spawn_mesh_geom_from_mesh( # create material cfg.visual_material.func(material_path, cfg.visual_material) # apply material - bind_visual_material(mesh_prim_path, material_path) + bind_visual_material(mesh_prim_path, material_path, stage=stage) # apply physics material if cfg.physics_material is not None: @@ -353,12 +386,12 @@ def _spawn_mesh_geom_from_mesh( # create material cfg.physics_material.func(material_path, cfg.physics_material) # apply material - bind_physics_material(mesh_prim_path, material_path) + bind_physics_material(mesh_prim_path, material_path, stage=stage) # 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) + schemas.define_mass_properties(prim_path, cfg.mass_props, stage=stage) # apply rigid properties - schemas.define_rigid_body_properties(prim_path, cfg.rigid_props) + schemas.define_rigid_body_properties(prim_path, cfg.rigid_props, stage=stage) diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py index 4ec7e173c30a..d5c39a505b8b 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py index e2dd445788e7..ac61868c0255 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index b56134bfdc0d..6270447169e9 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -1,23 +1,23 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +import logging 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.sim.utils import change_prim_property, clone, create_prim, get_current_stage from isaaclab.utils import to_camel_case if TYPE_CHECKING: from . import sensors_cfg +# import logger +logger = logging.getLogger(__name__) CUSTOM_PINHOLE_CAMERA_ATTRIBUTES = { "projection_type": ("cameraProjectionType", Sdf.ValueTypeNames.Token), @@ -54,6 +54,7 @@ def spawn_camera( cfg: sensors_cfg.PinholeCameraCfg | sensors_cfg.FisheyeCameraCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USD camera prim with given projection type. @@ -73,6 +74,7 @@ def spawn_camera( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -80,19 +82,21 @@ def spawn_camera( Raises: ValueError: If a prim already exists at the given path. """ + # obtain stage handle + stage = get_current_stage() + # 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) + if not stage.GetPrimAtPath(prim_path).IsValid(): + create_prim(prim_path, "Camera", translation=translation, orientation=orientation, stage=stage) 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"), + change_prim_property( + prop_path=f"{prim_path}.omni:kit:cameraLock", value=True, - prev=None, + stage=stage, type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool, ) # decide the custom attributes to add @@ -104,7 +108,7 @@ def spawn_camera( # 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.") + logger.warning("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 = [ @@ -116,7 +120,7 @@ def spawn_camera( "from_intrinsic_matrix", ] # get camera prim - prim = prim_utils.get_prim_at_path(prim_path) + prim = stage.GetPrimAtPath(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(): @@ -139,4 +143,4 @@ def spawn_camera( # 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) + return prim diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index cff5f9be17f5..44e5eb061733 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,6 +8,7 @@ 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 @@ -23,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 @@ -102,7 +99,7 @@ def from_intrinsic_matrix( width: int, height: int, clipping_range: tuple[float, float] = (0.01, 1e6), - focal_length: float = 24.0, + focal_length: float | None = None, focus_distance: float = 400.0, f_stop: float = 0.0, projection_type: str = "pinhole", @@ -120,8 +117,8 @@ def from_intrinsic_matrix( 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. + 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. @@ -129,7 +126,8 @@ def from_intrinsic_matrix( 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). Defaults to 24.0 cm. + 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". @@ -142,27 +140,20 @@ def from_intrinsic_matrix( if projection_type != "pinhole": raise NotImplementedError("Only pinhole projection type is supported.") - # 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 + 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=focal_length, + focal_length=usd_camera_params["focal_length"], focus_distance=focus_distance, f_stop=f_stop, - horizontal_aperture=horizontal_aperture, - vertical_aperture=vertical_aperture, - horizontal_aperture_offset=horizontal_aperture_offset, - vertical_aperture_offset=vertical_aperture_offset, + 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, ) @@ -175,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/__init__.py b/source/isaaclab/isaaclab/sim/spawners/shapes/__init__.py index cc59d1a8ccb6..8f6cab9439cd 100644 --- a/source/isaaclab/isaaclab/sim/spawners/shapes/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/shapes/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py index e10dc94d5288..a7780c25596d 100644 --- a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py +++ b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,10 @@ 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 +from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone, create_prim, get_current_stage if TYPE_CHECKING: from . import shapes_cfg @@ -23,6 +22,7 @@ def spawn_sphere( cfg: shapes_cfg.SphereCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USDGeom-based sphere prim with the given attributes. @@ -41,6 +41,7 @@ def spawn_sphere( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -48,11 +49,13 @@ def spawn_sphere( Raises: ValueError: If a prim already exists at the given path. """ + # obtain stage handle + stage = get_current_stage() # spawn sphere if it doesn't exist. attributes = {"radius": cfg.radius} - _spawn_geom_from_prim_type(prim_path, cfg, "Sphere", attributes, translation, orientation) + _spawn_geom_from_prim_type(prim_path, cfg, "Sphere", attributes, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -61,6 +64,7 @@ def spawn_cuboid( cfg: shapes_cfg.CuboidCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USDGeom-based cuboid prim with the given attributes. @@ -83,6 +87,7 @@ def spawn_cuboid( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -90,14 +95,16 @@ def spawn_cuboid( Raises: If a prim already exists at the given path. """ + # obtain stage handle + stage = get_current_stage() # 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) + _spawn_geom_from_prim_type(prim_path, cfg, "Cube", attributes, translation, orientation, scale, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -106,6 +113,7 @@ def spawn_cylinder( cfg: shapes_cfg.CylinderCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USDGeom-based cylinder prim with the given attributes. @@ -124,6 +132,7 @@ def spawn_cylinder( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -131,11 +140,13 @@ def spawn_cylinder( Raises: ValueError: If a prim already exists at the given path. """ + # obtain stage handle + stage = get_current_stage() # 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) + _spawn_geom_from_prim_type(prim_path, cfg, "Cylinder", attributes, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -144,6 +155,7 @@ def spawn_capsule( cfg: shapes_cfg.CapsuleCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USDGeom-based capsule prim with the given attributes. @@ -162,6 +174,7 @@ def spawn_capsule( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -169,11 +182,13 @@ def spawn_capsule( Raises: ValueError: If a prim already exists at the given path. """ + # obtain stage handle + stage = get_current_stage() # 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) + _spawn_geom_from_prim_type(prim_path, cfg, "Capsule", attributes, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) @clone @@ -182,6 +197,7 @@ def spawn_cone( cfg: shapes_cfg.ConeCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + **kwargs, ) -> Usd.Prim: """Create a USDGeom-based cone prim with the given attributes. @@ -200,6 +216,7 @@ def spawn_cone( 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. + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. Returns: The created prim. @@ -207,11 +224,13 @@ def spawn_cone( Raises: ValueError: If a prim already exists at the given path. """ + # obtain stage handle + stage = get_current_stage() # 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) + _spawn_geom_from_prim_type(prim_path, cfg, "Cone", attributes, translation, orientation, stage=stage) # return the prim - return prim_utils.get_prim_at_path(prim_path) + return stage.GetPrimAtPath(prim_path) """ @@ -227,6 +246,7 @@ def _spawn_geom_from_prim_type( translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, scale: tuple[float, float, float] | None = None, + stage: Usd.Stage | None = None, ): """Create a USDGeom-based prim with the given attributes. @@ -234,7 +254,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: @@ -252,13 +272,17 @@ def _spawn_geom_from_prim_type( 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. + stage: The stage to spawn the asset at. Defaults to None, in which case the current stage is used. Raises: ValueError: If a prim already exists at the given path. """ + # obtain stage handle + stage = stage if stage is not None else get_current_stage() + # 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) + if not stage.GetPrimAtPath(prim_path).IsValid(): + create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation, stage=stage) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") @@ -267,10 +291,10 @@ def _spawn_geom_from_prim_type( mesh_prim_path = geom_prim_path + "/mesh" # create the geometry prim - prim_utils.create_prim(mesh_prim_path, prim_type, scale=scale, attributes=attributes) + create_prim(mesh_prim_path, prim_type, scale=scale, attributes=attributes, stage=stage) # apply collision properties if cfg.collision_props is not None: - schemas.define_collision_properties(mesh_prim_path, cfg.collision_props) + schemas.define_collision_properties(mesh_prim_path, cfg.collision_props, stage=stage) # apply visual material if cfg.visual_material is not None: if not cfg.visual_material_path.startswith("/"): @@ -280,7 +304,7 @@ def _spawn_geom_from_prim_type( # create material cfg.visual_material.func(material_path, cfg.visual_material) # apply material - bind_visual_material(mesh_prim_path, material_path) + bind_visual_material(mesh_prim_path, material_path, stage=stage) # apply physics material if cfg.physics_material is not None: if not cfg.physics_material_path.startswith("/"): @@ -290,12 +314,12 @@ def _spawn_geom_from_prim_type( # create material cfg.physics_material.func(material_path, cfg.physics_material) # apply material - bind_physics_material(mesh_prim_path, material_path) + bind_physics_material(mesh_prim_path, material_path, stage=stage) # 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) + schemas.define_mass_properties(prim_path, cfg.mass_props, stage=stage) # apply rigid body properties if cfg.rigid_props is not None: - schemas.define_rigid_body_properties(prim_path, cfg.rigid_props) + schemas.define_rigid_body_properties(prim_path, cfg.rigid_props, stage=stage) diff --git a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes_cfg.py b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes_cfg.py index 9d3b0cd779a8..d2de5a7f9416 100644 --- a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py b/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py index 633aa4cd4e18..2dea8db8fcb6 100644 --- a/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/__init__.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/__init__.py index 32afadf6cf38..4006fa1a6abc 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py index 0e99fe37aadc..64d0c4f4ab91 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,8 +10,6 @@ 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 @@ -26,6 +24,8 @@ def spawn_multi_asset( cfg: wrappers_cfg.MultiAssetSpawnerCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + clone_in_fabric: bool = False, + replicate_physics: bool = False, ) -> Usd.Prim: """Spawn multiple assets based on the provided configurations. @@ -38,10 +38,15 @@ def spawn_multi_asset( 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. + clone_in_fabric: Whether to clone in fabric. Default is False. + replicate_physics: Whether to replicate physics. Default is False. Returns: The created prim at the first prim path. """ + # get stage handle + stage = sim_utils.get_current_stage() + # resolve: {SPAWN_NS}/AssetName # note: this assumes that the spawn namespace already exists in the stage root_path, asset_path = prim_path.rsplit("/", 1) @@ -61,8 +66,8 @@ def spawn_multi_asset( 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") + template_prim_path = sim_utils.get_next_free_prim_path("/World/Template", stage=stage) + sim_utils.create_prim(template_prim_path, "Scope", stage=stage) # spawn everything first in a "Dataset" prim proto_prim_paths = list() @@ -81,16 +86,20 @@ def spawn_multi_asset( 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) + asset_cfg.func( + proto_prim_path, + asset_cfg, + translation=translation, + orientation=orientation, + clone_in_fabric=clone_in_fabric, + replicate_physics=replicate_physics, + ) # 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. @@ -107,7 +116,7 @@ def spawn_multi_asset( 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) + sim_utils.delete_prim(template_prim_path, stage=stage) # 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. @@ -116,7 +125,7 @@ def spawn_multi_asset( carb_settings_iface.set_bool("/isaaclab/spawn/multi_assets", True) # return the prim - return prim_utils.get_prim_at_path(prim_paths[0]) + return stage.GetPrimAtPath(prim_paths[0]) def spawn_multi_usd_file( @@ -124,6 +133,8 @@ def spawn_multi_usd_file( cfg: wrappers_cfg.MultiUsdFileCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, + clone_in_fabric: bool = False, + replicate_physics: bool = False, ) -> Usd.Prim: """Spawn multiple USD files based on the provided configurations. @@ -135,6 +146,8 @@ def spawn_multi_usd_file( 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. + clone_in_fabric: Whether to clone in fabric. Default is False. + replicate_physics: Whether to replicate physics. Default is False. Returns: The created prim at the first prim path. @@ -174,4 +187,4 @@ def spawn_multi_usd_file( 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) + return spawn_multi_asset(prim_path, multi_asset_cfg, translation, orientation, clone_in_fabric, replicate_physics) diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py index f1b725d4f6f7..07c585f7c4c3 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/sim/utils/__init__.py b/source/isaaclab/isaaclab/sim/utils/__init__.py new file mode 100644 index 000000000000..3a85ae44c2f1 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities built around USD operations.""" + +from .legacy import * # noqa: F401, F403 +from .prims import * # noqa: F401, F403 +from .queries import * # noqa: F401, F403 +from .semantics import * # noqa: F401, F403 +from .stage import * # noqa: F401, F403 +from .transforms import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/sim/utils/legacy.py b/source/isaaclab/isaaclab/sim/utils/legacy.py new file mode 100644 index 000000000000..0e3aef861733 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/legacy.py @@ -0,0 +1,342 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for legacy functionality. + +This sub-module contains legacy functions from Isaac Sim that are no longer +required for Isaac Lab. Most functions are simple wrappers around USD APIs +and are provided mainly for convenience. + +It is recommended to use the USD APIs directly whenever possible. +""" + +from __future__ import annotations + +import logging +from collections.abc import Iterable + +from pxr import Usd, UsdGeom + +from .prims import add_usd_reference +from .queries import get_next_free_prim_path +from .stage import get_current_stage + +# import logger +logger = logging.getLogger(__name__) + + +""" +Stage utilities. +""" + + +def add_reference_to_stage(usd_path: str, path: str, prim_type: str = "Xform") -> Usd.Prim: + """Adds a USD reference to the stage at the specified prim path. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the :func:`isaaclab.sim.utils.prims.add_usd_reference` function instead. + + Args: + usd_path: The path to the USD file to reference. + path: The prim path to add the reference to. + prim_type: The type of prim to create if it doesn't exist. Defaults to "Xform". + + Returns: + The USD prim at the specified prim path. + """ + logger.warning("Function 'add_reference_to_stage' is deprecated. Please use 'add_usd_reference' instead.") + return add_usd_reference(prim_path=path, usd_path=usd_path, prim_type=prim_type) + + +def get_stage_up_axis() -> str: + """Gets the up axis of the stage. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the USD APIs directly instead. + + >>> import isaaclab.sim as sim_utils + >>> from pxr import UsdGeom + >>> + >>> UsdGeom.GetStageUpAxis(sim_utils.get_current_stage()) + 'Z' + """ + msg = """Function 'get_stage_up_axis' is deprecated. Please use the USD APIs directly instead. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import UsdGeom + >>> + >>> UsdGeom.GetStageUpAxis(sim_utils.get_current_stage()) + 'Z' + """ + logger.warning(msg) + return UsdGeom.GetStageUpAxis(get_current_stage()) + + +def traverse_stage(fabric: bool = False) -> Iterable[Usd.Prim]: + """Traverses the stage and returns all the prims. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the USD APIs directly instead. + + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> for prim in stage.Traverse(): + >>> print(prim) + Usd.Prim() + Usd.Prim() + Usd.Prim() + Usd.Prim() + + Args: + fabric: True for fabric stage and False for USD stage. Defaults to False. + + Returns: + An iterable of all the prims in the stage. + """ + msg = """Function 'traverse_stage' is deprecated. Please use the USD APIs directly instead. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> for prim in stage.Traverse(): + >>> print(prim) + """ + logger.warning(msg) + # get current stage + stage = get_current_stage(fabric=fabric) + # traverse stage + return stage.Traverse() + + +""" +Prims utilities. +""" + + +def get_prim_at_path(prim_path: str, fabric: bool = False) -> Usd.Prim | None: + """Gets the USD prim at the specified path. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the USD APIs directly instead. + + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> stage.GetPrimAtPath("/World/Cube") + Usd.Prim() + + Args: + prim_path: The path of the prim to get. + fabric: Whether to get the prim from the fabric stage. Defaults to False. + + Returns: + The USD prim at the specified path. If stage is not found, returns None. + """ + msg = """Function 'get_prim_at_path' is deprecated. Please use the USD APIs directly instead. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> stage.GetPrimAtPath("/World/Cube") + Usd.Prim() + """ + logger.warning(msg) + # get current stage + stage = get_current_stage(fabric=fabric) + if stage is not None: + return stage.GetPrimAtPath(prim_path) + return None + + +def get_prim_path(prim: Usd.Prim) -> str: + """Gets the path of the specified USD prim. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the USD APIs directly instead. + + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/Cube") + >>> prim.GetPath().pathString + "/World/Cube" + + Args: + prim: The USD prim to get the path of. + + Returns: + The path of the specified USD prim. + """ + msg = """Function 'get_prim_path' is deprecated. Please use the USD APIs directly instead. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/Cube") + >>> prim.GetPath().pathString + "/World/Cube" + """ + logger.warning(msg) + return prim.GetPath().pathString if prim.IsValid() else "" + + +def is_prim_path_valid(prim_path: str, fabric: bool = False) -> bool: + """Check if a path has a valid USD Prim on the specified stage. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the USD APIs directly instead. + + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/Cube") + >>> prim.IsValid() + True + + Args: + prim_path: path of the prim in the stage + fabric: True for fabric stage and False for USD stage. Defaults to False. + + Returns: + True if the path points to a valid prim. False otherwise. + """ + msg = """Function 'is_prim_path_valid' is deprecated. Please use the USD APIs directly instead. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/Cube") + >>> prim.IsValid() + True + """ + logger.warning(msg) + # get prim at path + prim = get_prim_at_path(prim_path, fabric=fabric) + # return validity + return prim.IsValid() if prim else False + + +def define_prim(prim_path: str, prim_type: str = "Xform", fabric: bool = False) -> Usd.Prim: + """Create a USD Prim at the given prim_path of type prim type unless one already exists. + + This function creates a prim of the specified type in the specified path. To apply a + transformation (position, orientation, scale), set attributes or load an USD file while + creating the prim use the :func:`isaaclab.sim.utils.prims.create_prim` function. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the USD APIs directly instead. + In case, a new prim is needed, use the :func:`isaaclab.sim.utils.prims.create_prim` + function instead. + + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> stage.DefinePrim("/World/Shapes", "Xform") + Usd.Prim() + + Args: + prim_path: path of the prim in the stage + prim_type: The type of the prim to create. Defaults to "Xform". + fabric: True for fabric stage and False for USD stage. Defaults to False. + + Returns: + The created USD prim. + + Raises: + ValueError: If there is already a prim at the prim_path + """ + msg = """Function 'define_prim' is deprecated. Please use the USD APIs directly instead. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> stage.DefinePrim("/World/Shapes", "Xform") + Usd.Prim() + """ + logger.warning(msg) + # get current stage + stage = get_current_stage(fabric=fabric) + # check if prim path is valid + if stage.GetPrimAtPath(prim_path).IsValid(): + raise ValueError(f"A prim already exists at prim path: {prim_path}") + # define prim + return stage.DefinePrim(prim_path, prim_type) + + +def get_prim_type_name(prim_path: str | Usd.Prim, fabric: bool = False) -> str: + """Get the type name of the USD Prim at the provided path. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the USD APIs directly instead. + + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/Cube") + >>> prim.GetTypeName() + "Cube" + + Args: + prim_path: path of the prim in the stage or the prim itself + fabric: True for fabric stage and False for USD stage. Defaults to False. + + Returns: + The type name of the USD Prim at the provided path. + + Raises: + ValueError: If there is not a valid prim at the provided path + """ + msg = """Function 'get_prim_type_name' is deprecated. Please use the USD APIs directly instead. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/Cube") + >>> prim.GetTypeName() + "Cube" + """ + logger.warning(msg) + # check if string + if isinstance(prim_path, str): + stage = get_current_stage(fabric=fabric) + prim = stage.GetPrimAtPath(prim_path) + else: + prim = prim_path + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"A prim does not exist at prim path: {prim_path}") + # return type name + return prim.GetTypeName() + + +""" +Queries utilities. +""" + + +def get_next_free_path(path: str) -> str: + """Gets a new prim path that doesn't exist in the stage given a base path. + + .. deprecated:: 2.3.0 + This function is deprecated. Please use the + :func:`isaaclab.sim.utils.queries.get_next_free_prim_path` function instead. + + Args: + path: The base prim path to check. + stage: The stage to check. Defaults to the current stage. + + Returns: + A new path that is guaranteed to not exist on the current stage + """ + logger.warning("Function 'get_next_free_path' is deprecated. Please use 'get_next_free_prim_path' instead.") + return get_next_free_prim_path(path) diff --git a/source/isaaclab/isaaclab/sim/utils.py b/source/isaaclab/isaaclab/sim/utils/prims.py similarity index 51% rename from source/isaaclab/isaaclab/sim/utils.py rename to source/isaaclab/isaaclab/sim/utils/prims.py index 36ab6dd2a819..6fb7850a5b5d 100644 --- a/source/isaaclab/isaaclab/sim/utils.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -1,42 +1,342 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""Sub-module with USD-related utilities.""" +"""Utilities for creating and manipulating USD prims.""" from __future__ import annotations import functools import inspect +import logging import re -from collections.abc import Callable +from collections.abc import Callable, Sequence from typing import TYPE_CHECKING, Any -import isaacsim.core.utils.stage as stage_utils +import torch + import omni.kit.commands -import omni.log +import omni.usd 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 pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade, UsdUtils from isaaclab.utils.string import to_camel_case +from isaaclab.utils.version import get_isaac_sim_version -from . import schemas +from .queries import find_matching_prim_paths +from .semantics import add_labels +from .stage import get_current_stage, get_current_stage_id +from .transforms import convert_world_pose_to_local, standardize_xform_ops if TYPE_CHECKING: - from .spawners.spawner_cfg import SpawnerCfg + from isaaclab.sim.spawners.spawner_cfg import SpawnerCfg + +# import logger +logger = logging.getLogger(__name__) + + +""" +General Utils +""" + + +def create_prim( + prim_path: str, + prim_type: str = "Xform", + position: Any | None = None, + translation: Any | None = None, + orientation: Any | None = None, + scale: Any | None = None, + usd_path: str | None = None, + semantic_label: str | None = None, + semantic_type: str = "class", + attributes: dict | None = None, + stage: Usd.Stage | None = None, +) -> Usd.Prim: + """Creates a prim in the provided USD stage. + + The method applies the specified transforms, the semantic label and sets the specified attributes. + The transform can be specified either in world space (using ``position``) or local space (using + ``translation``). + + The function determines the coordinate system of the transform based on the provided arguments. + + * If ``position`` is provided, it is assumed the orientation is provided in the world frame as well. + * If ``translation`` is provided, it is assumed the orientation is provided in the local frame as well. + + The scale is always applied in the local frame. + + The function handles various sequence types (list, tuple, numpy array, torch tensor) + and converts them to properly-typed tuples for operations on the prim. + + .. note:: + Transform operations are standardized to the USD convention: translate, orient (quaternion), + and scale, in that order. See :func:`standardize_xform_ops` for more details. + + Args: + prim_path: + The path of the new prim. + prim_type: + Prim type name. Defaults to "Xform", in which case a simple Xform prim is created. + position: + Prim position in world space as (x, y, z). If the prim has a parent, this is + automatically converted to local space relative to the parent. Cannot be used with + ``translation``. Defaults to None, in which case no position is applied. + translation: + Prim translation in local space as (x, y, z). This is applied directly without + any coordinate transformation. Cannot be used with ``position``. Defaults to None, + in which case no translation is applied. + orientation: + Prim rotation as a quaternion (w, x, y, z). When used with ``position``, the + orientation is also converted from world space to local space. When used with ``translation``, + it is applied directly as local orientation. Defaults to None. + scale: + Scaling factor in x, y, z. Applied in local space. Defaults to None, + in which case a uniform scale of 1.0 is applied. + usd_path: + Path to the USD file that this prim will reference. Defaults to None. + semantic_label: + Semantic label to apply to the prim. Defaults to None, in which case no label is added. + semantic_type: + Semantic type for the label. Defaults to "class". + attributes: + Key-value pairs of prim attributes to set. Defaults to None, in which case no attributes are set. + stage: + The stage to create the prim in. Defaults to None, in which case the current stage is used. + + Returns: + The created USD prim. + + Raises: + ValueError: If there is already a prim at the provided prim path. + ValueError: If both position and translation are provided. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> # Create a cube at world position (1.0, 0.5, 0.0) + >>> sim_utils.create_prim( + ... prim_path="/World/Parent/Cube", + ... prim_type="Cube", + ... position=(1.0, 0.5, 0.0), + ... attributes={"size": 2.0}, + ... ) + Usd.Prim() + >>> + >>> # Create a sphere with local translation relative to its parent + >>> sim_utils.create_prim( + ... prim_path="/World/Parent/Sphere", + ... prim_type="Sphere", + ... translation=(0.5, 0.0, 0.0), + ... scale=(2.0, 2.0, 2.0), + ... ) + Usd.Prim() + """ + # Ensure that user doesn't provide both position and translation + if position is not None and translation is not None: + raise ValueError("Cannot provide both position and translation. Please provide only one.") + + # obtain stage handle + stage = get_current_stage() if stage is None else stage + + # check if prim already exists + if stage.GetPrimAtPath(prim_path).IsValid(): + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + + # create prim in stage + prim = stage.DefinePrim(prim_path, prim_type) + if not prim.IsValid(): + raise ValueError(f"Failed to create prim at path: '{prim_path}' of type: '{prim_type}'.") + # apply attributes into prim + if attributes is not None: + for k, v in attributes.items(): + prim.GetAttribute(k).Set(v) + # add reference to USD file + if usd_path is not None: + add_usd_reference(prim_path=prim_path, usd_path=usd_path, stage=stage) + # add semantic label to prim + if semantic_label is not None: + add_labels(prim, labels=[semantic_label], instance_name=semantic_type) + + # check if prim type is Xformable + if not prim.IsA(UsdGeom.Xformable): + logger.debug( + f"Prim at path '{prim.GetPath().pathString}' is of type '{prim.GetTypeName()}', " + "which is not an Xformable. Transform operations will not be standardized. " + "This is expected for material, shader, and scope prims." + ) + return prim + + # convert input arguments to tuples + position = _to_tuple(position) if position is not None else None + translation = _to_tuple(translation) if translation is not None else None + orientation = _to_tuple(orientation) if orientation is not None else None + scale = _to_tuple(scale) if scale is not None else None + + # convert position and orientation to translation and orientation + # world --> local + if position is not None: + # this means that user provided pose in the world frame + translation, orientation = convert_world_pose_to_local(position, orientation, ref_prim=prim.GetParent()) + + # standardize the xform ops + standardize_xform_ops(prim, translation, orientation, scale) + + return prim + + +def delete_prim(prim_path: str | Sequence[str], stage: Usd.Stage | None = None) -> bool: + """Removes the USD Prim and its descendants from the scene if able. + + Args: + prim_path: The path of the prim to delete. If a list of paths is provided, + the function will delete all the prims in the list. + stage: The stage to delete the prim in. Defaults to None, in which case the current stage is used. + + Returns: + True if the prim or prims were deleted successfully, False otherwise. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.delete_prim("/World/Cube") + """ + # convert prim_path to list if it is a string + if isinstance(prim_path, str): + prim_path = [prim_path] + # get stage handle + stage = get_current_stage() if stage is None else stage + # FIXME: We should not need to cache the stage here. It should + # happen at the creation of the stage. + # the prim command looks for the stage ID in the stage cache + # so we need to ensure the stage is cached + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(stage).ToLongInt() + if stage_id < 0: + stage_id = stage_cache.Insert(stage).ToLongInt() + # delete prims + success, _ = omni.kit.commands.execute( + "DeletePrimsCommand", + paths=prim_path, + stage=stage, + ) + return success + + +def move_prim(path_from: str, path_to: str, keep_world_transform: bool = True, stage: Usd.Stage | None = None) -> bool: + """Moves a prim from one path to another within a USD stage. + + This function moves the prim from the source path to the destination path. If the :attr:`keep_world_transform` + is set to True, the world transform of the prim is kept. This implies that the prim's local transform is reset + such that the prim's world transform is the same as the source path's world transform. If it is set to False, + the prim's local transform is preserved. + + .. warning:: + Reparenting or moving prims in USD is an expensive operation that may trigger + significant recomposition costs, especially in large or deeply layered stages. + + Args: + path_from: Path of the USD Prim you wish to move + path_to: Final destination of the prim + keep_world_transform: Whether to keep the world transform of the prim. Defaults to True. + stage: The stage to move the prim in. Defaults to None, in which case the current stage is used. + + Returns: + True if the prim was moved successfully, False otherwise. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> # given the stage: /World/Cube. Move the prim Cube outside the prim World + >>> sim_utils.move_prim("/World/Cube", "/Cube") + """ + # get stage handle + stage = get_current_stage() if stage is None else stage + # move prim + success, _ = omni.kit.commands.execute( + "MovePrimCommand", + path_from=path_from, + path_to=path_to, + keep_world_transform=keep_world_transform, + stage_or_context=stage, + ) + return success + """ -Attribute - Setters. +USD Prim properties and attributes. """ +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 '/'). + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # 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 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 is instanced + if child_prim.IsInstance(): + # make the prim uninstanceable + child_prim.SetInstanceable(False) + # add children to list + all_prims += child_prim.GetFilteredChildren(Usd.TraverseInstanceProxies()) + + +def set_prim_visibility(prim: Usd.Prim, visible: bool) -> None: + """Sets the visibility of the prim in the opened stage. + + .. note:: + + The method does this through the USD API. + + Args: + prim: the USD prim + visible: flag to set the visibility of the usd prim in stage. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> # given the stage: /World/Cube. Make the Cube not visible + >>> prim = sim_utils.get_prim_at_path("/World/Cube") + >>> sim_utils.set_prim_visibility(prim, False) + """ + imageable = UsdGeom.Imageable(prim) + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + 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. @@ -70,7 +370,7 @@ def safe_set_attribute_on_usd_schema(schema_api: Usd.APISchemaBase, name: str, v 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()}'.") + logger.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()}'.") @@ -108,19 +408,179 @@ def safe_set_attribute_on_usd_prim(prim: Usd.Prim, attr_name: str, value: Any, c 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}"), + + # change property using the change_prim_property function + change_prim_property( + prop_path=f"{prim.GetPath()}.{attr_name}", value=value, - prev=None, + stage=prim.GetStage(), type_to_create_if_not_exist=sdf_type, - usd_context_name=prim.GetStage(), ) +def change_prim_property( + prop_path: str | Sdf.Path, + value: Any, + stage: Usd.Stage | None = None, + type_to_create_if_not_exist: Sdf.ValueTypeNames | None = None, + is_custom: bool = False, +) -> bool: + """Change or create a property value on a USD prim. + + This is a simplified property setter that works with the current edit target. If you need + complex layer management, use :class:`omni.kit.commands.ChangePropertyCommand` instead. + + By default, this function changes the value of the property when it exists. If the property + doesn't exist, :attr:`type_to_create_if_not_exist` must be provided to create it. + + Note: + The attribute :attr:`value` must be the correct type for the property. + For example, if the property is a float, the value must be a float. + If it is supposed to be a RGB color, the value must be of type :class:`Gf.Vec3f`. + + Args: + prop_path: Property path in the format ``/World/Prim.propertyName``. + value: Value to set. If None, the attribute value goes to its default value. + If the attribute has no default value, it is a silent no-op. + stage: The USD stage. Defaults to None, in which case the current stage is used. + type_to_create_if_not_exist: If not None and property doesn't exist, a new property will + be created with the given type and value. Defaults to None. + is_custom: If the property is created, specify if it is a custom property (not part of + the schema). Defaults to False. + + Returns: + True if the property was successfully changed, False otherwise. + + Raises: + ValueError: If the prim does not exist at the specified path. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import Sdf + >>> + >>> # Change an existing property + >>> sim_utils.change_prim_property(prop_path="/World/Cube.size", value=2.0) + True + >>> + >>> # Create a new custom property + >>> sim_utils.change_prim_property( + ... prop_path="/World/Cube.customValue", + ... value=42, + ... type_to_create_if_not_exist=Sdf.ValueTypeNames.Int, + ... is_custom=True, + ... ) + True + """ + # get stage handle + stage = get_current_stage() if stage is None else stage + + # convert to Sdf.Path if needed + prop_path = Sdf.Path(prop_path) if isinstance(prop_path, str) else prop_path + + # get the prim path + prim_path = prop_path.GetAbsoluteRootOrPrimPath() + prim = stage.GetPrimAtPath(prim_path) + if not prim or not prim.IsValid(): + raise ValueError(f"Prim does not exist at path: '{prim_path}'") + + # get or create the property + prop = stage.GetPropertyAtPath(prop_path) + + if not prop: + if type_to_create_if_not_exist is not None: + # create new attribute on the prim + prop = prim.CreateAttribute(prop_path.name, type_to_create_if_not_exist, is_custom) + else: + logger.error(f"Property {prop_path} does not exist and 'type_to_create_if_not_exist' was not provided.") + return False + + if not prop: + logger.error(f"Failed to get or create property at path: '{prop_path}'") + return False + + # set the value + if value is None: + return bool(prop.Clear()) + else: + return bool(prop.Set(value, Usd.TimeCode.Default())) + + """ -Decorators. +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 '/'). + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # 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 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() + + +""" +Decorators """ @@ -160,7 +620,8 @@ def wrapper(prim_path: str | Sdf.Path, *args, **kwargs): # get current stage stage = bound_args.arguments.get("stage") if stage is None: - stage = stage_utils.get_current_stage() + stage = get_current_stage() + # get USD prim prim: Usd.Prim = stage.GetPrimAtPath(prim_path) # check if prim is valid @@ -189,7 +650,7 @@ def wrapper(prim_path: str | Sdf.Path, *args, **kwargs): count_success += 1 # check if we were successful in applying the function to any prim if count_success == 0: - omni.log.warn( + logger.warning( 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." @@ -222,6 +683,9 @@ def clone(func: Callable) -> Callable: @functools.wraps(func) def wrapper(prim_path: str | Sdf.Path, cfg: SpawnerCfg, *args, **kwargs): + # get stage handle + stage = get_current_stage() + # cast prim_path to str type in case its an Sdf.Path prim_path = str(prim_path) # check prim path is global @@ -263,22 +727,35 @@ def wrapper(prim_path: str | Sdf.Path, cfg: SpawnerCfg, *args, **kwargs): # 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) + # add labels to the prim + add_labels( + prim, labels=[semantic_value_sanitized], instance_name=semantic_type_sanitized, overwrite=False + ) + # activate rigid body contact sensors (lazy import to avoid circular import with schemas) + if hasattr(cfg, "activate_contact_sensors") and cfg.activate_contact_sensors: # type: ignore + from ..schemas import schemas as _schemas + + _schemas.activate_contact_sensors(prim_paths[0]) # 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) + cloner = Cloner(stage=stage) + # check version of Isaac Sim to determine whether clone_in_fabric is valid + if get_isaac_sim_version().major < 5: + # clone the prim + cloner.clone( + prim_paths[0], prim_paths[1:], replicate_physics=False, copy_from_source=cfg.copy_from_source + ) + else: + # clone the prim + clone_in_fabric = kwargs.get("clone_in_fabric", False) + replicate_physics = kwargs.get("replicate_physics", False) + cloner.clone( + prim_paths[0], + prim_paths[1:], + replicate_physics=replicate_physics, + copy_from_source=cfg.copy_from_source, + clone_in_fabric=clone_in_fabric, + ) # return the source prim return prim @@ -318,9 +795,10 @@ def bind_visual_material( Raises: ValueError: If the provided prim paths do not exist on stage. """ - # resolve stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = 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.") @@ -328,6 +806,7 @@ def bind_visual_material( raise ValueError(f"Visual material '{material_path}' does not exist.") # resolve token for weaker than descendants + # bind material command expects a string token if stronger_than_descendants: binding_strength = "strongerThanDescendants" else: @@ -362,7 +841,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. @@ -375,9 +854,10 @@ def bind_physics_material( Raises: ValueError: If the provided prim paths do not exist on stage. """ - # resolve stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = 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.") @@ -391,7 +871,7 @@ def bind_physics_material( 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( + logger.debug( 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." ) @@ -403,6 +883,7 @@ def bind_physics_material( 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: @@ -416,378 +897,104 @@ def bind_physics_material( """ -Exporting. +USD References and Variants. """ -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. +def add_usd_reference( + prim_path: str, usd_path: str, prim_type: str = "Xform", stage: Usd.Stage | None = None +) -> Usd.Prim: + """Adds a USD reference at the specified prim path on the provided stage. - 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 + This function adds a reference to an external USD file at the specified prim path on the provided stage. + If the prim does not exist, it will be created with the specified type. - -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. + The function also handles stage units verification to ensure compatibility. For instance, + if the current stage is in meters and the referenced USD file is in centimeters, the function will + convert the units to match. This is done using the :mod:`omni.metrics.assembler` functionality. 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. + prim_path: The prim path where the reference will be attached. + usd_path: The path to USD file to reference. + prim_type: The type of prim to create if it doesn't exist. Defaults to "Xform". + stage: The stage to add the reference to. Defaults to None, in which case the current stage is used. Returns: - A list containing all the prims matching the predicate. + The USD prim at the specified prim path. Raises: - ValueError: If the prim path is not global (i.e: does not start with '/'). + FileNotFoundError: When the input USD file is not found at the specified path. """ - # 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 + stage = get_current_stage() if stage is None else stage + # get prim at path 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. + prim = stage.DefinePrim(prim_path, prim_type) + + def _add_reference_to_prim(prim: Usd.Prim) -> Usd.Prim: + """Helper function to add a reference to a prim.""" + success_bool = prim.GetReferences().AddReference(usd_path) + if not success_bool: + raise RuntimeError( + f"Unable to add USD reference to the prim at path: {prim_path} from the USD file at path: {usd_path}" + ) + return prim - Returns: - A list of prim paths that match input expression. + # Compatibility with Isaac Sim 4.5 where omni.metrics is not available + if get_isaac_sim_version().major < 5: + return _add_reference_to_prim(prim) - 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 + # check if the USD file is valid and add reference to the prim + sdf_layer = Sdf.Layer.FindOrOpen(usd_path) + if not sdf_layer: + raise FileNotFoundError(f"Unable to open the usd file at path: {usd_path}") + # import metrics assembler interface + # note: this is only available in Isaac Sim 5.0 and above + from omni.metrics.assembler.core import get_metrics_assembler_interface -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. + # obtain the stage ID + stage_id = get_current_stage_id() + # check if the layers are compatible (i.e. the same units) + ret_val = get_metrics_assembler_interface().check_layers( + stage.GetRootLayer().identifier, sdf_layer.identifier, stage_id + ) + # log that metric assembler did not detect any issues + if ret_val["ret_val"]: + logger.info( + "Metric assembler detected no issues between the current stage and the referenced USD file at path:" + f" {usd_path}" + ) + # add reference to the prim + return _add_reference_to_prim(prim) - 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. +def get_usd_references(prim_path: str, stage: Usd.Stage | None = None) -> list[str]: + """Gets the USD references at the specified prim path on the provided stage. 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. + prim_path: The prim path to get the USD references from. + stage: The stage to get the USD references from. 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. + A list of USD reference paths. 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. + ValueError: If the prim at the specified path is not valid. """ - # 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 + # get stage handle + stage = get_current_stage() if stage is None else stage + # get prim at path 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. -""" + # get USD references + references = [] + for prim_spec in prim.GetPrimStack(): + for ref in prim_spec.referenceList.prependedItems: + references.append(str(ref.assetPath)) + return references def select_usd_variants(prim_path: str, variants: object | dict[str, str], stage: Usd.Stage | None = None): @@ -821,6 +1028,7 @@ class TableVariants: color: Literal["blue", "red"] = "red" size: Literal["small", "large"] = "large" + select_usd_variants( prim_path="/World/Table", variants=TableVariants(), @@ -836,29 +1044,86 @@ class TableVariants: .. _USD Variants: https://graphics.pixar.com/usd/docs/USD-Glossary.html#USDGlossary-Variant """ - # Resolve stage + # get stage handle if stage is None: - stage = stage_utils.get_current_stage() + stage = 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() + variants = variants.to_dict() # type: ignore existing_variant_sets = prim.GetVariantSets() - for variant_set_name, variant_selection in variants.items(): + for variant_set_name, variant_selection in variants.items(): # type: ignore # 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}'.") + logger.warning(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( + logger.info( f"Setting variant selection '{variant_selection}' for variant set '{variant_set_name}' on" f" prim '{prim_path}'." ) + + +""" +Internal Helpers. +""" + + +def _to_tuple(value: Any) -> tuple[float, ...]: + """Convert various sequence types to a Python tuple of floats. + + This function provides robust conversion from different array-like types (list, tuple, numpy array, + torch tensor) to Python tuples. It handles edge cases like malformed sequences, CUDA tensors, + and arrays with singleton dimensions. + + Args: + value: A sequence-like object containing floats. Supported types include: + - Python list or tuple + - NumPy array (any device) + - PyTorch tensor (CPU or CUDA) + - Mixed sequences with numpy/torch scalar items and float values + + Returns: + A one-dimensional tuple of floats. + + Raises: + ValueError: If the input value is not one-dimensional after squeezing singleton dimensions. + + Example: + >>> import torch + >>> import numpy as np + >>> + >>> _to_tuple([1.0, 2.0, 3.0]) + (1.0, 2.0, 3.0) + >>> _to_tuple(torch.tensor([[1.0, 2.0]])) # Squeezes first dimension + (1.0, 2.0) + >>> _to_tuple(np.array([1.0, 2.0, 3.0])) + (1.0, 2.0, 3.0) + >>> _to_tuple((1.0, 2.0, 3.0)) + (1.0, 2.0, 3.0) + + """ + # Normalize to tensor if value is a plain sequence (list with mixed types, etc.) + # This handles cases like [np.float32(1.0), 2.0, torch.tensor(3.0)] + if not hasattr(value, "tolist"): + value = torch.tensor(value, device="cpu", dtype=torch.float) + + # Remove leading singleton dimension if present (e.g., shape (1, 3) -> (3,)) + # This is common when batched operations produce single-item batches + if value.ndim != 1: + value = value.squeeze() + # Validate that the result is one-dimensional + if value.ndim != 1: + raise ValueError(f"Input value is not one dimensional: {value.shape}") + + # Convert to tuple - works for both numpy arrays and torch tensors + return tuple(value.tolist()) diff --git a/source/isaaclab/isaaclab/sim/utils/queries.py b/source/isaaclab/isaaclab/sim/utils/queries.py new file mode 100644 index 000000000000..035681a726b4 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/queries.py @@ -0,0 +1,407 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for querying the USD stage.""" + +from __future__ import annotations + +import logging +import re +from collections.abc import Callable + +import omni +import omni.kit.app +from pxr import Sdf, Usd, UsdPhysics + +from .stage import get_current_stage + +# import logger +logger = logging.getLogger(__name__) + + +def get_next_free_prim_path(path: str, stage: Usd.Stage | None = None) -> str: + """Gets a new prim path that doesn't exist in the stage given a base path. + + If the given path doesn't exist in the stage already, it returns the given path. Otherwise, + it appends a suffix with an incrementing number to the given path. + + Args: + path: The base prim path to check. + stage: The stage to check. Defaults to the current stage. + + Returns: + A new path that is guaranteed to not exist on the current stage + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> # given the stage: /World/Cube, /World/Cube_01. + >>> # Get the next available path for /World/Cube + >>> sim_utils.get_next_free_prim_path("/World/Cube") + /World/Cube_02 + """ + # get current stage + stage = get_current_stage() if stage is None else stage + # get next free path + return omni.usd.get_stage_next_free_path(stage, path, True) + + +def get_first_matching_ancestor_prim( + prim_path: str | Sdf.Path, + predicate: Callable[[Usd.Prim], bool], + stage: Usd.Stage | None = None, +) -> Usd.Prim | None: + """Gets the first ancestor prim that passes the predicate function. + + This function walks up the prim hierarchy starting from the target prim and returns the first ancestor prim + that passes the predicate function. This includes the prim itself if it passes the predicate. + + 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 ancestor prim that passes the predicate. If no ancestor prim passes the predicate, it returns None. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # 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 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.") + + # walk up to find the first matching ancestor prim + ancestor_prim = prim + while ancestor_prim and ancestor_prim.IsValid(): + # check if prim passes predicate + if predicate(ancestor_prim): + return ancestor_prim + # get parent prim + ancestor_prim = ancestor_prim.GetParent() + + # If no ancestor prim passes the predicate, return None + return None + + +def get_first_matching_child_prim( + prim_path: str | Sdf.Path, + predicate: Callable[[Usd.Prim], bool], + stage: Usd.Stage | None = None, + traverse_instance_prims: bool = True, +) -> Usd.Prim | None: + """Recursively get the first USD Prim at the path string that passes the predicate function. + + This function performs a depth-first traversal of the prim hierarchy starting from + :attr:`prim_path`, returning the first prim that satisfies the provided :attr:`predicate`. + It optionally supports traversal through instance prims, which are normally skipped in standard USD + traversals. + + USD instance prims are lightweight copies of prototype scene structures and are not included + in default traversals unless explicitly handled. This function allows traversing into instances + when :attr:`traverse_instance_prims` is set to :attr:`True`. + + .. versionchanged:: 2.3.0 + + Added :attr:`traverse_instance_prims` to control whether to traverse instance prims. + By default, instance prims are now traversed. + + 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. + traverse_instance_prims: Whether to traverse instance prims. Defaults to True. + + 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 '/'). + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # 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 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 + if traverse_instance_prims: + all_prims += child_prim.GetFilteredChildren(Usd.TraverseInstanceProxies()) + else: + 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, + traverse_instance_prims: bool = True, +) -> list[Usd.Prim]: + """Performs a search starting from the root and returns all the prims matching the predicate. + + This function performs a depth-first traversal of the prim hierarchy starting from + :attr:`prim_path`, returning all prims that satisfy the provided :attr:`predicate`. It optionally + supports traversal through instance prims, which are normally skipped in standard USD traversals. + + USD instance prims are lightweight copies of prototype scene structures and are not included + in default traversals unless explicitly handled. This function allows traversing into instances + when :attr:`traverse_instance_prims` is set to :attr:`True`. + + .. versionchanged:: 2.3.0 + + Added :attr:`traverse_instance_prims` to control whether to traverse instance prims. + By default, instance prims are now traversed. + + 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. + traverse_instance_prims: Whether to traverse instance prims. Defaults to True. + + 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 '/'). + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # 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 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: + # resolve prims under the current prim + if traverse_instance_prims: + children = child_prim.GetFilteredChildren(Usd.TraverseInstanceProxies()) + else: + children = child_prim.GetChildren() + # add children to list + all_prims_queue += [(child, current_depth + 1) for child in children] + + 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 '/'). + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # 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 '/'.") + prim_path_regex = _normalize_legacy_wildcard_pattern(prim_path_regex) + # 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 _normalize_legacy_wildcard_pattern(prim_path_regex: str) -> str: + """Convert legacy '*' wildcard usage to '.*' and warn users.""" + fixed_regex = re.sub(r"(? 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 '/'). + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # normalize legacy wildcard pattern + prim_path_regex = _normalize_legacy_wildcard_pattern(prim_path_regex) + + # 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 '/'.") + # 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. + """ + # get stage handle + if stage is None: + stage = get_current_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 '/'.") + + # 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 diff --git a/source/isaaclab/isaaclab/sim/utils/semantics.py b/source/isaaclab/isaaclab/sim/utils/semantics.py new file mode 100644 index 000000000000..463eb8b5d42f --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/semantics.py @@ -0,0 +1,232 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for applying and removing semantic labels to USD prims.""" + +from __future__ import annotations + +import contextlib +import logging + +from pxr import Usd, UsdGeom + +# USD Semantics is only available in Isaac Sim 5.0 and later. +with contextlib.suppress(ModuleNotFoundError, ImportError): + from pxr import UsdSemantics + +from isaaclab.utils.version import get_isaac_sim_version + +from .stage import get_current_stage + +# import logger +logger = logging.getLogger(__name__) + + +def add_labels(prim: Usd.Prim, labels: list[str], instance_name: str = "class", overwrite: bool = True) -> None: + """Apply semantic labels to a prim using the :class:`UsdSemantics.LabelsAPI`. + + This function is a wrapper around the :func:`omni.replicator.core.functional.modify.semantics` function. + It applies the labels to the prim using the :class:`UsdSemantics.LabelsAPI`. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later, which introduces the :class:`UsdSemantics.LabelsAPI`. + For previous versions, the function falls back to use the deprecated :class:`UsdSemantics.SemanticsAPI` instead. + + Example: + >>> prim = sim_utils.create_prim("/World/Test/Sphere", "Sphere", stage=stage, attributes={"radius": 10.0}) + >>> sim_utils.add_labels(prim, labels=["sphere"], instance_name="class") + + Args: + prim: The USD prim to add or update labels on. + labels: The list of labels to apply. + instance_name: The name of the semantic instance. Defaults to "class". + overwrite: Whether to overwrite existing labels for this instance. If False, + the new labels are appended to existing ones (if any). Defaults to True. + """ + # Try modern approach (Isaac Sim >= 5.0) + try: + import omni.replicator.core.functional as rep_functional + + mode = "replace" if overwrite else "add" + rep_functional.modify.semantics(prim, {instance_name: labels}, mode=mode) + + return + except (ModuleNotFoundError, ImportError) as e: + # check if we are using isaac sim 5.0 + if get_isaac_sim_version().major >= 5: + logger.warning( + f"Failed to add labels to prim {prim.GetPath()} using Replicator API: {e}. " + "\nPlease ensure Replicator API is enabled by passing '--enable_cameras' to the AppLauncher." + "\nFalling back to legacy approach." + ) + + # Try legacy approach (Isaac Sim < 5.0) + try: + import Semantics + + # check we have only one label + if len(labels) != 1: + raise ValueError(f"Only one label can be applied to a prim. Received: {labels}") + # set the semantic API for the instance + instance_name = f"{instance_name}_{labels[0]}" + sem = Semantics.SemanticsAPI.Apply(prim, instance_name) + # create semantic type and data attributes + sem.CreateSemanticTypeAttr() + sem.CreateSemanticDataAttr() + sem.GetSemanticTypeAttr().Set(instance_name) + sem.GetSemanticDataAttr().Set(labels[0]) + except Exception as e: + logger.warning( + f"Failed to add labels to prim {prim.GetPath()} using legacy API: {e}. " + "\nSemantics functionality may not be available in this Isaac Sim version." + " Please open an issue at https://github.com/isaac-sim/IsaacLab/issues if you believe this is a bug." + ) + + +def get_labels(prim: Usd.Prim) -> dict[str, list[str]]: + """Get all semantic labels (:class:`UsdSemantics.LabelsAPI`) applied to a prim. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later. For previous versions, + please use :mod:`isaacsim.core.utils.semantics` module instead. + + Args: + prim: The USD prim to return labels for. + + Returns: + A dictionary mapping instance names to a list of labels. + If no labels are found, it returns an empty dictionary. + """ + result = {} + for schema_name in prim.GetAppliedSchemas(): + if schema_name.startswith("SemanticsLabelsAPI:"): + instance_name = schema_name.split(":", 1)[1] + sem_api = UsdSemantics.LabelsAPI(prim, instance_name) + labels_attr = sem_api.GetLabelsAttr() + if labels_attr: + labels = labels_attr.Get() + result[instance_name] = list(labels) if labels is not None else [] + else: + result[instance_name] = [] + return result + + +def remove_labels(prim: Usd.Prim, instance_name: str | None = None, include_descendants: bool = False): + """Removes semantic labels (:class:`UsdSemantics.LabelsAPI`) from a prim and optionally its descendants. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later. For previous versions, + please use :mod:`isaacsim.core.utils.semantics` module instead. + + Args: + prim: The USD prim to remove labels from. + instance_name: The specific instance name to remove. Defaults to None, in which case + *all* labels are removed. + include_descendants: Whether to also traverse children and remove labels recursively. + Defaults to False. + """ + + def _remove_single_prim_labels(target_prim: Usd.Prim): + """Helper function to remove labels from a single prim.""" + schemas_to_remove = [] + for schema_name in target_prim.GetAppliedSchemas(): + if schema_name.startswith("SemanticsLabelsAPI:"): + current_instance = schema_name.split(":", 1)[1] + if instance_name is None or current_instance == instance_name: + schemas_to_remove.append(current_instance) + + for inst_to_remove in schemas_to_remove: + target_prim.RemoveAPI(UsdSemantics.LabelsAPI, inst_to_remove) + + if include_descendants: + for p in Usd.PrimRange(prim): + _remove_single_prim_labels(p) + else: + _remove_single_prim_labels(prim) + + +def check_missing_labels(prim_path: str | None = None, stage: Usd.Stage | None = None) -> list[str]: + """Checks whether the prim and its descendants at the provided path have missing + semantic labels (:class:`UsdSemantics.LabelsAPI`). + + .. note:: + The function checks only prims that are :class:`UsdGeom.Gprim` type. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later. For previous versions, + please use :mod:`isaacsim.core.utils.semantics` module instead. + + Args: + prim_path: The prim path to search from. If None, the entire stage is inspected. + stage: The stage to search from. If None, the current stage is used. + + Returns: + A list containing prim paths to prims with no labels applied. + """ + # check if stage is valid + stage = stage if stage else get_current_stage() + + # check if inspect path is valid + start_prim = stage.GetPrimAtPath(prim_path) if prim_path else stage.GetPseudoRoot() + if not start_prim: + # Allow None prim_path for whole stage check, warn if path specified but not found + if prim_path: + logger.warning(f"No prim found at path '{prim_path}'. Returning from check for semantic labels.") + return [] + + # iterate over prim and its children + prim_paths = [] + for prim in Usd.PrimRange(start_prim): + if prim.IsA(UsdGeom.Gprim): + has_any_label = False + for schema_name in prim.GetAppliedSchemas(): + if schema_name.startswith("SemanticsLabelsAPI:"): + has_any_label = True + break + if not has_any_label: + prim_paths.append(prim.GetPath().pathString) + + return prim_paths + + +def count_total_labels(prim_path: str | None = None, stage: Usd.Stage | None = None) -> dict[str, int]: + """Counts the number of semantic labels (:class:`UsdSemantics.LabelsAPI`) applied to the prims at the provided path. + + This function iterates over all the prims from the provided path and counts the number of times + each label is applied to the prims. It returns a dictionary of labels and their corresponding count. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later. For previous versions, + please use :mod:`isaacsim.core.utils.semantics` module instead. + + Args: + prim_path: The prim path to search from. If None, the entire stage is inspected. + stage: The stage to search from. If None, the current stage is used. + + Returns: + A dictionary mapping individual labels to their total count across all instances. + The dictionary includes a 'missing_labels' count for prims with no labels. + """ + stage = stage if stage else get_current_stage() + + start_prim = stage.GetPrimAtPath(prim_path) if prim_path else stage.GetPseudoRoot() + if not start_prim: + if prim_path: + logger.warning(f"No prim found at path '{prim_path}'. Returning from count for semantic labels.") + return {"missing_labels": 0} + + labels_counter = {"missing_labels": 0} + for prim in Usd.PrimRange(start_prim): + if prim.IsA(UsdGeom.Gprim): + labels_dict = get_labels(prim) + if not labels_dict: + labels_counter["missing_labels"] += 1 + else: + # Iterate through all labels from all instances on the prim + all_labels = [label for sublist in labels_dict.values() for label in sublist if label] + for label in all_labels: + labels_counter[label] = labels_counter.get(label, 0) + 1 + + return labels_counter diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py new file mode 100644 index 000000000000..88c61744e7db --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -0,0 +1,492 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for operating on the USD stage.""" + +import builtins +import contextlib +import logging +import threading +from collections.abc import Callable, Generator + +import omni.kit.app +import omni.usd +from isaacsim.core.utils import stage as sim_stage +from pxr import Sdf, Usd, UsdUtils + +from isaaclab.utils.version import get_isaac_sim_version + +# import logger +logger = logging.getLogger(__name__) +_context = threading.local() # thread-local storage to handle nested contexts and concurrent access + +# _context is a singleton design in isaacsim and for that reason +# until we fully replace all modules that references the singleton(such as XformPrim, Prim ....), we have to point +# that singleton to this _context +sim_stage._context = _context # type: ignore + + +def create_new_stage() -> Usd.Stage: + """Create a new stage attached to the USD context. + + Returns: + Usd.Stage: The created USD stage. + + Raises: + RuntimeError: When failed to create a new stage. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.create_new_stage() + Usd.Stage.Open(rootLayer=Sdf.Find('anon:0x7fba6c04f840:World7.usd'), + sessionLayer=Sdf.Find('anon:0x7fba6c01c5c0:World7-session.usda'), + pathResolverContext=) + """ + result = omni.usd.get_context().new_stage() + if result: + return omni.usd.get_context().get_stage() + else: + raise RuntimeError("Failed to create a new stage. Please check if the USD context is valid.") + + +def create_new_stage_in_memory() -> Usd.Stage: + """Creates a new stage in memory, if supported. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later. For backwards + compatibility, it falls back to creating a new stage attached to the USD context. + + Returns: + The new stage in memory. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.create_new_stage_in_memory() + Usd.Stage.Open(rootLayer=Sdf.Find('anon:0xf7b00e0:tmp.usda'), + sessionLayer=Sdf.Find('anon:0xf7cd2e0:tmp-session.usda'), + pathResolverContext=) + """ + if get_isaac_sim_version().major < 5: + logger.warning( + "Isaac Sim < 5.0 does not support creating a new stage in memory. Falling back to creating a new" + " stage attached to USD context." + ) + return create_new_stage() + else: + return Usd.Stage.CreateInMemory() + + +def is_current_stage_in_memory() -> bool: + """Checks if the current stage is in memory. + + This function compares the stage id of the current USD stage with the stage id of the USD context stage. + + Returns: + Whether the current stage is in memory. + """ + # grab current stage id + stage_id = get_current_stage_id() + + # grab context stage id + context_stage = omni.usd.get_context().get_stage() + with use_stage(context_stage): + context_stage_id = get_current_stage_id() + + # check if stage ids are the same + return stage_id != context_stage_id + + +def open_stage(usd_path: str) -> bool: + """Open the given usd file and replace currently opened stage. + + Args: + usd_path: The path to the USD file to open. + + Returns: + True if operation is successful, otherwise False. + + Raises: + ValueError: When input path is not a supported file type by USD. + """ + # check if USD file is supported + if not Usd.Stage.IsSupportedFile(usd_path): + raise ValueError(f"The USD file at path '{usd_path}' is not supported.") + + # get USD context + usd_context = omni.usd.get_context() + # disable save to recent files + usd_context.disable_save_to_recent_files() + # open stage + result = usd_context.open_stage(usd_path) + # enable save to recent files + usd_context.enable_save_to_recent_files() + # return result + return result + + +@contextlib.contextmanager +def use_stage(stage: Usd.Stage) -> Generator[None, None, None]: + """Context manager that sets a thread-local stage, if supported. + + This function binds the stage to the thread-local context for the duration of the context manager. + During the context manager, any call to :func:`get_current_stage` will return the stage specified + in the context manager. After the context manager is exited, the stage is restored to the default + stage attached to the USD context. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later. For backwards + compatibility, it falls back to a no-op context manager in Isaac Sim < 5.0. + + Args: + stage: The stage to set in the context. + + Returns: + A context manager that sets the stage in the context. + + Raises: + AssertionError: If the stage is not a USD stage instance. + + Example: + >>> from pxr import Usd + >>> import isaaclab.sim as sim_utils + >>> + >>> stage_in_memory = Usd.Stage.CreateInMemory() + >>> with sim_utils.use_stage(stage_in_memory): + ... # operate on the specified stage + ... pass + >>> # operate on the default stage attached to the USD context + """ + if get_isaac_sim_version().major < 5: + logger.warning("Isaac Sim < 5.0 does not support thread-local stage contexts. Skipping use_stage().") + yield # no-op + else: + # check stage + if not isinstance(stage, Usd.Stage): + raise TypeError(f"Expected a USD stage instance, got: {type(stage)}") + # store previous context value if it exists + previous_stage = getattr(_context, "stage", None) + # set new context value + try: + _context.stage = stage + yield + # remove context value or restore previous one if it exists + finally: + if previous_stage is None: + delattr(_context, "stage") + else: + _context.stage = previous_stage + + +def update_stage() -> None: + """Updates the current stage by triggering an application update cycle. + + This function triggers a single update cycle of the application interface, which + in turn updates the stage and all associated systems (rendering, physics, etc.). + This is necessary to ensure that changes made to the stage are properly processed + and reflected in the simulation. + + Note: + This function calls the application update interface rather than directly + updating the stage because the stage update is part of the broader + application update cycle that includes rendering, physics, and other systems. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.update_stage() + """ + # TODO: Why is this updating the simulation and not the stage? + omni.kit.app.get_app_interface().update() + + +def save_stage(usd_path: str, save_and_reload_in_place: bool = True) -> bool: + """Saves contents of the root layer of the current stage to the specified USD file. + + If the file already exists, it will be overwritten. + + Args: + usd_path: The file path to save the current stage to + save_and_reload_in_place: Whether to open the saved USD file in place. Defaults to True. + + Returns: + True if operation is successful, otherwise False. + + Raises: + ValueError: When input path is not a supported file type by USD. + RuntimeError: When layer creation or save operation fails. + """ + # check if USD file is supported + if not Usd.Stage.IsSupportedFile(usd_path): + raise ValueError(f"The USD file at path '{usd_path}' is not supported.") + + # create new layer + layer = Sdf.Layer.CreateNew(usd_path) + if layer is None: + raise RuntimeError(f"Failed to create new USD layer at path '{usd_path}'.") + + # get root layer + root_layer = get_current_stage().GetRootLayer() + # transfer content from root layer to new layer + layer.TransferContent(root_layer) + # resolve paths + omni.usd.resolve_paths(root_layer.identifier, layer.identifier) + # save layer + result = layer.Save() + if not result: + logger.error(f"Failed to save USD layer to path '{usd_path}'.") + + # if requested, open the saved USD file in place + if save_and_reload_in_place and result: + open_stage(usd_path) + + return result + + +def close_stage(callback_fn: Callable[[bool, str], None] | None = None) -> bool: + """Closes the current USD stage. + + .. note:: + + Once the stage is closed, it is necessary to open a new stage or create a + new one in order to work on it. + + Args: + callback_fn: A callback function to call while closing the stage. + The function should take two arguments: a boolean indicating whether the stage is closing + and a string indicating the error message if the stage closing fails. Defaults to None, + in which case the stage will be closed without a callback. + + Returns: + True if operation is successful, otherwise False. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.close_stage() + True + >>> + + Example with callback function: + >>> import isaaclab.sim as sim_utils + >>> + >>> def callback(*args, **kwargs): + ... print("callback:", args, kwargs) + >>> sim_utils.close_stage(callback) + True + >>> sim_utils.close_stage(callback) + callback: (False, 'Stage opening or closing already in progress!!') {} + False + """ + if callback_fn is None: + result = omni.usd.get_context().close_stage() + else: + result = omni.usd.get_context().close_stage_with_callback(callback_fn) + return result + + +def clear_stage(predicate: Callable[[Usd.Prim], bool] | None = None) -> None: + """Deletes all prims in the stage without populating the undo command buffer. + + The function will delete all prims in the stage that satisfy the predicate. If the predicate + is None, a default predicate will be used that deletes all prims. The default predicate deletes + all prims that are not the root prim, are not under the /Render namespace, have the ``no_delete`` + metadata, are not ancestral to any other prim, and are not hidden in the stage window. + + Args: + predicate: A user defined function that takes the USD prim as an argument and + returns a boolean indicating if the prim should be deleted. If the predicate is None, + a default predicate will be used that deletes all prims. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> # clear the whole stage + >>> sim_utils.clear_stage() + >>> + >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Delete only the prims of type Cube + >>> predicate = lambda _prim: _prim.GetTypeName() == "Cube" + >>> sim_utils.clear_stage(predicate) # after the execution the stage will be /World + """ + # Note: Need to import this here to prevent circular dependencies. + from .prims import delete_prim + from .queries import get_all_matching_child_prims + + def _default_predicate(prim: Usd.Prim) -> bool: + """Check if the prim should be deleted.""" + prim_path = prim.GetPath().pathString + if prim_path == "/": + return False + if prim_path.startswith("/Render"): + return False + if prim.GetMetadata("no_delete"): + return False + if prim.GetMetadata("hide_in_stage_window"): + return False + if omni.usd.check_ancestral(prim): + return False + return True + + def _predicate_from_path(prim: Usd.Prim) -> bool: + if predicate is None: + return _default_predicate(prim) + return predicate(prim) + + # get all prims to delete + if predicate is None: + prims = get_all_matching_child_prims("/", _default_predicate) + else: + prims = get_all_matching_child_prims("/", _predicate_from_path) + # convert prims to prim paths + prim_paths_to_delete = [prim.GetPath().pathString for prim in prims] + # delete prims + delete_prim(prim_paths_to_delete) + + if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: # type: ignore + omni.kit.app.get_app_interface().update() + + +def is_stage_loading() -> bool: + """Convenience function to see if any files are being loaded. + + Returns: + True if loading, False otherwise + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.is_stage_loading() + False + """ + context = omni.usd.get_context() + if context is None: + return False + else: + _, _, loading = context.get_stage_loading_status() + return loading > 0 + + +def get_current_stage(fabric: bool = False) -> Usd.Stage: + """Get the current open USD or Fabric stage + + Args: + fabric: True to get the fabric stage. False to get the USD stage. Defaults to False. + + Returns: + The USD or Fabric stage as specified by the input arg fabric. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.get_current_stage() + Usd.Stage.Open(rootLayer=Sdf.Find('anon:0x7fba6c04f840:World7.usd'), + sessionLayer=Sdf.Find('anon:0x7fba6c01c5c0:World7-session.usda'), + pathResolverContext=) + """ + stage = getattr(_context, "stage", omni.usd.get_context().get_stage()) + + if fabric: + import usdrt + + # Get stage ID and attach to Fabric stage + stage_id = get_current_stage_id() + return usdrt.Usd.Stage.Attach(stage_id) + + return stage + + +def get_current_stage_id() -> int: + """Get the current open stage ID. + + Returns: + The current open stage id. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> sim_utils.get_current_stage_id() + 1234567890 + """ + # get current stage + stage = get_current_stage() + # retrieve stage ID from stage cache + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(stage).ToLongInt() + # if stage ID is not found, insert it into the stage cache + if stage_id < 0: + stage_id = stage_cache.Insert(stage).ToLongInt() + # return stage ID + return stage_id + + +def attach_stage_to_usd_context(attaching_early: bool = False): + """Attaches the current USD stage in memory to the USD context. + + This function should be called during or after scene is created and before stage is simulated or rendered. + If the stage is not in memory or rendering is not enabled, this function will return without attaching. + + .. versionadded:: 2.3.0 + This function is available in Isaac Sim 5.0 and later. For backwards + compatibility, it returns without attaching to the USD context. + + Args: + attaching_early: Whether to attach the stage to the usd context before stage is created. Defaults to False. + """ + + import carb + import omni.physx + import omni.usd + from isaacsim.core.simulation_manager import SimulationManager + + from isaaclab.sim.simulation_context import SimulationContext + + # if Isaac Sim version is less than 5.0, stage in memory is not supported + if get_isaac_sim_version().major < 5: + return + + # if stage is not in memory, we can return early + if not is_current_stage_in_memory(): + return + + # attach stage to physx + stage_id = get_current_stage_id() + physx_sim_interface = omni.physx.get_physx_simulation_interface() + physx_sim_interface.attach_stage(stage_id) + + # this carb flag is equivalent to if rendering is enabled + carb_setting = carb.settings.get_settings() # type: ignore + is_rendering_enabled = carb_setting.get("/physics/fabricUpdateTransformations") + + # if rendering is not enabled, we don't need to attach it + if not is_rendering_enabled: + return + + # early attach warning msg + if attaching_early: + logger.warning( + "Attaching stage in memory to USD context early to support an operation which" + " does not support stage in memory." + ) + + # skip this callback to avoid wiping the stage after attachment + SimulationContext.instance().skip_next_stage_open_callback() + + # disable stage open callback to avoid clearing callbacks + SimulationManager.enable_stage_open_callback(False) + + # enable physics fabric + SimulationContext.instance()._physics_context.enable_fabric(True) # type: ignore + + # attach stage to usd context + omni.usd.get_context().attach_stage_with_callback(stage_id) + + # attach stage to physx + physx_sim_interface = omni.physx.get_physx_simulation_interface() + physx_sim_interface.attach_stage(stage_id) + + # re-enable stage open callback + SimulationManager.enable_stage_open_callback(True) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py new file mode 100644 index 000000000000..d7ae57a16a6a --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -0,0 +1,453 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for working with USD transform (xform) operations. + +This module provides utilities for manipulating USD transform operations (xform ops) on prims. +Transform operations in USD define how geometry is positioned, oriented, and scaled in 3D space. + +The utilities in this module help standardize transform stacks, clear operations, and manipulate +transforms in a consistent way across different USD assets. +""" + +from __future__ import annotations + +import logging + +from pxr import Gf, Sdf, Usd, UsdGeom + +# import logger +logger = logging.getLogger(__name__) + +_INVALID_XFORM_OPS = [ + "xformOp:rotateX", + "xformOp:rotateXZY", + "xformOp:rotateY", + "xformOp:rotateYXZ", + "xformOp:rotateYZX", + "xformOp:rotateZ", + "xformOp:rotateZYX", + "xformOp:rotateZXY", + "xformOp:rotateXYZ", + "xformOp:transform", +] +"""List of invalid xform ops that should be removed.""" + + +def standardize_xform_ops( + prim: Usd.Prim, + translation: tuple[float, ...] | None = None, + orientation: tuple[float, ...] | None = None, + scale: tuple[float, ...] | None = None, +) -> bool: + """Standardize the transform operation stack on a USD prim to a canonical form. + + This function converts a prim's transform stack to use the standard USD transform operation + order: [translate, orient, scale]. The function performs the following operations: + + 1. Validates that the prim is Xformable + 2. Captures the current local transform (translation, rotation, scale) + 3. Resolves and bakes unit scale conversions (xformOp:scale:unitsResolve) + 4. Creates or reuses standard transform operations (translate, orient, scale) + 5. Sets the transform operation order to [translate, orient, scale] + 6. Applies the preserved or user-specified transform values + + The entire modification is performed within an ``Sdf.ChangeBlock`` for optimal performance + when processing multiple prims. + + .. note:: + **Standard Transform Order:** The function enforces the USD best practice order: + ``xformOp:translate``, ``xformOp:orient``, ``xformOp:scale``. This order is + compatible with most USD tools and workflows, and uses quaternions for rotation + (avoiding gimbal lock issues). + + .. note:: + **Pose Preservation:** By default, the function preserves the prim's local transform + (relative to its parent). The world-space position of the prim remains unchanged + unless explicit ``translation``, ``orientation``, or ``scale`` values are provided. + + .. warning:: + **Animation Data Loss:** This function only preserves transform values at the default + time code (``Usd.TimeCode.Default()``). Any animation or time-sampled transform data + will be lost. Use this function during asset import or preparation, not on animated prims. + + .. warning:: + **Unit Scale Resolution:** If the prim has a ``xformOp:scale:unitsResolve`` attribute + (common in imported assets with unit mismatches), it will be baked into the scale + and removed. For example, a scale of (1, 1, 1) with unitsResolve of (100, 100, 100) + becomes a final scale of (100, 100, 100). + + Args: + prim: The USD prim to standardize. Must be a valid prim that supports the + UsdGeom.Xformable schema (e.g., Xform, Mesh, Cube, etc.). Material and + Shader prims are not Xformable and will return False. + translation: Optional translation vector (x, y, z) in local space. If provided, + overrides the prim's current translation. If None, preserves the current + local translation. Defaults to None. + orientation: Optional orientation quaternion (w, x, y, z) in local space. If provided, + overrides the prim's current orientation. If None, preserves the current + local orientation. Defaults to None. + scale: Optional scale vector (x, y, z). If provided, overrides the prim's current scale. + If None, preserves the current scale (after unit resolution) or uses (1, 1, 1) + if no scale exists. Defaults to None. + + Returns: + bool: True if the transform operations were successfully standardized. False if the + prim is not Xformable (e.g., Material, Shader prims). The function will log an + error message when returning False. + + Raises: + ValueError: If the prim is not valid (i.e., does not exist or is an invalid prim). + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> # Standardize a prim with non-standard transform operations + >>> prim = stage.GetPrimAtPath("/World/ImportedAsset") + >>> result = sim_utils.standardize_xform_ops(prim) + >>> if result: + ... print("Transform stack standardized successfully") + >>> # The prim now uses: [translate, orient, scale] in that order + >>> + >>> # Standardize and set new transform values + >>> sim_utils.standardize_xform_ops( + ... prim, + ... translation=(1.0, 2.0, 3.0), + ... orientation=(1.0, 0.0, 0.0, 0.0), # identity rotation (w, x, y, z) + ... scale=(2.0, 2.0, 2.0), + ... ) + >>> + >>> # Batch processing for performance + >>> prims_to_standardize = [stage.GetPrimAtPath(p) for p in prim_paths] + >>> for prim in prims_to_standardize: + ... sim_utils.standardize_xform_ops(prim) # Each call uses Sdf.ChangeBlock + """ + # Validate prim + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPath()}' is not valid.") + + # Check if prim is an Xformable + if not prim.IsA(UsdGeom.Xformable): + logger.error( + f"Prim at path '{prim.GetPath().pathString}' is of type '{prim.GetTypeName()}', " + "which is not an Xformable. Transform operations will not be standardized. " + "This is expected for material, shader, and scope prims." + ) + return False + + # Create xformable interface + xformable = UsdGeom.Xformable(prim) + # Get current property names + prop_names = prim.GetPropertyNames() + + # Obtain current local transformations + tf = Gf.Transform(xformable.GetLocalTransformation()) + xform_pos = Gf.Vec3d(tf.GetTranslation()) + xform_quat = Gf.Quatd(tf.GetRotation().GetQuat()) + xform_scale = Gf.Vec3d(tf.GetScale()) + + if translation is not None: + xform_pos = Gf.Vec3d(*translation) + if orientation is not None: + xform_quat = Gf.Quatd(*orientation) + + # Handle scale resolution + if scale is not None: + # User provided scale + xform_scale = Gf.Vec3d(scale) + elif "xformOp:scale" in prop_names: + # Handle unit resolution for scale if present + # This occurs when assets are imported with different unit scales + # Reference: Omniverse Metrics Assembler + if "xformOp:scale:unitsResolve" in prop_names: + units_resolve = prim.GetAttribute("xformOp:scale:unitsResolve").Get() + for i in range(3): + xform_scale[i] = xform_scale[i] * units_resolve[i] + else: + # No scale exists, use default uniform scale + xform_scale = Gf.Vec3d(1.0, 1.0, 1.0) + + # Verify if xform stack is reset + has_reset = xformable.GetResetXformStack() + # Batch the operations + with Sdf.ChangeBlock(): + # Clear the existing transform operation order + for prop_name in prop_names: + if prop_name in _INVALID_XFORM_OPS: + prim.RemoveProperty(prop_name) + + # Remove unitsResolve attribute if present (already handled in scale resolution above) + if "xformOp:scale:unitsResolve" in prop_names: + prim.RemoveProperty("xformOp:scale:unitsResolve") + + # Set up or retrieve scale operation + xform_op_scale = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) + if not xform_op_scale: + xform_op_scale = xformable.AddXformOp(UsdGeom.XformOp.TypeScale, UsdGeom.XformOp.PrecisionDouble, "") + + # Set up or retrieve translate operation + xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) + if not xform_op_translate: + xform_op_translate = xformable.AddXformOp( + UsdGeom.XformOp.TypeTranslate, UsdGeom.XformOp.PrecisionDouble, "" + ) + + # Set up or retrieve orient (quaternion rotation) operation + xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) + if not xform_op_orient: + xform_op_orient = xformable.AddXformOp(UsdGeom.XformOp.TypeOrient, UsdGeom.XformOp.PrecisionDouble, "") + + # Handle different floating point precisions + # Existing Xform operations might have floating or double precision. + # We need to cast the data to the correct type to avoid setting the wrong type. + xform_ops = [xform_op_translate, xform_op_orient, xform_op_scale] + xform_values = [xform_pos, xform_quat, xform_scale] + for xform_op, value in zip(xform_ops, xform_values): + # Get current value to determine precision type + current_value = xform_op.Get() + # Cast to existing type to preserve precision (float/double) + xform_op.Set(type(current_value)(value) if current_value is not None else value) + + # Set the transform operation order: translate -> orient -> scale + # This is the standard USD convention and ensures consistent behavior + xformable.SetXformOpOrder([xform_op_translate, xform_op_orient, xform_op_scale], has_reset) + + return True + + +def validate_standard_xform_ops(prim: Usd.Prim) -> bool: + """Validate if the transform operations on a prim are standardized. + + This function checks if the transform operations on a prim are standardized to the canonical form: + [translate, orient, scale]. + + Args: + prim: The USD prim to validate. + """ + # check if prim is valid + if not prim.IsValid(): + logger.error(f"Prim at path '{prim.GetPath().pathString}' is not valid.") + return False + # check if prim is an xformable + if not prim.IsA(UsdGeom.Xformable): + logger.error(f"Prim at path '{prim.GetPath().pathString}' is not an xformable.") + return False + # get the xformable interface + xformable = UsdGeom.Xformable(prim) + # get the xform operation order + xform_op_order = xformable.GetOrderedXformOps() + xform_op_order = [op.GetOpName() for op in xform_op_order] + # check if the xform operation order is the canonical form + if xform_op_order != ["xformOp:translate", "xformOp:orient", "xformOp:scale"]: + msg = f"Xform operation order for prim at path '{prim.GetPath().pathString}' is not the canonical form." + msg += f" Received order: {xform_op_order}" + msg += " Expected order: ['xformOp:translate', 'xformOp:orient', 'xformOp:scale']" + logger.error(msg) + return False + return True + + +def resolve_prim_pose( + prim: Usd.Prim, ref_prim: Usd.Prim | None = None +) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: + """Resolve the pose of a prim with respect to another prim. + + Note: + This function ignores scale and skew by orthonormalizing the transformation + matrix at the final step. However, if any ancestor prim in the hierarchy + has non-uniform scale, that scale will still affect the resulting position + and orientation of the prim (because it's baked into the transform before + scale removal). + + In other words: scale **is not removed hierarchically**. If you need + completely scale-free poses, you must walk the transform chain and strip + scale at each level. Please open an issue if you need this functionality. + + Args: + prim: The USD prim to resolve the pose for. + ref_prim: The USD prim to compute the pose with respect to. + Defaults to None, in which case the world frame is used. + + Returns: + A tuple containing the position (as a 3D vector) and the quaternion orientation + in the (w, x, y, z) format. + + Raises: + ValueError: If the prim or ref prim is not valid. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import Usd, UsdGeom + >>> + >>> # Get prim + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/ImportedAsset") + >>> + >>> # Resolve pose + >>> pos, quat = sim_utils.resolve_prim_pose(prim) + >>> print(f"Position: {pos}") + >>> print(f"Orientation: {quat}") + >>> + >>> # Resolve pose with respect to another prim + >>> ref_prim = stage.GetPrimAtPath("/World/Reference") + >>> pos, quat = sim_utils.resolve_prim_pose(prim, ref_prim) + >>> print(f"Position: {pos}") + >>> print(f"Orientation: {quat}") + """ + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPath().pathString}' is not valid.") + # get prim xform + xform = UsdGeom.Xformable(prim) + prim_tf = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # sanitize quaternion + # this is needed, otherwise the quaternion might be non-normalized + prim_tf.Orthonormalize() + + if ref_prim is not None: + # if reference prim is the root, we can skip the computation + if ref_prim.GetPath() != Sdf.Path.absoluteRootPath: + # get ref prim xform + ref_xform = UsdGeom.Xformable(ref_prim) + ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # make sure ref tf is orthonormal + ref_tf.Orthonormalize() + # compute relative transform to get prim in ref frame + prim_tf = prim_tf * ref_tf.GetInverse() + + # extract position and orientation + prim_pos = [*prim_tf.ExtractTranslation()] + prim_quat = [prim_tf.ExtractRotationQuat().real, *prim_tf.ExtractRotationQuat().imaginary] + return tuple(prim_pos), tuple(prim_quat) + + +def resolve_prim_scale(prim: Usd.Prim) -> tuple[float, float, float]: + """Resolve the scale of a prim in the world frame. + + At an attribute level, a USD prim's scale is a scaling transformation applied to the prim with + respect to its parent prim. This function resolves the scale of the prim in the world frame, + by computing the local to world transform of the prim. This is equivalent to traversing up + the prim hierarchy and accounting for the rotations and scales of the prims. + + For instance, if a prim has a scale of (1, 2, 3) and it is a child of a prim with a scale of (4, 5, 6), + then the scale of the prim in the world frame is (4, 10, 18). + + Args: + prim: The USD prim to resolve the scale for. + + Returns: + The scale of the prim in the x, y, and z directions in the world frame. + + Raises: + ValueError: If the prim is not valid. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import Usd, UsdGeom + >>> + >>> # Get prim + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/ImportedAsset") + >>> + >>> # Resolve scale + >>> scale = sim_utils.resolve_prim_scale(prim) + >>> print(f"Scale: {scale}") + """ + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPath().pathString}' is not valid.") + # compute local to world transform + xform = UsdGeom.Xformable(prim) + world_transform = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # extract scale + return tuple([*(v.GetLength() for v in world_transform.ExtractRotationMatrix())]) + + +def convert_world_pose_to_local( + position: tuple[float, ...], + orientation: tuple[float, ...] | None, + ref_prim: Usd.Prim, +) -> tuple[tuple[float, float, float], tuple[float, float, float, float] | None]: + """Convert a world-space pose to local-space pose relative to a reference prim. + + This function takes a position and orientation in world space and converts them to local space + relative to the given reference prim. This is useful when creating or positioning prims where you + know the desired world position but need to set local transform attributes relative to another prim. + + The conversion uses the standard USD transformation math: + ``local_transform = world_transform * inverse(ref_world_transform)`` + + .. note:: + If the reference prim is the root prim ("/"), the position and orientation are returned + unchanged, as they are already effectively in local/world space. + + Args: + position: The world-space position as (x, y, z). + orientation: The world-space orientation as quaternion (w, x, y, z). If None, only position is converted + and None is returned for orientation. + ref_prim: The reference USD prim to compute the local transform relative to. If this is + the root prim ("/"), the world pose is returned unchanged. + + Returns: + A tuple of (local_translation, local_orientation) where: + + - local_translation is a tuple of (x, y, z) in local space relative to ref_prim + - local_orientation is a tuple of (w, x, y, z) in local space relative to ref_prim, + or None if no orientation was provided + + Raises: + ValueError: If the reference prim is not a valid USD prim. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import Usd, UsdGeom + >>> + >>> # Get reference prim + >>> stage = sim_utils.get_current_stage() + >>> ref_prim = stage.GetPrimAtPath("/World/Reference") + >>> + >>> # Convert world pose to local (relative to ref_prim) + >>> world_pos = (10.0, 5.0, 0.0) + >>> world_quat = (1.0, 0.0, 0.0, 0.0) # identity rotation + >>> local_pos, local_quat = sim_utils.convert_world_pose_to_local(world_pos, world_quat, ref_prim) + >>> print(f"Local position: {local_pos}") + >>> print(f"Local orientation: {local_quat}") + """ + # Check if prim is valid + if not ref_prim.IsValid(): + raise ValueError(f"Reference prim at path '{ref_prim.GetPath().pathString}' is not valid.") + + # If reference prim is the root, return world pose as-is + if ref_prim.GetPath() == Sdf.Path.absoluteRootPath: + return position, orientation # type: ignore + + # Check if reference prim is a valid xformable + ref_xformable = UsdGeom.Xformable(ref_prim) + # Get reference prim's world transform + ref_world_tf = ref_xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + + # Create world transform for the desired position and orientation + desired_world_tf = Gf.Matrix4d() + desired_world_tf.SetTranslateOnly(Gf.Vec3d(*position)) + + if orientation is not None: + # Set rotation from quaternion (w, x, y, z) + quat = Gf.Quatd(*orientation) + desired_world_tf.SetRotateOnly(quat) + + # Convert world transform to local: local = world * inv(ref_world) + ref_world_tf_inv = ref_world_tf.GetInverse() + local_tf = desired_world_tf * ref_world_tf_inv + + # Extract local translation and orientation + local_transform = Gf.Transform(local_tf) + local_translation = tuple(local_transform.GetTranslation()) + + local_orientation = None + if orientation is not None: + quat_result = local_transform.GetRotation().GetQuat() + local_orientation = (quat_result.GetReal(), *quat_result.GetImaginary()) + + return local_translation, local_orientation diff --git a/source/isaaclab/isaaclab/sim/views/__init__.py b/source/isaaclab/isaaclab/sim/views/__init__.py new file mode 100644 index 000000000000..eb5bea7690cb --- /dev/null +++ b/source/isaaclab/isaaclab/sim/views/__init__.py @@ -0,0 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Views for manipulating USD prims.""" + +from .xform_prim_view import XformPrimView diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py new file mode 100644 index 000000000000..38a786117bcf --- /dev/null +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -0,0 +1,1114 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence + +import numpy as np +import torch +import warp as wp + +import carb +from pxr import Gf, Sdf, Usd, UsdGeom, Vt + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.utils.warp import fabric as fabric_utils + +logger = logging.getLogger(__name__) + + +class XformPrimView: + """Optimized batched interface for reading and writing transforms of multiple USD prims. + + This class provides efficient batch operations for getting and setting poses (position and orientation) + of multiple prims at once using torch tensors. It is designed for scenarios where you need to manipulate + many prims simultaneously, such as in multi-agent simulations or large-scale procedural generation. + + The class supports both world-space and local-space pose operations: + + - **World poses**: Positions and orientations in the global world frame + - **Local poses**: Positions and orientations relative to each prim's parent + + When Fabric is enabled, the class leverages NVIDIA's Fabric API for GPU-accelerated batch operations: + + - Uses `omni:fabric:worldMatrix` and `omni:fabric:localMatrix` attributes for all Boundable prims + - Performs batch matrix decomposition/composition using Warp kernels on GPU + - Achieves performance comparable to Isaac Sim's XFormPrim implementation + - Works for both physics-enabled and non-physics prims (cameras, meshes, etc.). + Note: renderers typically consume USD-authored camera transforms. + + .. warning:: + **Fabric requires CUDA**: Fabric is only supported with on CUDA devices. + Warp's CPU backend for fabric-array writes has known issues, so attempting to use + Fabric with CPU device (``device="cpu"``) will raise a ValueError at initialization. + + .. note:: + **Fabric Support:** + + When Fabric is enabled, this view ensures prims have the required Fabric hierarchy + attributes (``omni:fabric:localMatrix`` and ``omni:fabric:worldMatrix``). On first Fabric + read, USD-authored transforms initialize Fabric state. Fabric writes can optionally + be mirrored back to USD via :attr:`sync_usd_on_fabric_write`. + + For more information, see the `Fabric Hierarchy documentation`_. + + .. _Fabric Hierarchy documentation: https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/fabric_hierarchy.html + + .. note:: + **Performance Considerations:** + + * Tensor operations are performed on the specified device (CPU/CUDA) + * USD write operations use ``Sdf.ChangeBlock`` for batched updates + * Fabric operations use GPU-accelerated Warp kernels for maximum performance + * For maximum performance, minimize get/set operations within tight loops + + .. note:: + **Transform Requirements:** + + All prims in the view must be Xformable and have standardized transform operations: + ``[translate, orient, scale]``. Non-standard prims will raise a ValueError during + initialization if :attr:`validate_xform_ops` is True. Please use the function + :func:`isaaclab.sim.utils.standardize_xform_ops` to prepare prims before using this view. + + .. warning:: + This class operates at the USD default time code. Any animation or time-sampled data + will not be affected by write operations. For animated transforms, you need to handle + time-sampled keyframes separately. + """ + + def __init__( + self, + prim_path: str, + device: str = "cpu", + validate_xform_ops: bool = True, + sync_usd_on_fabric_write: bool = False, + stage: Usd.Stage | None = None, + ): + """Initialize the view with matching prims. + + This method searches the USD stage for all prims matching the provided path pattern, + validates that they are Xformable with standard transform operations, and stores + references for efficient batch operations. + + We generally recommend to validate the xform operations, as it ensures that the prims are in a consistent state + and have the standard transform operations (translate, orient, scale in that order). + However, if you are sure that the prims are in a consistent state, you can set this to False to improve + performance. This can save around 45-50% of the time taken to initialize the view. + + Args: + prim_path: USD prim path pattern to match prims. Supports wildcards (``*``) and + regex patterns (e.g., ``"/World/Env_.*/Robot"``). See + :func:`isaaclab.sim.utils.find_matching_prims` for pattern syntax. + device: Device to place the tensors on. Can be ``"cpu"`` or CUDA devices like + ``"cuda:0"``. Defaults to ``"cpu"``. + validate_xform_ops: Whether to validate that the prims have standard xform operations. + Defaults to True. + sync_usd_on_fabric_write: Whether to mirror Fabric transform writes back to USD. + When True, transform updates are synchronized to USD so that USD data readers (e.g., rendering + cameras) can observe these changes. Defaults to False for better performance. + stage: USD stage to search for prims. Defaults to None, in which case the current active stage + from the simulation context is used. + + Raises: + ValueError: If any matched prim is not Xformable or doesn't have standardized + transform operations (translate, orient, scale in that order). + """ + # Store configuration + self._prim_path = prim_path + self._device = device + + # Find and validate matching prims + stage = sim_utils.get_current_stage() if stage is None else stage + self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) + + # Validate all prims have standard xform operations + if validate_xform_ops: + for prim in self._prims: + if not sim_utils.validate_standard_xform_ops(prim): + raise ValueError( + f"Prim at path '{prim.GetPath().pathString}' is not a xformable prim with standard transform" + f" operations [translate, orient, scale]. Received type: '{prim.GetTypeName()}'." + " Use sim_utils.standardize_xform_ops() to prepare the prim." + ) + + # Determine if Fabric is supported on the device + self._use_fabric = carb.settings.get_settings().get("/physics/fabricEnabled") + logger.debug(f"Using Fabric for the XFormPrimView over '{self._prim_path}' on device '{self._device}'.") + + # Check for unsupported Fabric + CPU combination + if self._use_fabric and self._device == "cpu": + logger.warning( + "Fabric mode with Warp fabric-array operations is not supported on CPU devices. " + "While Fabric itself can run on both CPU and GPU, our batch Warp kernels for " + "fabric-array operations require CUDA and are not reliable on the CPU backend. " + "To ensure stability, Fabric is being disabled and execution will fall back " + "to standard USD operations on the CPU. This may impact performance." + ) + self._use_fabric = False + + # Create indices buffer + # Since we iterate over the indices, we need to use range instead of torch tensor + self._ALL_INDICES = list(range(len(self._prims))) + + # Some prims (e.g., Cameras) require USD-authored transforms for rendering. + # When enabled, mirror Fabric pose writes to USD for those prims. + self._sync_usd_on_fabric_write = sync_usd_on_fabric_write + + # Fabric batch infrastructure (initialized lazily on first use) + self._fabric_initialized = False + self._fabric_usd_sync_done = False + self._fabric_selection = None + self._fabric_to_view: wp.array | None = None + self._view_to_fabric: wp.array | None = None + self._default_view_indices: wp.array | None = None + self._fabric_hierarchy = None + # Create a valid USD attribute name: namespace:name + # Use "isaaclab" namespace to identify our custom attributes + self._view_index_attr = f"isaaclab:view_index:{abs(hash(self))}" + + """ + Properties. + """ + + @property + def count(self) -> int: + """Number of prims in this view.""" + return len(self._prims) + + @property + def device(self) -> str: + """Device where tensors are allocated (cpu or cuda).""" + return self._device + + @property + def prims(self) -> list[Usd.Prim]: + """List of USD prims being managed by this view.""" + return self._prims + + @property + def prim_paths(self) -> list[str]: + """List of prim paths (as strings) for all prims being managed by this view. + + This property converts each prim to its path string representation. The conversion is + performed lazily on first access and cached for subsequent accesses. + + Note: + For most use cases, prefer using :attr:`prims` directly as it provides direct access + to the USD prim objects without the conversion overhead. This property is mainly useful + for logging, debugging, or when string paths are explicitly required. + """ + # we cache it the first time it is accessed. + # we don't compute it in constructor because it is expensive and we don't need it most of the time. + # users should usually deal with prims directly as they typically need to access the prims directly. + if not hasattr(self, "_prim_paths"): + self._prim_paths = [prim.GetPath().pathString for prim in self._prims] + return self._prim_paths + + """ + Operations - Setters. + """ + + def set_world_poses( + self, + positions: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: Sequence[int] | None = None, + ): + """Set world-space poses for prims in the view. + + This method sets the position and/or orientation of each prim in world space. + + - When Fabric is enabled, the function writes directly to Fabric's ``omni:fabric:worldMatrix`` + attribute using GPU-accelerated batch operations. + - When Fabric is disabled, the function converts to local space and writes to USD's ``xformOp:translate`` + and ``xformOp:orient`` attributes. + + Args: + positions: World-space positions as a tensor of shape (M, 3) where M is the number of prims + to set (either all prims if indices is None, or the number of indices provided). + Defaults to None, in which case positions are not modified. + orientations: World-space orientations as quaternions (w, x, y, z) with shape (M, 4). + Defaults to None, in which case orientations are not modified. + indices: Indices of prims to set poses for. Defaults to None, in which case poses are set + for all prims in the view. + + Raises: + ValueError: If positions shape is not (M, 3) or orientations shape is not (M, 4). + ValueError: If the number of poses doesn't match the number of indices provided. + """ + if self._use_fabric: + self._set_world_poses_fabric(positions, orientations, indices) + else: + self._set_world_poses_usd(positions, orientations, indices) + + def set_local_poses( + self, + translations: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: Sequence[int] | None = None, + ): + """Set local-space poses for prims in the view. + + This method sets the position and/or orientation of each prim in local space (relative to + their parent prims). + + The function writes directly to USD's ``xformOp:translate`` and ``xformOp:orient`` attributes. + + Note: + Even in Fabric mode, local pose operations use USD. This behavior is based on Isaac Sim's design + where Fabric is only used for world pose operations. + + Rationale: + - Local pose writes need correct parent-child hierarchy relationships + - USD maintains these relationships correctly and efficiently + - Fabric is optimized for world pose operations, not local hierarchies + + Args: + translations: Local-space translations as a tensor of shape (M, 3) where M is the number of prims + to set (either all prims if indices is None, or the number of indices provided). + Defaults to None, in which case translations are not modified. + orientations: Local-space orientations as quaternions (w, x, y, z) with shape (M, 4). + Defaults to None, in which case orientations are not modified. + indices: Indices of prims to set poses for. Defaults to None, in which case poses are set + for all prims in the view. + + Raises: + ValueError: If translations shape is not (M, 3) or orientations shape is not (M, 4). + ValueError: If the number of poses doesn't match the number of indices provided. + """ + if self._use_fabric: + self._set_local_poses_fabric(translations, orientations, indices) + else: + self._set_local_poses_usd(translations, orientations, indices) + + def set_scales(self, scales: torch.Tensor, indices: Sequence[int] | None = None): + """Set scales for prims in the view. + + This method sets the scale of each prim in the view. + + - When Fabric is enabled, the function updates scales in Fabric matrices using GPU-accelerated batch operations. + - When Fabric is disabled, the function writes to USD's ``xformOp:scale`` attributes. + + Args: + scales: Scales as a tensor of shape (M, 3) where M is the number of prims + to set (either all prims if indices is None, or the number of indices provided). + indices: Indices of prims to set scales for. Defaults to None, in which case scales are set + for all prims in the view. + + Raises: + ValueError: If scales shape is not (M, 3). + """ + if self._use_fabric: + self._set_scales_fabric(scales, indices) + else: + self._set_scales_usd(scales, indices) + + def set_visibility(self, visibility: torch.Tensor, indices: Sequence[int] | None = None): + """Set visibility for prims in the view. + + This method sets the visibility of each prim in the view. + + Args: + visibility: Visibility as a boolean tensor of shape (M,) where M is the + number of prims to set (either all prims if indices is None, or the number of indices provided). + indices: Indices of prims to set visibility for. Defaults to None, in which case visibility is set + for all prims in the view. + + Raises: + ValueError: If visibility shape is not (M,). + """ + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Validate inputs + if visibility.shape != (len(indices_list),): + raise ValueError(f"Expected visibility shape ({len(indices_list)},), got {visibility.shape}.") + + # Set visibility for each prim + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + # Convert prim to imageable + imageable = UsdGeom.Imageable(self._prims[prim_idx]) + # Set visibility + if visibility[idx]: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + """ + Operations - Getters. + """ + + def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get world-space poses for prims in the view. + + This method retrieves the position and orientation of each prim in world space by computing + the full transform hierarchy from the prim to the world root. + + - When Fabric is enabled, the function uses Fabric batch operations with Warp kernels. + - When Fabric is disabled, the function uses USD XformCache. + + Note: + Scale and skew are ignored. The returned poses contain only translation and rotation. + + Args: + indices: Indices of prims to get poses for. Defaults to None, in which case poses are retrieved + for all prims in the view. + + Returns: + A tuple of (positions, orientations) where: + + - positions: Torch tensor of shape (M, 3) containing world-space positions (x, y, z), + where M is the number of prims queried. + - orientations: Torch tensor of shape (M, 4) containing world-space quaternions (w, x, y, z) + """ + if self._use_fabric: + return self._get_world_poses_fabric(indices) + else: + return self._get_world_poses_usd(indices) + + def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get local-space poses for prims in the view. + + This method retrieves the position and orientation of each prim in local space (relative to + their parent prims). It reads directly from USD's ``xformOp:translate`` and ``xformOp:orient`` attributes. + + Note: + Even in Fabric mode, local pose operations use USD. This behavior is based on Isaac Sim's design + where Fabric is only used for world pose operations. + + Rationale: + - Local pose reads need correct parent-child hierarchy relationships + - USD maintains these relationships correctly and efficiently + - Fabric is optimized for world pose operations, not local hierarchies + + Note: + Scale is ignored. The returned poses contain only translation and rotation. + + Args: + indices: Indices of prims to get poses for. Defaults to None, in which case poses are retrieved + for all prims in the view. + + Returns: + A tuple of (translations, orientations) where: + + - translations: Torch tensor of shape (M, 3) containing local-space translations (x, y, z), + where M is the number of prims queried. + - orientations: Torch tensor of shape (M, 4) containing local-space quaternions (w, x, y, z) + """ + if self._use_fabric: + return self._get_local_poses_fabric(indices) + else: + return self._get_local_poses_usd(indices) + + def get_scales(self, indices: Sequence[int] | None = None) -> torch.Tensor: + """Get scales for prims in the view. + + This method retrieves the scale of each prim in the view. + + - When Fabric is enabled, the function extracts scales from Fabric matrices using batch operations with + Warp kernels. + - When Fabric is disabled, the function reads from USD's ``xformOp:scale`` attributes. + + Args: + indices: Indices of prims to get scales for. Defaults to None, in which case scales are retrieved + for all prims in the view. + + Returns: + A tensor of shape (M, 3) containing the scales of each prim, where M is the number of prims queried. + """ + if self._use_fabric: + return self._get_scales_fabric(indices) + else: + return self._get_scales_usd(indices) + + def get_visibility(self, indices: Sequence[int] | None = None) -> torch.Tensor: + """Get visibility for prims in the view. + + This method retrieves the visibility of each prim in the view. + + Args: + indices: Indices of prims to get visibility for. Defaults to None, in which case visibility is retrieved + for all prims in the view. + + Returns: + A tensor of shape (M,) containing the visibility of each prim, where M is the number of prims queried. + The tensor is of type bool. + """ + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Create buffers + visibility = torch.zeros(len(indices_list), dtype=torch.bool, device=self._device) + + for idx, prim_idx in enumerate(indices_list): + # Get prim + imageable = UsdGeom.Imageable(self._prims[prim_idx]) + # Get visibility + visibility[idx] = imageable.ComputeVisibility() != UsdGeom.Tokens.invisible + + return visibility + + """ + Internal Functions - USD. + """ + + def _set_world_poses_usd( + self, + positions: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: Sequence[int] | None = None, + ): + """Set world poses to USD.""" + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Validate inputs + if positions is not None: + if positions.shape != (len(indices_list), 3): + raise ValueError( + f"Expected positions shape ({len(indices_list)}, 3), got {positions.shape}. " + "Number of positions must match the number of prims in the view." + ) + positions_array = Vt.Vec3dArray.FromNumpy(positions.cpu().numpy()) + else: + positions_array = None + if orientations is not None: + if orientations.shape != (len(indices_list), 4): + raise ValueError( + f"Expected orientations shape ({len(indices_list)}, 4), got {orientations.shape}. " + "Number of orientations must match the number of prims in the view." + ) + # Vt expects quaternions in xyzw order + orientations_array = Vt.QuatdArray.FromNumpy(math_utils.convert_quat(orientations, to="xyzw").cpu().numpy()) + else: + orientations_array = None + + # Create xform cache instance + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + # Set poses for each prim + # We use Sdf.ChangeBlock to minimize notification overhead. + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + # Get prim + prim = self._prims[prim_idx] + # Get parent prim for local space conversion + parent_prim = prim.GetParent() + + # Determine what to set + world_pos = positions_array[idx] if positions_array is not None else None + world_quat = orientations_array[idx] if orientations_array is not None else None + + # Convert world pose to local if we have a valid parent + # Note: We don't use :func:`isaaclab.sim.utils.transforms.convert_world_pose_to_local` + # here since it isn't optimized for batch operations. + if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: + # Get current world pose if we're only setting one component + if positions_array is None or orientations_array is None: + # get prim xform + prim_tf = xform_cache.GetLocalToWorldTransform(prim) + # sanitize quaternion + # this is needed, otherwise the quaternion might be non-normalized + prim_tf.Orthonormalize() + # populate desired world transform + if world_pos is not None: + prim_tf.SetTranslateOnly(world_pos) + if world_quat is not None: + prim_tf.SetRotateOnly(world_quat) + else: + # Both position and orientation are provided, create new transform + prim_tf = Gf.Matrix4d() + prim_tf.SetTranslateOnly(world_pos) + prim_tf.SetRotateOnly(world_quat) + + # Convert to local space + parent_world_tf = xform_cache.GetLocalToWorldTransform(parent_prim) + local_tf = prim_tf * parent_world_tf.GetInverse() + local_pos = local_tf.ExtractTranslation() + local_quat = local_tf.ExtractRotationQuat() + else: + # No parent or parent is root, world == local + local_pos = world_pos + local_quat = world_quat + + # Get or create the standard transform operations + if local_pos is not None: + prim.GetAttribute("xformOp:translate").Set(local_pos) + if local_quat is not None: + prim.GetAttribute("xformOp:orient").Set(local_quat) + + def _set_local_poses_usd( + self, + translations: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: Sequence[int] | None = None, + ): + """Set local poses to USD.""" + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Validate inputs + if translations is not None: + if translations.shape != (len(indices_list), 3): + raise ValueError(f"Expected translations shape ({len(indices_list)}, 3), got {translations.shape}.") + translations_array = Vt.Vec3dArray.FromNumpy(translations.cpu().numpy()) + else: + translations_array = None + if orientations is not None: + if orientations.shape != (len(indices_list), 4): + raise ValueError(f"Expected orientations shape ({len(indices_list)}, 4), got {orientations.shape}.") + orientations_array = Vt.QuatdArray.FromNumpy(math_utils.convert_quat(orientations, to="xyzw").cpu().numpy()) + else: + orientations_array = None + + # Set local poses + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + if translations_array is not None: + prim.GetAttribute("xformOp:translate").Set(translations_array[idx]) + if orientations_array is not None: + prim.GetAttribute("xformOp:orient").Set(orientations_array[idx]) + + def _set_scales_usd(self, scales: torch.Tensor, indices: Sequence[int] | None = None): + """Set scales to USD.""" + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Validate inputs + if scales.shape != (len(indices_list), 3): + raise ValueError(f"Expected scales shape ({len(indices_list)}, 3), got {scales.shape}.") + + scales_array = Vt.Vec3dArray.FromNumpy(scales.cpu().numpy()) + # Set scales for each prim + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + prim.GetAttribute("xformOp:scale").Set(scales_array[idx]) + + def _get_world_poses_usd(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get world poses from USD.""" + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Create buffers + positions = Vt.Vec3dArray(len(indices_list)) + orientations = Vt.QuatdArray(len(indices_list)) + # Create xform cache instance + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + # Note: We don't use :func:`isaaclab.sim.utils.transforms.resolve_prim_pose` + # here since it isn't optimized for batch operations. + for idx, prim_idx in enumerate(indices_list): + # Get prim + prim = self._prims[prim_idx] + # get prim xform + prim_tf = xform_cache.GetLocalToWorldTransform(prim) + # sanitize quaternion + # this is needed, otherwise the quaternion might be non-normalized + prim_tf.Orthonormalize() + # extract position and orientation + positions[idx] = prim_tf.ExtractTranslation() + orientations[idx] = prim_tf.ExtractRotationQuat() + + # move to torch tensors + positions = torch.tensor(np.array(positions), dtype=torch.float32, device=self._device) + orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) + # underlying data is in xyzw order, convert to wxyz order + orientations = math_utils.convert_quat(orientations, to="wxyz") + + return positions, orientations # type: ignore + + def _get_local_poses_usd(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get local poses from USD.""" + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Create buffers + translations = Vt.Vec3dArray(len(indices_list)) + orientations = Vt.QuatdArray(len(indices_list)) + + # Create a fresh XformCache to avoid stale cached values + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + prim_tf = xform_cache.GetLocalTransformation(prim)[0] + prim_tf.Orthonormalize() + translations[idx] = prim_tf.ExtractTranslation() + orientations[idx] = prim_tf.ExtractRotationQuat() + + translations = torch.tensor(np.array(translations), dtype=torch.float32, device=self._device) + orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) + orientations = math_utils.convert_quat(orientations, to="wxyz") + + return translations, orientations # type: ignore + + def _get_scales_usd(self, indices: Sequence[int] | None = None) -> torch.Tensor: + """Get scales from USD.""" + # Resolve indices + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + + # Create buffers + scales = Vt.Vec3dArray(len(indices_list)) + + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + scales[idx] = prim.GetAttribute("xformOp:scale").Get() + + # Convert to tensor + return torch.tensor(np.array(scales), dtype=torch.float32, device=self._device) + + """ + Internal Functions - Fabric. + """ + + def _set_world_poses_fabric( + self, + positions: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: Sequence[int] | None = None, + ): + """Set world poses using Fabric GPU batch operations. + + Writes directly to Fabric's ``omni:fabric:worldMatrix`` attribute using Warp kernels. + Changes are propagated through Fabric's hierarchy system but remain GPU-resident. + + For workflows mixing Fabric world pose writes with USD local pose queries, note + that local poses read from USD's xformOp:* attributes, which may not immediately + reflect Fabric changes. For best performance and consistency, use Fabric methods + exclusively (get_world_poses/set_world_poses with Fabric enabled). + """ + # Lazy initialization + if not self._fabric_initialized: + self._initialize_fabric() + + # Resolve indices (treat slice(None) as None for consistency with USD path) + indices_wp = self._resolve_indices_wp(indices) + + count = indices_wp.shape[0] + + # Convert torch to warp (if provided), use dummy arrays for None to avoid Warp kernel issues + if positions is not None: + positions_wp = wp.from_torch(positions) + else: + positions_wp = wp.zeros((0, 3), dtype=wp.float32).to(self._device) + + if orientations is not None: + orientations_wp = wp.from_torch(orientations) + else: + orientations_wp = wp.zeros((0, 4), dtype=wp.float32).to(self._device) + + # Dummy array for scales (not modifying) + scales_wp = wp.zeros((0, 3), dtype=wp.float32).to(self._device) + + # Use cached fabricarray for world matrices + world_matrices = self._fabric_world_matrices + + # Batch compose matrices with a single kernel launch + wp.launch( + kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, + dim=count, + inputs=[ + world_matrices, + positions_wp, + orientations_wp, + scales_wp, # dummy array instead of None + False, # broadcast_positions + False, # broadcast_orientations + False, # broadcast_scales + indices_wp, + self._view_to_fabric, + ], + device=self._device, + ) + + # Synchronize to ensure kernel completes + wp.synchronize() + + # Update world transforms within Fabric hierarchy + self._fabric_hierarchy.update_world_xforms() + # Fabric now has authoritative data; skip future USD syncs + self._fabric_usd_sync_done = True + # Mirror to USD for renderer-facing prims when enabled. + if self._sync_usd_on_fabric_write: + self._set_world_poses_usd(positions, orientations, indices) + + # Fabric writes are GPU-resident; local pose operations still use USD. + + def _set_local_poses_fabric( + self, + translations: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: Sequence[int] | None = None, + ): + """Set local poses using USD (matches Isaac Sim's design). + + Note: Even in Fabric mode, local pose operations use USD. + This is Isaac Sim's design: the ``usd=False`` parameter only affects world poses. + + Rationale: + - Local pose writes need correct parent-child hierarchy relationships + - USD maintains these relationships correctly and efficiently + - Fabric is optimized for world pose operations, not local hierarchies + """ + self._set_local_poses_usd(translations, orientations, indices) + + def _set_scales_fabric(self, scales: torch.Tensor, indices: Sequence[int] | None = None): + """Set scales using Fabric GPU batch operations.""" + # Lazy initialization + if not self._fabric_initialized: + self._initialize_fabric() + + # Resolve indices (treat slice(None) as None for consistency with USD path) + indices_wp = self._resolve_indices_wp(indices) + + count = indices_wp.shape[0] + + # Convert torch to warp + scales_wp = wp.from_torch(scales) + + # Dummy arrays for positions and orientations (not modifying) + positions_wp = wp.zeros((0, 3), dtype=wp.float32).to(self._device) + orientations_wp = wp.zeros((0, 4), dtype=wp.float32).to(self._device) + + # Use cached fabricarray for world matrices + world_matrices = self._fabric_world_matrices + + # Batch compose matrices on GPU with a single kernel launch + wp.launch( + kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, + dim=count, + inputs=[ + world_matrices, + positions_wp, # dummy array instead of None + orientations_wp, # dummy array instead of None + scales_wp, + False, # broadcast_positions + False, # broadcast_orientations + False, # broadcast_scales + indices_wp, + self._view_to_fabric, + ], + device=self._device, + ) + + # Synchronize to ensure kernel completes before syncing + wp.synchronize() + + # Update world transforms to propagate changes + self._fabric_hierarchy.update_world_xforms() + # Fabric now has authoritative data; skip future USD syncs + self._fabric_usd_sync_done = True + # Mirror to USD for renderer-facing prims when enabled. + if self._sync_usd_on_fabric_write: + self._set_scales_usd(scales, indices) + + def _get_world_poses_fabric(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get world poses from Fabric using GPU batch operations.""" + # Lazy initialization of Fabric infrastructure + if not self._fabric_initialized: + self._initialize_fabric() + # Sync once from USD to ensure reads see the latest authored transforms + if not self._fabric_usd_sync_done: + self._sync_fabric_from_usd_once() + + # Resolve indices (treat slice(None) as None for consistency with USD path) + indices_wp = self._resolve_indices_wp(indices) + + count = indices_wp.shape[0] + + # Use pre-allocated buffers for full reads, allocate only for partial reads + use_cached_buffers = indices is None or indices == slice(None) + if use_cached_buffers: + # Full read: Use cached buffers (zero allocation overhead!) + positions_wp = self._fabric_positions_buffer + orientations_wp = self._fabric_orientations_buffer + scales_wp = self._fabric_dummy_buffer + else: + # Partial read: Need to allocate buffers of appropriate size + positions_wp = wp.zeros((count, 3), dtype=wp.float32).to(self._device) + orientations_wp = wp.zeros((count, 4), dtype=wp.float32).to(self._device) + scales_wp = self._fabric_dummy_buffer # Always use dummy for scales + + # Use cached fabricarray for world matrices + # This eliminates the 0.06-0.30ms variability from creating fabricarray each call + world_matrices = self._fabric_world_matrices + + # Launch GPU kernel to decompose matrices in parallel + wp.launch( + kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, + dim=count, + inputs=[ + world_matrices, + positions_wp, + orientations_wp, + scales_wp, # dummy array instead of None + indices_wp, + self._view_to_fabric, + ], + device=self._device, + ) + + # Return tensors: zero-copy for cached buffers, conversion for partial reads + if use_cached_buffers: + # Zero-copy! The Warp kernel wrote directly into the PyTorch tensors + # We just need to synchronize to ensure the kernel is done + wp.synchronize() + return self._fabric_positions_torch, self._fabric_orientations_torch + else: + # Partial read: Need to convert from Warp to torch + positions = wp.to_torch(positions_wp) + orientations = wp.to_torch(orientations_wp) + return positions, orientations + + def _get_local_poses_fabric(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get local poses using USD (matches Isaac Sim's design). + + Note: + Even in Fabric mode, local pose operations use USD's XformCache. + This is Isaac Sim's design: the ``usd=False`` parameter only affects world poses. + + Rationale: + - Local pose computation requires parent transforms which may not be in the view + - USD's XformCache provides efficient hierarchy-aware local transform queries + - Fabric is optimized for world pose operations, not local hierarchies + """ + return self._get_local_poses_usd(indices) + + def _get_scales_fabric(self, indices: Sequence[int] | None = None) -> torch.Tensor: + """Get scales from Fabric using GPU batch operations.""" + # Lazy initialization + if not self._fabric_initialized: + self._initialize_fabric() + # Sync once from USD to ensure reads see the latest authored transforms + if not self._fabric_usd_sync_done: + self._sync_fabric_from_usd_once() + + # Resolve indices (treat slice(None) as None for consistency with USD path) + indices_wp = self._resolve_indices_wp(indices) + + count = indices_wp.shape[0] + + # Use pre-allocated buffers for full reads, allocate only for partial reads + use_cached_buffers = indices is None or indices == slice(None) + if use_cached_buffers: + # Full read: Use cached buffers (zero allocation overhead!) + scales_wp = self._fabric_scales_buffer + else: + # Partial read: Need to allocate buffer of appropriate size + scales_wp = wp.zeros((count, 3), dtype=wp.float32).to(self._device) + + # Always use dummy buffers for positions and orientations (not needed for scales) + positions_wp = self._fabric_dummy_buffer + orientations_wp = self._fabric_dummy_buffer + + # Use cached fabricarray for world matrices + world_matrices = self._fabric_world_matrices + + # Launch GPU kernel to decompose matrices in parallel + wp.launch( + kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, + dim=count, + inputs=[ + world_matrices, + positions_wp, # dummy array instead of None + orientations_wp, # dummy array instead of None + scales_wp, + indices_wp, + self._view_to_fabric, + ], + device=self._device, + ) + + # Return tensor: zero-copy for cached buffers, conversion for partial reads + if use_cached_buffers: + # Zero-copy! The Warp kernel wrote directly into the PyTorch tensor + wp.synchronize() + return self._fabric_scales_torch + else: + # Partial read: Need to convert from Warp to torch + return wp.to_torch(scales_wp) + + """ + Internal Functions - Initialization. + """ + + def _initialize_fabric(self) -> None: + """Initialize Fabric batch infrastructure for GPU-accelerated pose queries. + + This method ensures all prims have the required Fabric hierarchy attributes + (``omni:fabric:localMatrix`` and ``omni:fabric:worldMatrix``) and creates the necessary + infrastructure for batch GPU operations using Warp. + + Based on the Fabric Hierarchy documentation, when Fabric Scene Delegate is enabled, + all boundable prims should have these attributes. This method ensures they exist + and are properly synchronized with USD. + """ + import usdrt + from usdrt import Rt + + # Get USDRT (Fabric) stage + stage_id = sim_utils.get_current_stage_id() + fabric_stage = usdrt.Usd.Stage.Attach(stage_id) + + # Step 1: Ensure all prims have Fabric hierarchy attributes + # According to the documentation, these attributes are created automatically + # when Fabric Scene Delegate is enabled, but we ensure they exist + for i in range(self.count): + rt_prim = fabric_stage.GetPrimAtPath(self.prim_paths[i]) + rt_xformable = Rt.Xformable(rt_prim) + + # Create Fabric hierarchy world matrix attribute if it doesn't exist + has_attr = ( + rt_xformable.HasFabricHierarchyWorldMatrixAttr() + if hasattr(rt_xformable, "HasFabricHierarchyWorldMatrixAttr") + else False + ) + if not has_attr: + rt_xformable.CreateFabricHierarchyWorldMatrixAttr() + + # Best-effort USD->Fabric sync; authoritative initialization happens on first read. + rt_xformable.SetWorldXformFromUsd() + + # Create view index attribute for batch operations + rt_prim.CreateAttribute(self._view_index_attr, usdrt.Sdf.ValueTypeNames.UInt, custom=True) + rt_prim.GetAttribute(self._view_index_attr).Set(i) + + # After syncing all prims, update the Fabric hierarchy to ensure world matrices are computed + self._fabric_hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( + fabric_stage.GetFabricId(), fabric_stage.GetStageIdAsStageId() + ) + self._fabric_hierarchy.update_world_xforms() + + # Step 2: Create index arrays for batch operations + self._default_view_indices = wp.zeros((self.count,), dtype=wp.uint32).to(self._device) + wp.launch( + kernel=fabric_utils.arange_k, + dim=self.count, + inputs=[self._default_view_indices], + device=self._device, + ) + wp.synchronize() # Ensure indices are ready + + # Step 3: Create Fabric selection with attribute filtering + # SelectPrims expects device format like "cuda:0" not "cuda" + # + # KNOWN ISSUE: SelectPrims may return prims in a different order than self._prims + # (which comes from USD's find_matching_prims). We create a bidirectional mapping + # (_view_to_fabric and _fabric_to_view) to handle this ordering difference. + # This works correctly for full-view operations but partial indexing still has issues. + fabric_device = self._device + if self._device == "cuda": + logger.warning("Fabric device is not specified, defaulting to 'cuda:0'.") + fabric_device = "cuda:0" + + self._fabric_selection = fabric_stage.SelectPrims( + require_attrs=[ + (usdrt.Sdf.ValueTypeNames.UInt, self._view_index_attr, usdrt.Usd.Access.Read), + (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.ReadWrite), + ], + device=fabric_device, + ) + + # Step 4: Create bidirectional mapping between view and fabric indices + self._view_to_fabric = wp.zeros((self.count,), dtype=wp.uint32).to(self._device) + self._fabric_to_view = wp.fabricarray(self._fabric_selection, self._view_index_attr) + + wp.launch( + kernel=fabric_utils.set_view_to_fabric_array, + dim=self._fabric_to_view.shape[0], + inputs=[self._fabric_to_view, self._view_to_fabric], + device=self._device, + ) + # Synchronize to ensure mapping is ready before any operations + wp.synchronize() + + # Pre-allocate reusable output buffers for read operations + self._fabric_positions_torch = torch.zeros((self.count, 3), dtype=torch.float32, device=self._device) + self._fabric_orientations_torch = torch.zeros((self.count, 4), dtype=torch.float32, device=self._device) + self._fabric_scales_torch = torch.zeros((self.count, 3), dtype=torch.float32, device=self._device) + + # Create Warp views of the PyTorch tensors + self._fabric_positions_buffer = wp.from_torch(self._fabric_positions_torch, dtype=wp.float32) + self._fabric_orientations_buffer = wp.from_torch(self._fabric_orientations_torch, dtype=wp.float32) + self._fabric_scales_buffer = wp.from_torch(self._fabric_scales_torch, dtype=wp.float32) + + # Dummy array for unused outputs (always empty) + self._fabric_dummy_buffer = wp.zeros((0, 3), dtype=wp.float32).to(self._device) + + # Cache fabricarray for world matrices to avoid recreation overhead + # Refs: https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/usdrt_prim_selection.html + # https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/scenegraph_use.html + self._fabric_world_matrices = wp.fabricarray(self._fabric_selection, "omni:fabric:worldMatrix") + + # Cache Fabric stage to avoid expensive get_current_stage() calls + self._fabric_stage = fabric_stage + + self._fabric_initialized = True + # Force a one-time USD->Fabric sync on first read to pick up any USD edits + # made after the view was constructed. + self._fabric_usd_sync_done = False + + def _sync_fabric_from_usd_once(self) -> None: + """Sync Fabric world matrices from USD once, on the first read.""" + # Ensure Fabric is initialized + if not self._fabric_initialized: + self._initialize_fabric() + + # Ensure authored USD transforms are flushed before reading into Fabric. + sim_utils.update_stage() + + # Read authoritative transforms from USD and write once into Fabric. + positions_usd, orientations_usd = self._get_world_poses_usd() + scales_usd = self._get_scales_usd() + + prev_sync = self._sync_usd_on_fabric_write + self._sync_usd_on_fabric_write = False + self._set_world_poses_fabric(positions_usd, orientations_usd) + self._set_scales_fabric(scales_usd) + self._sync_usd_on_fabric_write = prev_sync + + self._fabric_usd_sync_done = True + + def _resolve_indices_wp(self, indices: Sequence[int] | None) -> wp.array: + """Resolve view indices as a Warp array.""" + if indices is None or indices == slice(None): + if self._default_view_indices is None: + raise RuntimeError("Fabric indices are not initialized.") + return self._default_view_indices + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + return wp.array(indices_list, dtype=wp.uint32).to(self._device) diff --git a/source/isaaclab/isaaclab/terrains/__init__.py b/source/isaaclab/isaaclab/terrains/__init__.py index ac22f8c76a85..6f0b50185572 100644 --- a/source/isaaclab/isaaclab/terrains/__init__.py +++ b/source/isaaclab/isaaclab/terrains/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -19,10 +19,10 @@ * :meth:`TerrainImporter.import_usd`: spawn a prim as reference to input USD file. """ - from .height_field import * # noqa: F401, F403 +from .sub_terrain_cfg import FlatPatchSamplingCfg, SubTerrainBaseCfg from .terrain_generator import TerrainGenerator -from .terrain_generator_cfg import FlatPatchSamplingCfg, SubTerrainBaseCfg, TerrainGeneratorCfg +from .terrain_generator_cfg import TerrainGeneratorCfg from .terrain_importer import TerrainImporter from .terrain_importer_cfg import TerrainImporterCfg from .trimesh import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/terrains/config/__init__.py b/source/isaaclab/isaaclab/terrains/config/__init__.py index 08b5f7a96fea..264189e183b4 100644 --- a/source/isaaclab/isaaclab/terrains/config/__init__.py +++ b/source/isaaclab/isaaclab/terrains/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/terrains/config/rough.py b/source/isaaclab/isaaclab/terrains/config/rough.py index 0f673b45a9f8..fde3bf8408b4 100644 --- a/source/isaaclab/isaaclab/terrains/config/rough.py +++ b/source/isaaclab/isaaclab/terrains/config/rough.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/terrains/height_field/__init__.py b/source/isaaclab/isaaclab/terrains/height_field/__init__.py index 5e3fb9a4515d..3bc28ba3ccfe 100644 --- a/source/isaaclab/isaaclab/terrains/height_field/__init__.py +++ b/source/isaaclab/isaaclab/terrains/height_field/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/terrains/height_field/hf_terrains.py b/source/isaaclab/isaaclab/terrains/height_field/hf_terrains.py index 779109e13a33..3869eae25c3f 100644 --- a/source/isaaclab/isaaclab/terrains/height_field/hf_terrains.py +++ b/source/isaaclab/isaaclab/terrains/height_field/hf_terrains.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,9 +7,10 @@ from __future__ import annotations +from typing import TYPE_CHECKING + import numpy as np import scipy.interpolate as interpolate -from typing import TYPE_CHECKING from .utils import height_field_to_mesh diff --git a/source/isaaclab/isaaclab/terrains/height_field/hf_terrains_cfg.py b/source/isaaclab/isaaclab/terrains/height_field/hf_terrains_cfg.py index 502011c71342..df1d6dcc21a2 100644 --- a/source/isaaclab/isaaclab/terrains/height_field/hf_terrains_cfg.py +++ b/source/isaaclab/isaaclab/terrains/height_field/hf_terrains_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,7 +7,7 @@ from isaaclab.utils import configclass -from ..terrain_generator_cfg import SubTerrainBaseCfg +from ..sub_terrain_cfg import SubTerrainBaseCfg from . import hf_terrains @@ -21,10 +21,13 @@ class HfTerrainBaseCfg(SubTerrainBaseCfg): 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.""" @@ -43,8 +46,10 @@ class HfRandomUniformTerrainCfg(HfTerrainBaseCfg): 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. @@ -62,8 +67,10 @@ class HfPyramidSlopedTerrainCfg(HfTerrainBaseCfg): 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. @@ -92,10 +99,13 @@ class HfPyramidStairsTerrainCfg(HfTerrainBaseCfg): 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. @@ -127,12 +137,16 @@ class HfDiscreteObstaclesTerrainCfg(HfTerrainBaseCfg): 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.""" @@ -145,8 +159,9 @@ class HfWaveTerrainCfg(HfTerrainBaseCfg): 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.""" + + num_waves: int = 1 + """The number of waves to generate. Defaults to 1.""" @configclass @@ -157,11 +172,15 @@ class HfSteppingStonesTerrainCfg(HfTerrainBaseCfg): 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/source/isaaclab/isaaclab/terrains/height_field/utils.py b/source/isaaclab/isaaclab/terrains/height_field/utils.py index 21332bb81d96..256e8129fe34 100644 --- a/source/isaaclab/isaaclab/terrains/height_field/utils.py +++ b/source/isaaclab/isaaclab/terrains/height_field/utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,12 @@ import copy import functools -import numpy as np -import trimesh from collections.abc import Callable from typing import TYPE_CHECKING +import numpy as np +import trimesh + if TYPE_CHECKING: from .hf_terrains_cfg import HfTerrainBaseCfg diff --git a/source/isaaclab/isaaclab/terrains/sub_terrain_cfg.py b/source/isaaclab/isaaclab/terrains/sub_terrain_cfg.py new file mode 100644 index 000000000000..0dab3ea8f3c1 --- /dev/null +++ b/source/isaaclab/isaaclab/terrains/sub_terrain_cfg.py @@ -0,0 +1,96 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import MISSING + +import numpy as np +import trimesh + +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. + """ diff --git a/source/isaaclab/isaaclab/terrains/terrain_generator.py b/source/isaaclab/isaaclab/terrains/terrain_generator.py index 0d08f019a420..58c0c85be9d1 100644 --- a/source/isaaclab/isaaclab/terrains/terrain_generator.py +++ b/source/isaaclab/isaaclab/terrains/terrain_generator.py @@ -1,25 +1,33 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import numpy as np +from __future__ import annotations + +import logging import os +from typing import TYPE_CHECKING + +import numpy as np 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 +if TYPE_CHECKING: + from .sub_terrain_cfg import FlatPatchSamplingCfg, SubTerrainBaseCfg + from .terrain_generator_cfg import TerrainGeneratorCfg + +# import logger +logger = logging.getLogger(__name__) + class TerrainGenerator: r"""Terrain generator to handle different terrain generation functions. @@ -45,21 +53,23 @@ class TerrainGenerator: .. math:: - \text{difficulty} = \frac{\text{row_id} + \eta}{\text{num_rows}} \times (\text{upper} - \text{lower}) + \text{lower} + \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: + 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 + 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. @@ -108,6 +118,8 @@ def __init__(self, cfg: TerrainGeneratorCfg, device: str = "cpu"): self.device = device # set common values to all sub-terrains config + from .height_field import HfTerrainBaseCfg # prevent circular import + for sub_cfg in self.cfg.sub_terrains.values(): # size of all terrains sub_cfg.size = self.cfg.size @@ -119,7 +131,7 @@ def __init__(self, cfg: TerrainGeneratorCfg, device: str = "cpu"): # 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( + logger.warning( "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." ) @@ -271,7 +283,7 @@ def _add_terrain_border(self): -self.cfg.border_height / 2, ) # border mesh - border_meshes = make_border(border_size, inner_size, height=self.cfg.border_height, position=border_center) + 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) @@ -295,7 +307,7 @@ def _add_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})") + logger.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 diff --git a/source/isaaclab/isaaclab/terrains/terrain_generator_cfg.py b/source/isaaclab/isaaclab/terrains/terrain_generator_cfg.py index f500123d522e..6a3238c7cb4a 100644 --- a/source/isaaclab/isaaclab/terrains/terrain_generator_cfg.py +++ b/source/isaaclab/isaaclab/terrains/terrain_generator_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,100 +14,25 @@ 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.""" +from .sub_terrain_cfg import SubTerrainBaseCfg +from .terrain_generator import TerrainGenerator @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. - """ +class TerrainGeneratorCfg: + """Configuration for the terrain generator.""" - 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. + class_type: type = TerrainGenerator + """The class to use for the terrain generator. - The keys correspond to the name of the flat patch sampling configuration and the values are the - corresponding configurations. + Defaults to :class:`isaaclab.terrains.terrain_generator.TerrainGenerator`. """ - -@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. @@ -136,7 +61,13 @@ class TerrainGeneratorCfg: """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.""" + """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.""" diff --git a/source/isaaclab/isaaclab/terrains/terrain_importer.py b/source/isaaclab/isaaclab/terrains/terrain_importer.py index b0a2138dfc1d..e9ddc691b35b 100644 --- a/source/isaaclab/isaaclab/terrains/terrain_importer.py +++ b/source/isaaclab/isaaclab/terrains/terrain_importer.py @@ -1,27 +1,29 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +import logging +from typing import TYPE_CHECKING + 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 +# import logger +logger = logging.getLogger(__name__) + class TerrainImporter: r"""A class to handle terrain meshes and import them into the simulator. @@ -84,10 +86,15 @@ def __init__(self, cfg: TerrainImporterCfg): 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) + terrain_generator = self.cfg.terrain_generator.class_type( + 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) + if self.cfg.use_terrain_origins: + # configure the terrain origins based on the terrain generator + self.configure_env_origins(terrain_generator.terrain_origins) + else: + self.configure_env_origins() # refer to the flat patches self._terrain_flat_patches = terrain_generator.flat_patches elif self.cfg.terrain_type == "usd": @@ -207,7 +214,7 @@ def import_ground_plane(self, name: str, size: tuple[float, float] = (2.0e6, 2.0 if "diffuse_color" in material: color = material["diffuse_color"] else: - omni.log.warn( + logger.warning( "Visual material specified for ground plane but no diffuse color found." " Using default color: (0.0, 0.0, 0.0)" ) @@ -372,7 +379,7 @@ def warp_meshes(self): .. deprecated:: v2.1.0 The `warp_meshes` attribute is deprecated. It is no longer stored inside the class. """ - omni.log.warn( + logger.warning( "The `warp_meshes` attribute is deprecated. It is no longer stored inside the `TerrainImporter` class." " Returning an empty dictionary." ) @@ -385,7 +392,7 @@ def meshes(self) -> dict[str, trimesh.Trimesh]: .. deprecated:: v2.1.0 The `meshes` attribute is deprecated. It is no longer stored inside the class. """ - omni.log.warn( + logger.warning( "The `meshes` attribute is deprecated. It is no longer stored inside the `TerrainImporter` class." " Returning an empty dictionary." ) diff --git a/source/isaaclab/isaaclab/terrains/terrain_importer_cfg.py b/source/isaaclab/isaaclab/terrains/terrain_importer_cfg.py index 2a0a15a33134..7b42e06caaf3 100644 --- a/source/isaaclab/isaaclab/terrains/terrain_importer_cfg.py +++ b/source/isaaclab/isaaclab/terrains/terrain_importer_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -65,12 +65,19 @@ class TerrainImporterCfg: """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". + This parameter is used only when the ``terrain_type`` is "plane" or "usd" or if + :attr:`use_terrain_origins` is False. """ - visual_material: sim_utils.VisualMaterialCfg | None = sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.065, 0.0725, 0.080) - ) + use_terrain_origins: bool = True + """Whether to set the environment origins based on the terrain origins or in a grid + according to :attr:`env_spacing`. Defaults to True. + + Note: + This parameter is used only when the :attr:`terrain type` is "generator". + """ + + 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. diff --git a/source/isaaclab/isaaclab/terrains/trimesh/__init__.py b/source/isaaclab/isaaclab/terrains/trimesh/__init__.py index ab741224300c..b27b7a921109 100644 --- a/source/isaaclab/isaaclab/terrains/trimesh/__init__.py +++ b/source/isaaclab/isaaclab/terrains/trimesh/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py b/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py index a31472b4448e..d5a327aebe58 100644 --- a/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py +++ b/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,11 +7,12 @@ from __future__ import annotations +from typing import TYPE_CHECKING + 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 @@ -253,9 +254,9 @@ def random_grid_terrain( """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. + 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. @@ -397,7 +398,7 @@ def rails_terrain( 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]) + rail_height = cfg.rail_height_range[0] + difficulty * (cfg.rail_height_range[1] - cfg.rail_height_range[0]) # initialize list of meshes meshes_list = list() @@ -773,6 +774,7 @@ def repeated_objects_terrain( # -- 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) + platform_height = cfg.platform_height if cfg.platform_height >= 0.0 else height # -- object specific parameters # note: SIM114 requires duplicated logical blocks under a single body. if isinstance(cfg, MeshRepeatedBoxesTerrainCfg): @@ -808,11 +810,13 @@ def repeated_objects_terrain( # 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], - ]) + origin = np.asarray((0.5 * cfg.size[0], 0.5 * cfg.size[1], 0.5 * platform_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 @@ -840,7 +844,9 @@ def repeated_objects_terrain( # 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) + abs_height_noise = np.random.uniform(cfg.abs_height_noise[0], cfg.abs_height_noise[1]) + rel_height_noise = np.random.uniform(cfg.rel_height_noise[0], cfg.rel_height_noise[1]) + ob_height = height * rel_height_noise + abs_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) @@ -849,8 +855,8 @@ def repeated_objects_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) + dim = (cfg.platform_width, cfg.platform_width, 0.5 * platform_height) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], 0.25 * platform_height) platform = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) meshes_list.append(platform) diff --git a/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains_cfg.py b/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains_cfg.py index 7d733126e710..4247e21486be 100644 --- a/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains_cfg.py +++ b/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains_cfg.py @@ -1,8 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +import warnings from dataclasses import MISSING from typing import Literal @@ -10,7 +11,7 @@ import isaaclab.terrains.trimesh.utils as mesh_utils_terrains from isaaclab.utils import configclass -from ..terrain_generator_cfg import SubTerrainBaseCfg +from ..sub_terrain_cfg import SubTerrainBaseCfg """ Different trimesh terrain configurations. @@ -35,12 +36,16 @@ class MeshPyramidStairsTerrainCfg(SubTerrainBaseCfg): 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. @@ -69,10 +74,13 @@ class MeshRandomGridTerrainCfg(SubTerrainBaseCfg): 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. @@ -89,8 +97,10 @@ class MeshRailsTerrainCfg(SubTerrainBaseCfg): 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.""" @@ -103,8 +113,10 @@ class MeshPitTerrainCfg(SubTerrainBaseCfg): 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.""" @@ -117,8 +129,10 @@ class MeshBoxTerrainCfg(SubTerrainBaseCfg): 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.""" @@ -131,6 +145,7 @@ class MeshGapTerrainCfg(SubTerrainBaseCfg): 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.""" @@ -143,10 +158,13 @@ class MeshFloatingRingTerrainCfg(SubTerrainBaseCfg): 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.""" @@ -159,10 +177,13 @@ class MeshStarTerrainCfg(SubTerrainBaseCfg): 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.""" @@ -189,16 +210,43 @@ class ObjectCfg: ``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.""" + max_height_noise: float | None = None + """"This parameter is deprecated, but stated here to support backward compatibility""" + + abs_height_noise: tuple[float, float] = (0.0, 0.0) + """The minimum and maximum amount of additive noise for the height of the objects. Default is set to 0.0, + which is no noise. + """ + + rel_height_noise: tuple[float, float] = (1.0, 1.0) + """The minimum and maximum amount of multiplicative noise for the height of the objects. Default is set to 1.0, + which is no noise. + """ + platform_width: float = 1.0 """The width of the cylindrical platform at the center of the terrain. Defaults to 1.0.""" + platform_height: float = -1.0 + """The height of the platform. Defaults to -1.0. + + If the value is negative, the height is the same as the object height. + """ + + def __post_init__(self): + if self.max_height_noise is not None: + warnings.warn( + "MeshRepeatedObjectsTerrainCfg: max_height_noise:float is deprecated and support will be removed in the" + " future. Use abs_height_noise:list[float] instead." + ) + self.abs_height_noise = (-self.max_height_noise, self.max_height_noise) + @configclass class MeshRepeatedPyramidsTerrainCfg(MeshRepeatedObjectsTerrainCfg): @@ -219,6 +267,7 @@ class ObjectCfg(MeshRepeatedObjectsTerrainCfg.ObjectCfg): 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.""" @@ -242,6 +291,7 @@ class ObjectCfg(MeshRepeatedObjectsTerrainCfg.ObjectCfg): 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.""" @@ -265,5 +315,6 @@ class ObjectCfg(MeshRepeatedObjectsTerrainCfg.ObjectCfg): 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/source/isaaclab/isaaclab/terrains/trimesh/utils.py b/source/isaaclab/isaaclab/terrains/trimesh/utils.py index 076859e8406b..aede42f3b7da 100644 --- a/source/isaaclab/isaaclab/terrains/trimesh/utils.py +++ b/source/isaaclab/isaaclab/terrains/trimesh/utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/terrains/utils.py b/source/isaaclab/isaaclab/terrains/utils.py index dc0a7ff96991..0feee6ca51f3 100644 --- a/source/isaaclab/isaaclab/terrains/utils.py +++ b/source/isaaclab/isaaclab/terrains/utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,7 +9,6 @@ import numpy as np import torch import trimesh - import warp as wp from isaaclab.utils.warp import raycast_mesh @@ -80,15 +79,14 @@ def create_prim_from_mesh(prim_path: str, mesh: trimesh.Trimesh, **kwargs): 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") + sim_utils.create_prim(prim_path, "Xform") # create mesh prim - prim = prim_utils.create_prim( + prim = sim_utils.create_prim( f"{prim_path}/mesh", "Mesh", translation=kwargs.get("translation"), diff --git a/source/isaaclab/isaaclab/ui/widgets/__init__.py b/source/isaaclab/isaaclab/ui/widgets/__init__.py index cd9c96958de8..cfd6de31fa2b 100644 --- a/source/isaaclab/isaaclab/ui/widgets/__init__.py +++ b/source/isaaclab/isaaclab/ui/widgets/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/ui/widgets/image_plot.py b/source/isaaclab/isaaclab/ui/widgets/image_plot.py index 71242c9e183e..939ef01dfa9f 100644 --- a/source/isaaclab/isaaclab/ui/widgets/image_plot.py +++ b/source/isaaclab/isaaclab/ui/widgets/image_plot.py @@ -1,15 +1,22 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + +import logging +from contextlib import suppress +from typing import TYPE_CHECKING + import numpy as np 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 @@ -17,6 +24,9 @@ import isaacsim.gui.components import omni.ui +# import logger +logger = logging.getLogger(__name__) + class ImagePlot(UIWidgetWrapper): """An image plot widget to display live data. @@ -36,7 +46,9 @@ class ImagePlot(UIWidgetWrapper): |||+-------------------------------------------------+||| ||| mode_frame ||| ||| ||| - ||| [x][Absolute] [x][Grayscaled] [ ][Colorized] ||| + ||| [Dropdown: Mode Selection] ||| + ||| [Collapsible: Manual Normalization Options] ||| + ||+---------------------------------------------------+|| |+-----------------------------------------------------+| +-------------------------------------------------------+ @@ -44,11 +56,11 @@ class ImagePlot(UIWidgetWrapper): def __init__( self, - image: Optional[np.ndarray] = None, + image: np.ndarray | None = None, label: str = "", widget_height: int = 200, - show_min_max: bool = True, - unit: tuple[float, str] = (1, ""), + min_value: float = 0.0, + max_value: float = 1.0, ): """Create an XY plot UI Widget with axis scaling, legends, and support for multiple plots. @@ -59,12 +71,9 @@ def __init__( 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 + min_value: Minimum value for manual normalization/colorization. Defaults to 0.0. + max_value: Maximum value for manual normalization/colorization. Defaults to 1.0. """ - self._show_min_max = show_min_max - self._unit_scale = unit[0] - self._unit_name = unit[1] self._curr_mode = "None" @@ -74,7 +83,7 @@ def __init__( self._byte_provider = omni.ui.ByteImageProvider() if image is None: - carb.log_warn("image is NONE") + logger.warning("image is NONE") image = np.ones((480, 640, 3), dtype=np.uint8) * 255 image[:, :, 0] = 0 image[:, :240, 1] = 0 @@ -110,7 +119,7 @@ def update_image(self, image: np.ndarray): 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") + logger.warning("Colorization mode is only available for single channel images") else: image = (image - image.min()) / (image.max() - image.min()) colormap = cm.get_cmap("jet") @@ -150,7 +159,6 @@ def _get_unit_description(self, min_value: float, max_value: float, median_value ) def _build_widget(self): - with omni.ui.VStack(spacing=3): with omni.ui.HStack(): # Write the leftmost label for what this plot is @@ -179,25 +187,124 @@ def _build_mode_frame(self): The built widget has the following layout: +-------------------------------------------------------+ - | legends_frame | + | mode_frame | ||+---------------------------------------------------+|| - ||| ||| - ||| [x][Series 1] [x][Series 2] [ ][Series 3] ||| - ||| ||| + ||| [Dropdown: Mode Selection] ||| + ||| [Collapsible: Manual Normalization Options] ||| |||+-------------------------------------------------+||| |+-----------------------------------------------------+| +-------------------------------------------------------+ """ - 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, - ) + with omni.ui.VStack(spacing=5, style=isaacsim.gui.components.ui_utils.get_style()): + + def _change_mode(value): + self._curr_mode = value + # Update visibility of collapsible frame + show_options = value in ["Normalization", "Colorization"] + if hasattr(self, "_options_collapsable"): + self._options_collapsable.visible = show_options + if show_options: + self._options_collapsable.title = f"{value} Options" + + # Mode dropdown + isaacsim.gui.components.ui_utils.dropdown_builder( + label="Mode", + type="dropdown", + items=["Original", "Normalization", "Colorization"], + tooltip="Select a mode", + on_clicked_fn=_change_mode, + ) + + # Collapsible frame for options (initially hidden) + self._options_collapsable = omni.ui.CollapsableFrame( + "Normalization Options", + height=0, + collapsed=False, + visible=False, + style=isaacsim.gui.components.ui_utils.get_style(), + style_type_name_override="CollapsableFrame", + ) + + with self._options_collapsable: + with omni.ui.VStack(spacing=5, style=isaacsim.gui.components.ui_utils.get_style()): + + def _on_manual_changed(enabled): + self._enabled_min_max = enabled + # Enable/disable the float fields + if hasattr(self, "_min_model"): + self._min_field.enabled = enabled + if hasattr(self, "_max_model"): + self._max_field.enabled = enabled + + def _on_min_changed(model): + self._min_value = model.as_float + + def _on_max_changed(model): + self._max_value = model.as_float + + # Manual values checkbox + isaacsim.gui.components.ui_utils.cb_builder( + label="Use Manual Values", + type="checkbox", + default_val=self._enabled_min_max, + tooltip="Enable manual min/max values", + on_clicked_fn=_on_manual_changed, + ) + + # Min value with reset button + with omni.ui.HStack(): + omni.ui.Label( + "Min", + width=isaacsim.gui.components.ui_utils.LABEL_WIDTH, + alignment=omni.ui.Alignment.LEFT_CENTER, + ) + self._min_field = omni.ui.FloatDrag( + name="FloatField", + width=omni.ui.Fraction(1), + height=0, + alignment=omni.ui.Alignment.LEFT_CENTER, + enabled=self._enabled_min_max, + ) + self._min_model = self._min_field.model + self._min_model.set_value(self._min_value) + self._min_model.add_value_changed_fn(_on_min_changed) + + omni.ui.Spacer(width=5) + omni.ui.Button( + "0", + width=20, + height=20, + clicked_fn=lambda: self._min_model.set_value(0.0), + tooltip="Reset to 0.0", + style=isaacsim.gui.components.ui_utils.get_style(), + ) + isaacsim.gui.components.ui_utils.add_line_rect_flourish(False) + + # Max value with reset button + with omni.ui.HStack(): + omni.ui.Label( + "Max", + width=isaacsim.gui.components.ui_utils.LABEL_WIDTH, + alignment=omni.ui.Alignment.LEFT_CENTER, + ) + self._max_field = omni.ui.FloatDrag( + name="FloatField", + width=omni.ui.Fraction(1), + height=0, + alignment=omni.ui.Alignment.LEFT_CENTER, + enabled=self._enabled_min_max, + ) + self._max_model = self._max_field.model + self._max_model.set_value(self._max_value) + self._max_model.add_value_changed_fn(_on_max_changed) + + omni.ui.Spacer(width=5) + omni.ui.Button( + "1", + width=20, + height=20, + clicked_fn=lambda: self._max_model.set_value(1.0), + tooltip="Reset to 1.0", + style=isaacsim.gui.components.ui_utils.get_style(), + ) + isaacsim.gui.components.ui_utils.add_line_rect_flourish(False) diff --git a/source/isaaclab/isaaclab/ui/widgets/line_plot.py b/source/isaaclab/isaaclab/ui/widgets/line_plot.py index 1443c41bcaed..1f5e315a07cf 100644 --- a/source/isaaclab/isaaclab/ui/widgets/line_plot.py +++ b/source/isaaclab/isaaclab/ui/widgets/line_plot.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,12 +6,14 @@ from __future__ import annotations import colorsys -import numpy as np from contextlib import suppress from typing import TYPE_CHECKING +import numpy as np + import omni -from isaacsim.core.api.simulation_context import SimulationContext + +from isaaclab.sim import SimulationContext with suppress(ImportError): # isaacsim.gui is not available when running in headless mode. @@ -73,13 +75,16 @@ def __init__( """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_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. + 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 @@ -157,7 +162,6 @@ def add_datapoint(self, y_coords: list[float]): """ 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:] @@ -307,8 +311,13 @@ def _build_single_plot(y_data: list[float], color: int, plot_idx: int): height=0, ) with omni.ui.Placer(offset_x=-20): + label_value = ( + self._y_max + - first_space * grid_resolution + - grid_line_idx * grid_resolution + ) omni.ui.Label( - f"{(self._y_max - first_space * grid_resolution - grid_line_idx * grid_resolution):.3f}", + f"{label_value:.3f}", width=8, height=8, alignment=omni.ui.Alignment.RIGHT_TOP, diff --git a/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py b/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py index 2a220514980b..0b20b10dabce 100644 --- a/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py +++ b/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py @@ -1,20 +1,21 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import numpy +import logging import weakref from dataclasses import MISSING from typing import TYPE_CHECKING -import carb +import numpy + import omni.kit.app -from isaacsim.core.api.simulation_context import SimulationContext from isaaclab.managers import ManagerBase +from isaaclab.sim import SimulationContext from isaaclab.utils import configclass from .image_plot import ImagePlot @@ -24,15 +25,20 @@ if TYPE_CHECKING: import omni.ui +# import logger +logger = logging.getLogger(__name__) + @configclass class ManagerLiveVisualizerCfg: - "Configuration for ManagerLiveVisualizer" + """Configuration for the :class:`ManagerLiveVisualizer` class.""" debug_vis: bool = False - """Flag used to set status of the live visualizers on startup. Defaults to closed.""" + """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. @@ -42,15 +48,21 @@ class ManagerLiveVisualizerCfg: 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 ManagerLiveVisualizerCfg. + """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 get_active_iterable_terms method. + manager: The manager with terms to be plotted. The manager must have a + :meth:`~isaaclab.managers.manager_base.ManagerBase.get_active_iterable_terms` method. cfg: The configuration file used to select desired manager terms to be plotted. """ @@ -72,7 +84,7 @@ def __init__(self, manager: ManagerBase, cfg: ManagerLiveVisualizerCfg = Manager if term_name in self._manager.active_terms: self.term_names.append(term_name) else: - carb.log_err( + logger.error( f"ManagerVisualizer Failure: ManagerTerm ({term_name}) does not exist in" f" Manager({self.cfg.manager_name})" ) @@ -87,17 +99,17 @@ def __init__(self, manager: ManagerBase, cfg: ManagerLiveVisualizerCfg = Manager if term_name in self._manager.active_terms[group]: self.term_names.append(f"{group}-{term_name}") else: - carb.log_err( + logger.error( f"ManagerVisualizer Failure: ManagerTerm ({term_name}) does not exist in" f" Group({group})" ) else: - carb.log_err( + logger.error( f"ManagerVisualizer Failure: Group ({group}) does not exist in" f" Manager({self.cfg.manager_name})" ) else: - carb.log_err( + logger.error( f"ManagerVisualizer Failure: Manager({self.cfg.manager_name}) does not utilize grouping of" " terms." ) @@ -108,12 +120,12 @@ def __init__(self, manager: ManagerBase, cfg: ManagerLiveVisualizerCfg = Manager @property def get_vis_frame(self) -> omni.ui.Frame: - """Getter for the UI Frame object tied to this visualizer.""" + """Returns the UI Frame object tied to this visualizer.""" return self._vis_frame @property def get_vis_window(self) -> omni.ui.Window: - """Getter for the UI Window object tied to this visualizer.""" + """Returns the UI Window object tied to this visualizer.""" return self._vis_window # @@ -141,7 +153,7 @@ def _set_env_selection_impl(self, env_idx: int): if env_idx > 0 and env_idx < self._manager.num_envs: self._env_idx = env_idx else: - carb.log_warn(f"Environment index is out of range (0,{self._manager.num_envs})") + logger.warning(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. @@ -210,24 +222,17 @@ def _set_debug_vis_impl(self, debug_vis: bool): style={"border_color": 0xFF8A8777, "padding": 4}, ) with frame: - # create line plot for single or multivariable signals + # 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, - ) + 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, - ) + image = ImagePlot(image=numpy.array(term), label=name) self._term_visualizers.append(image) else: - carb.log_warn( + logger.warning( f"ManagerLiveVisualizer: Term ({name}) is not a supported data type for" " visualization." ) diff --git a/source/isaaclab/isaaclab/ui/widgets/ui_visualizer_base.py b/source/isaaclab/isaaclab/ui/widgets/ui_visualizer_base.py index 28c1765c940f..61a32119f300 100644 --- a/source/isaaclab/isaaclab/ui/widgets/ui_visualizer_base.py +++ b/source/isaaclab/isaaclab/ui/widgets/ui_visualizer_base.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/ui/widgets/ui_widget_wrapper.py b/source/isaaclab/isaaclab/ui/widgets/ui_widget_wrapper.py index 28e230ae0abb..9025a8c2e93f 100644 --- a/source/isaaclab/isaaclab/ui/widgets/ui_widget_wrapper.py +++ b/source/isaaclab/isaaclab/ui/widgets/ui_widget_wrapper.py @@ -1,9 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -# This file has been adapted from _isaac_sim/exts/isaacsim.gui.components/isaacsim/gui/components/element_wrappers/base_ui_element_wrappers.py +# 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 diff --git a/source/isaaclab/isaaclab/ui/xr_widgets/__init__.py b/source/isaaclab/isaaclab/ui/xr_widgets/__init__.py new file mode 100644 index 000000000000..e0b341c8d2d5 --- /dev/null +++ b/source/isaaclab/isaaclab/ui/xr_widgets/__init__.py @@ -0,0 +1,7 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +from .instruction_widget import hide_instruction, show_instruction, update_instruction +from .scene_visualization import DataCollector, TriggerType, VisualizationManager, XRVisualization +from .teleop_visualization_manager import TeleopVisualizationManager diff --git a/source/isaaclab/isaaclab/ui/xr_widgets/instruction_widget.py b/source/isaaclab/isaaclab/ui/xr_widgets/instruction_widget.py new file mode 100644 index 000000000000..7d6fe00d7f6e --- /dev/null +++ b/source/isaaclab/isaaclab/ui/xr_widgets/instruction_widget.py @@ -0,0 +1,282 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import asyncio +import functools +import textwrap +from typing import Any, TypeAlias + +import omni.kit.commands +import omni.ui as ui +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 + +import isaaclab.sim as sim_utils + +Vec3Type: TypeAlias = Gf.Vec3f | Gf.Vec3d + +camera_facing_widget_container = {} +camera_facing_widget_timers = {} + + +class SimpleTextWidget(ui.Widget): + """A rectangular text label widget for XR overlays. + + The widget renders a centered label over a rectangular background. It keeps + track of the configured style and an original width value used by + higher-level helpers to update the text. + """ + + def __init__( + self, + text: str | None = "Simple Text", + style: dict[str, Any] | None = None, + original_width: float = 0.0, + **kwargs, + ): + """Initialize the text widget. + + Args: + text (str): Initial text to display. + style (dict[str, Any]): Optional style dictionary (for example: ``{"font_size": 1, "color": 0xFFFFFFFF}``). + original_width (float): Width used when updating the text. + **kwargs: Additional keyword arguments forwarded to ``ui.Widget``. + """ + super().__init__(**kwargs) + if style is None: + style = {"font_size": 1, "color": 0xFFFFFFFF} + self._text = text + self._style = style + self._ui_label = None + self._original_width = original_width + self._build_ui() + + def set_label_text(self, text: str): + """Update the text displayed by the label. + + Args: + text (str): New label text to display. + """ + self._text = text + if self._ui_label: + self._ui_label.text = self._text + + def get_font_size(self): + """Return the configured font size. + + Returns: + float: Font size value. + """ + return self._style.get("font_size", 1) + + def get_width(self): + """Return the width used when updating the text. + + Returns: + float: Width used when updating the text. + """ + return self._original_width + + 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, str]: + """Estimate widget width/height and wrap the text. + + Args: + text (str): Raw text to render. + font_size (float): Font size used for estimating character metrics. + max_width (float): Maximum allowed widget width. + min_width (float): Minimum allowed widget width. + + Returns: + tuple[float, float, str]: A tuple ``(width, height, wrapped_text)`` where + ``width`` and ``height`` are the computed widget dimensions, and + ``wrapped_text`` contains the input text broken into newline-separated + lines to fit within the width constraints. + """ + # 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 + wrapped_text = "\n".join(lines) + return actual_width, actual_height, wrapped_text + + +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, + text_color: int = 0xFFFFFFFF, + target_prim_path: str = "/newPrim", +) -> UiContainer | None: + """Create and display an instruction widget with the given text. + + The widget size is computed from the text and font size, wrapping content + to respect the width limits. If ``display_duration`` is provided and + non-zero, the widget is hidden automatically after the duration elapses. + + Args: + text (str): Instruction text to display. + prim_path_source (str | None): Optional prim path used as a spatial source for the widget. + translation (Gf.Vec3d): World translation to apply to the widget. + display_duration (float | None): Seconds to keep the widget visible. If ``None`` or ``0``, + the widget remains until hidden manually. + max_width (float): Maximum widget width used for wrapping. + min_width (float): Minimum widget width used for wrapping. + font_size (float): Font size of the rendered text. + text_color (int): RGBA color encoded as a 32-bit integer. + target_prim_path (str): Prim path where the widget prim will be created/copied. + + Returns: + UiContainer | None: The container that owns the instruction widget, or ``None`` if creation failed. + """ + 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] + + # Obtain stage handle + stage = sim_utils.get_current_stage() + # Clean up existing widget + if stage.GetPrimAtPath(target_prim_path).IsValid(): + sim_utils.delete_prim(target_prim_path) + + width, height, wrapped_text = compute_widget_dimensions(text, font_size, max_width, min_width) + + # Create the widget component. + widget_component = WidgetComponent( + SimpleTextWidget, + width=width, + height=height, + resolution_scale=300, + widget_args=[wrapped_text, {"font_size": font_size, "color": text_color}, width], + ) + + 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_instruction, target_prim_path) + ) + camera_facing_widget_timers[target_prim_path] = timer + + return container + + +def hide_instruction(target_prim_path: str = "/newPrim") -> None: + """Hide and clean up a specific instruction widget. + + Args: + target_prim_path (str): Prim path of the widget to hide. + + Returns: + None: This function does not return a value. + """ + + 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] + + +def update_instruction(target_prim_path: str = "/newPrim", text: str = ""): + """Update the text content of an existing instruction widget. + + Args: + target_prim_path (str): Prim path of the widget to update. + text (str): New text content to display. + + Returns: + bool: ``True`` if the widget existed and was updated, otherwise ``False``. + """ + global camera_facing_widget_container + + container_data = camera_facing_widget_container.get(target_prim_path) + if container_data: + container, current_text = container_data + + # Only update if the text has actually changed + if current_text != text: + # Access the widget through the manipulator as shown in ui_container.py + manipulator = container.manipulator + + # The WidgetComponent is stored in the manipulator's components + # Try to access the widget component and then the actual widget + components = getattr(manipulator, "_ComposableManipulator__components") + if len(components) > 0: + simple_text_widget = components[0] + if simple_text_widget and simple_text_widget.component and simple_text_widget.component.widget: + width, height, wrapped_text = compute_widget_dimensions( + text, + simple_text_widget.component.widget.get_font_size(), + simple_text_widget.component.widget.get_width(), + simple_text_widget.component.widget.get_width(), + ) + simple_text_widget.component.widget.set_label_text(wrapped_text) + # Update the stored text in the global dictionary + camera_facing_widget_container[target_prim_path] = (container, text) + return True + + return False diff --git a/source/isaaclab/isaaclab/ui/xr_widgets/scene_visualization.py b/source/isaaclab/isaaclab/ui/xr_widgets/scene_visualization.py new file mode 100644 index 000000000000..0be679a6929b --- /dev/null +++ b/source/isaaclab/isaaclab/ui/xr_widgets/scene_visualization.py @@ -0,0 +1,623 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import contextlib +import inspect +import logging +import threading +import time +from collections.abc import Callable +from enum import Enum +from typing import Any, Union + +import numpy as np +import torch + +from pxr import Gf + +from isaaclab.sim import SimulationContext +from isaaclab.ui.xr_widgets import show_instruction + +# import logger +logger = logging.getLogger(__name__) + + +class TriggerType(Enum): + """Enumeration of trigger types for visualization callbacks. + + Defines when callbacks should be executed: + - TRIGGER_ON_EVENT: Execute when a specific event occurs + - TRIGGER_ON_PERIOD: Execute at regular time intervals + - TRIGGER_ON_CHANGE: Execute when a specific data variable changes + - TRIGGER_ON_UPDATE: Execute every frame + """ + + TRIGGER_ON_EVENT = 0 + TRIGGER_ON_PERIOD = 1 + TRIGGER_ON_CHANGE = 2 + TRIGGER_ON_UPDATE = 3 + + +class DataCollector: + """Collects and manages data for visualization purposes. + + This class provides a centralized data store for visualization data, + with change detection and callback mechanisms for real-time updates. + """ + + def __init__(self): + """Initialize the data collector with empty data store and callback system.""" + self._data: dict[str, Any] = {} + self._visualization_callback: Callable | None = None + self._changed_flags: set[str] = set() + + def _values_equal(self, existing_value: Any, new_value: Any) -> bool: + """Compare two values using appropriate method based on their types. + + Handles different data types including None, NumPy arrays, PyTorch tensors, + and standard Python types for accurate change detection. + + Args: + existing_value: The current value stored in the data collector + new_value: The new value to compare against + + Returns: + bool: True if values are equal, False otherwise + """ + # If both are None or one is None + if existing_value is None or new_value is None: + return existing_value is new_value + + # If types are different, they're not equal + if type(existing_value) is not type(new_value): + return False + + # Handle NumPy arrays + if isinstance(existing_value, np.ndarray): + return np.array_equal(existing_value, new_value) + + # Handle torch tensors (if they exist) + if hasattr(existing_value, "equal"): + with contextlib.suppress(Exception): + return torch.equal(existing_value, new_value) + + # For all other types (int, float, string, bool, list, dict, set), use regular equality + with contextlib.suppress(Exception): + return existing_value == new_value + # If comparison fails for any reason, assume they're different + return False + + def update_data(self, name: str, value: Any) -> None: + """Update a data field and trigger change detection. + + This method handles data updates with intelligent change detection. + It also performs pre-processing and post-processing based on the field name. + + Args: + name: The name/key of the data field to update + value: The new value to store (None to remove the field) + """ + existing_value = self.get_data(name) + + if value is None: + self._data.pop(name) + if existing_value is not None: + self._changed_flags.add(name) + return + + # Todo: for list or array, the change won't be detected + # Check if the value has changed using appropriate comparison method + if self._values_equal(existing_value, value): + return + + # Save it + self._data[name] = value + self._changed_flags.add(name) + + def update_loop(self) -> None: + """Process pending changes and trigger visualization callbacks. + + This method should be called regularly to ensure visualization updates + are processed in a timely manner. + """ + if len(self._changed_flags) > 0: + if self._visualization_callback: + self._visualization_callback(self._changed_flags) + self._changed_flags.clear() + + def get_data(self, name: str) -> Any: + """Retrieve data by name. + + Args: + name: The name/key of the data field to retrieve + + Returns: + The stored value, or None if the field doesn't exist + """ + return self._data.get(name) + + def set_visualization_callback(self, callback: Callable) -> None: + """Set the VisualizationManager callback function to be called when data changes. + + Args: + callback: Function to call when data changes, receives set of changed field names + """ + self._visualization_callback = callback + + +class VisualizationManager: + """Base class for managing visualization rules and callbacks. + + Provides a framework for registering and executing callbacks based on + different trigger conditions (events, time periods, data changes). + """ + + # Type aliases for different callback signatures + StandardCallback = Callable[["VisualizationManager", "DataCollector"], None] + EventCallback = Callable[["VisualizationManager", "DataCollector", Any], None] + CallbackType = Union[StandardCallback, EventCallback] # noqa: UP007 + + class TimeCountdown: + """Internal class for managing periodic timer-based callbacks.""" + + period: float + countdown: float + last_time: float + + def __init__(self, period: float, initial_countdown: float = 0.0): + """Initialize a countdown timer. + + Args: + period: Time interval in seconds between callback executions + """ + self.period = period + self.countdown = initial_countdown + self.last_time = time.time() + + def update(self, current_time: float) -> bool: + """Update the countdown timer and check if callback should be triggered. + + Args: + current_time: Current time in seconds + + Returns: + bool: True if callback should be triggered, False otherwise + """ + self.countdown -= current_time - self.last_time + self.last_time = current_time + if self.countdown <= 0.0: + self.countdown = self.period + return True + return False + + # Widget presets for common visualization configurations + @classmethod + def message_widget_preset(cls) -> dict[str, Any]: + """Get the message widget preset configuration. + + Returns: + dict: Configuration dictionary for message widgets + """ + return { + "prim_path_source": "/_xr/stage/xrCamera", + "translation": Gf.Vec3f(0, 0, -2), + "display_duration": 3.0, + "max_width": 2.5, + "min_width": 1.0, + "font_size": 0.1, + "text_color": 0xFF00FFFF, + } + + @classmethod + def panel_widget_preset(cls) -> dict[str, Any]: + """Get the panel widget preset configuration. + + Returns: + dict: Configuration dictionary for panel widgets + """ + return { + "prim_path_source": "/XRAnchor", + "translation": Gf.Vec3f(0, 2, 2), # hard-coded temporarily + "display_duration": 0.0, + "font_size": 0.13, + "max_width": 2, + "min_width": 2, + } + + def display_widget(self, text: str, name: str, args: dict[str, Any]) -> None: + """Display a widget with the given text and configuration. + + Args: + text: Text content to display in the widget + name: Unique identifier for the widget. If duplicated, the old one will be removed from scene. + args: Configuration dictionary for widget appearance and behavior + """ + widget_config = args | {"text": text, "target_prim_path": name} + show_instruction(**widget_config) + + def __init__(self, data_collector: DataCollector): + """Initialize the visualization manager. + + Args: + data_collector: DataCollector instance to access the data for visualization use. + """ + self.data_collector: DataCollector = data_collector + data_collector.set_visualization_callback(self.on_change) + + self._rules_on_period: dict[VisualizationManager.TimeCountdown, VisualizationManager.StandardCallback] = {} + self._rules_on_event: dict[str, list[VisualizationManager.EventCallback]] = {} + self._rules_on_change: dict[str, list[VisualizationManager.StandardCallback]] = {} + self._rules_on_update: list[VisualizationManager.StandardCallback] = [] + + # Todo: add support to registering same callbacks for different names + def on_change(self, names: set[str]) -> None: + """Handle data changes by executing registered callbacks. + + Args: + names: Set of data field names that have changed + """ + for name in names: + callbacks = self._rules_on_change.get(name) + if callbacks: + # Create a copy of the list to avoid modification during iteration + for callback in list(callbacks): + callback(self, self.data_collector) + if len(names) > 0: + self.on_event("default_event_has_change") + + def update_loop(self) -> None: + """Update periodic timers and execute callbacks as needed. + + This method should be called regularly to ensure periodic callbacks + are executed at the correct intervals. + """ + + # Create a copy of the list to avoid modification during iteration + for callback in list(self._rules_on_update): + callback(self, self.data_collector) + + current_time = time.time() + # Create a copy of the items to avoid modification during iteration + for timer, callback in list(self._rules_on_period.items()): + triggered = timer.update(current_time) + if triggered: + callback(self, self.data_collector) + + def on_event(self, event: str, params: Any = None) -> None: + """Handle events by executing registered callbacks. + + Args: + event: Name of the event that occurred + """ + callbacks = self._rules_on_event.get(event) + if callbacks is None: + return + # Create a copy of the list to avoid modification during iteration + for callback in list(callbacks): + callback(self, self.data_collector, params) + + # Todo: better organization of callbacks + def register_callback(self, trigger: TriggerType, arg: dict, callback: CallbackType) -> Any: + """Register a callback function to be executed based on trigger conditions. + + Args: + trigger: Type of trigger that should execute the callback + arg: Dictionary containing trigger-specific parameters: + - For TRIGGER_ON_PERIOD: {"period": float} + - For TRIGGER_ON_EVENT: {"event_name": str} + - For TRIGGER_ON_CHANGE: {"variable_name": str} + - For TRIGGER_ON_UPDATE: {} + callback: Function to execute when trigger condition is met. The callback should have + the following signatures according to the trigger type: + - For TRIGGER_ON_EVENT: + callback( + manager: VisualizationManager, + data_collector: DataCollector, + event_params: Any, + ) + - For others: + callback( + manager: VisualizationManager, + data_collector: DataCollector, + ) + + Raises: + TypeError: If callback signature doesn't match the expected signature for the trigger type + """ + # Validate callback signature based on trigger type + self._validate_callback_signature(trigger, callback) + + match trigger: + case TriggerType.TRIGGER_ON_PERIOD: + period = arg.get("period") + initial_countdown = arg.get("initial_countdown", 0.0) + if isinstance(period, float) and isinstance(initial_countdown, float): + timer = VisualizationManager.TimeCountdown(period=period, initial_countdown=initial_countdown) + # Type cast since we've validated the signature + self._rules_on_period[timer] = callback # type: ignore + return timer + case TriggerType.TRIGGER_ON_EVENT: + event = arg.get("event_name") + if isinstance(event, str): + callbacks = self._rules_on_event.get(event) + if callbacks is None: + # Type cast since we've validated the signature + self._rules_on_event[event] = [callback] # type: ignore + else: + # Type cast since we've validated the signature + self._rules_on_event[event].append(callback) # type: ignore + return event + case TriggerType.TRIGGER_ON_CHANGE: + variable_name = arg.get("variable_name") + if isinstance(variable_name, str): + callbacks = self._rules_on_change.get(variable_name) + if callbacks is None: + # Type cast since we've validated the signature + self._rules_on_change[variable_name] = [callback] # type: ignore + else: + # Type cast since we've validated the signature + self._rules_on_change[variable_name].append(callback) # type: ignore + return variable_name + case TriggerType.TRIGGER_ON_UPDATE: + # Type cast since we've validated the signature + self._rules_on_update.append(callback) # type: ignore + return None + + # Todo: better callback-cancel method + def cancel_rule(self, trigger: TriggerType, arg: str | TimeCountdown, callback: Callable | None = None) -> None: + """Remove a previously registered callback. + + Periodic callbacks are not supported to be cancelled for now. + + Args: + trigger: Type of trigger for the callback to remove + arg: Trigger-specific identifier (event name or variable name) + callback: The callback function to remove + """ + callbacks = None + match trigger: + case TriggerType.TRIGGER_ON_CHANGE: + callbacks = self._rules_on_change.get(arg) + case TriggerType.TRIGGER_ON_EVENT: + callbacks = self._rules_on_event.get(arg) + case TriggerType.TRIGGER_ON_PERIOD: + self._rules_on_period.pop(arg) + case TriggerType.TRIGGER_ON_UPDATE: + callbacks = self._rules_on_update + if callbacks is not None: + if callback is not None: + callbacks.remove(callback) + else: + callbacks.clear() + + def set_attr(self, name: str, value: Any) -> None: + """Set an attribute of the visualization manager. + + Args: + name: Name of the attribute to set + value: Value to set the attribute to + """ + setattr(self, name, value) + + def _validate_callback_signature(self, trigger: TriggerType, callback: Callable) -> None: + """Validate that the callback has the correct signature for the trigger type. + + Args: + trigger: Type of trigger for the callback + callback: The callback function to validate + + Raises: + TypeError: If callback signature doesn't match expected signature + """ + try: + sig = inspect.signature(callback) + params = list(sig.parameters.values()) + + # Remove 'self' parameter if it's a bound method + if params and params[0].name == "self": + params = params[1:] + + param_count = len(params) + + if trigger == TriggerType.TRIGGER_ON_EVENT: + # Event callbacks should have 3 parameters: (manager, data_collector, event_params) + expected_count = 3 + expected_sig = ( + "callback(manager: VisualizationManager, data_collector: DataCollector, event_params: Any)" + ) + else: + # Other callbacks should have 2 parameters: (manager, data_collector) + expected_count = 2 + expected_sig = "callback(manager: VisualizationManager, data_collector: DataCollector)" + + if param_count != expected_count: + raise TypeError( + f"Callback for {trigger.name} must have {expected_count} parameters, " + f"but got {param_count}. Expected signature: {expected_sig}. " + f"Actual signature: {sig}" + ) + + except Exception as e: + if isinstance(e, TypeError): + raise + # If we can't inspect the signature (e.g., built-in functions), + # just log a warning and proceed + logger.warning(f"Could not validate callback signature for {trigger.name}: {e}") + + +class XRVisualization: + """Singleton class providing XR visualization functionality. + + This class implements the singleton pattern to ensure only one instance + of the visualization system exists across the application. It provides + a centralized API for managing XR visualization features. + + When manage a new event ordata field, please add a comment to the following list. + + Event names: + "ik_solver_failed" + + Data fields: + "manipulability_ellipsoid" : list[float] + "device_raw_data" : dict + "joints_distance_percentage_to_limit" : list[float] + "joints_torque" : list[float] + "joints_torque_limit" : list[float] + "joints_name" : list[str] + "wrist_pose" : list[float] + "approximated_working_space" : list[float] + "hand_torque_mapping" : list[str] + """ + + _lock = threading.Lock() + _instance: XRVisualization | None = None + _registered = False + + def __init__(self): + """Prevent direct instantiation.""" + raise RuntimeError("Use VisualizationInterface classmethods instead of direct instantiation") + + @classmethod + def __create_instance(cls, manager: type[VisualizationManager] = VisualizationManager) -> XRVisualization: + """Get the visualization manager instance. + + Returns: + VisualizationManager: The visualization manager instance + """ + with cls._lock: + if cls._instance is None: + # Bypass __init__ by calling __new__ directly + cls._instance = super().__new__(cls) + cls._instance._initialize(manager) + return cls._instance + + @classmethod + def __get_instance(cls) -> XRVisualization: + """Thread-safe singleton access. + + Returns: + XRVisualization: The singleton instance of the visualization system + """ + if cls._instance is None: + return cls.__create_instance() + elif not cls._instance._registered: + cls._instance._register() + return cls._instance + + def _register(self) -> bool: + """Register the visualization system. + + Returns: + bool: True if the visualization system is registered, False otherwise + """ + if self._registered: + return True + + sim = SimulationContext.instance() + if sim is not None: + sim.add_render_callback("visualization_render_callback", self.update_loop) + self._registered = True + return self._registered + + def _initialize(self, manager: type[VisualizationManager]) -> None: + """Initialize the singleton instance with data collector and visualization manager.""" + + self._data_collector = DataCollector() + self._visualization_manager = manager(self._data_collector) + + self._register() + + self._initialized = True + + # APIs + + def update_loop(self, event) -> None: + """Update the visualization system. + + This method should be called regularly (e.g., every frame) to ensure + visualization updates are processed and periodic callbacks are executed. + """ + self._visualization_manager.update_loop() + self._data_collector.update_loop() + + @classmethod + def push_event(cls, name: str, args: Any = None) -> None: + """Push an event to trigger registered callbacks. + + Args: + name: Name of the event to trigger + args: Optional arguments for the event (currently unused) + """ + instance = cls.__get_instance() + instance._visualization_manager.on_event(name, args) + + @classmethod + def push_data(cls, item: dict[str, Any]) -> None: + """Push data to the visualization system. + + Updates multiple data fields at once. Each key-value pair in the + dictionary will be processed by the data collector. + + Args: + item: Dictionary containing data field names and their values + """ + instance = cls.__get_instance() + for name, value in item.items(): + instance._data_collector.update_data(name, value) + + @classmethod + def set_attrs(cls, attributes: dict[str, Any]) -> None: + """Set configuration data for the visualization system. Not currently used. + + Args: + attributes: Dictionary containing configuration keys and values + """ + + instance = cls.__get_instance() + for name, data in attributes.items(): + instance._visualization_manager.set_attr(name, data) + + @classmethod + def get_attr(cls, name: str) -> Any: + """Get configuration data for the visualization system. Not currently used. + + Args: + name: Configuration key + """ + instance = cls.__get_instance() + return getattr(instance._visualization_manager, name) + + @classmethod + def register_callback(cls, trigger: TriggerType, arg: dict, callback: VisualizationManager.CallbackType) -> None: + """Register a callback function for visualization events. + + Args: + trigger: Type of trigger that should execute the callback + arg: Dictionary containing trigger-specific parameters: + - For TRIGGER_ON_PERIOD: {"period": float} + - For TRIGGER_ON_EVENT: {"event_name": str} + - For TRIGGER_ON_CHANGE: {"variable_name": str} + callback: Function to execute when trigger condition is met + """ + instance = cls.__get_instance() + instance._visualization_manager.register_callback(trigger, arg, callback) + + @classmethod + def assign_manager(cls, manager: type[VisualizationManager]) -> None: + """Assign a visualization manager type to the visualization system. + + Args: + manager: Type of the visualization manager to assign + """ + if cls._instance is not None: + logger.error( + f"Visualization system already initialized to {type(cls._instance._visualization_manager).__name__}," + f" cannot assign manager {manager.__name__}" + ) + return + + cls.__create_instance(manager) diff --git a/source/isaaclab/isaaclab/ui/xr_widgets/teleop_visualization_manager.py b/source/isaaclab/isaaclab/ui/xr_widgets/teleop_visualization_manager.py new file mode 100644 index 000000000000..3ba817c71190 --- /dev/null +++ b/source/isaaclab/isaaclab/ui/xr_widgets/teleop_visualization_manager.py @@ -0,0 +1,67 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from typing import Any + +from isaaclab.ui.xr_widgets import DataCollector, TriggerType, VisualizationManager +from isaaclab.ui.xr_widgets.instruction_widget import hide_instruction + + +class TeleopVisualizationManager(VisualizationManager): + """Specialized visualization manager for teleoperation scenarios. + For sample and debug use. + + Provides teleoperation-specific visualization features including: + - IK error handling and display + """ + + def __init__(self, data_collector: DataCollector): + """Initialize the teleop visualization manager and register callbacks. + + Args: + data_collector: DataCollector instance to read data for visualization use. + """ + super().__init__(data_collector) + + # Handle error event + self._error_text_color = 0xFF0000FF + self.ik_error_widget_id = "/ik_solver_failed" + + self.register_callback(TriggerType.TRIGGER_ON_EVENT, {"event_name": "ik_error"}, self._handle_ik_error) + + def _handle_ik_error(self, mgr: VisualizationManager, data_collector: DataCollector, params: Any = None) -> None: + """Handle IK error events by displaying an error message widget. + + Args: + data_collector: DataCollector instance (unused in this handler) + """ + # Todo: move display_widget to instruction_widget.py + if not hasattr(mgr, "_ik_error_widget_timer"): + self.display_widget( + "IK Error Detected", + mgr.ik_error_widget_id, + VisualizationManager.message_widget_preset() + | {"text_color": self._error_text_color, "display_duration": None}, + ) + mgr._ik_error_widget_timer = mgr.register_callback( + TriggerType.TRIGGER_ON_PERIOD, {"period": 3.0, "initial_countdown": 3.0}, self._hide_ik_error_widget + ) + if mgr._ik_error_widget_timer is None: + mgr.cancel_rule(TriggerType.TRIGGER_ON_PERIOD, mgr._ik_error_widget_timer) + mgr.cancel_rule(TriggerType.TRIGGER_ON_EVENT, "ik_solver_failed") + raise RuntimeWarning("Failed to register IK error widget timer") + else: + mgr._ik_error_widget_timer.countdown = 3.0 + + def _hide_ik_error_widget(self, mgr: VisualizationManager, data_collector: DataCollector) -> None: + """Hide the IK error widget. + + Args: + data_collector: DataCollector instance (unused in this handler) + """ + + hide_instruction(mgr.ik_error_widget_id) + mgr.cancel_rule(TriggerType.TRIGGER_ON_PERIOD, mgr._ik_error_widget_timer) + delattr(mgr, "_ik_error_widget_timer") diff --git a/source/isaaclab/isaaclab/utils/__init__.py b/source/isaaclab/isaaclab/utils/__init__.py index 8a759498bec6..1295715857f4 100644 --- a/source/isaaclab/isaaclab/utils/__init__.py +++ b/source/isaaclab/isaaclab/utils/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,7 +10,10 @@ from .configclass import configclass from .dict import * from .interpolation import * +from .logger import * +from .mesh import * from .modifiers import * from .string import * from .timer import Timer from .types import * +from .version import * diff --git a/source/isaaclab/isaaclab/utils/array.py b/source/isaaclab/isaaclab/utils/array.py index bed417a73ea9..d15fbc275dc7 100644 --- a/source/isaaclab/isaaclab/utils/array.py +++ b/source/isaaclab/isaaclab/utils/array.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,13 +8,13 @@ # 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 numpy as np +import torch import warp as wp -TensorData = Union[np.ndarray, torch.Tensor, wp.array] +TensorData = Union[np.ndarray, torch.Tensor, wp.array] # noqa: UP007 """Type definition for a tensor data. Union of numpy, torch, and warp arrays. diff --git a/source/isaaclab/isaaclab/utils/assets.py b/source/isaaclab/isaaclab/utils/assets.py index 1112902e08de..22c5141587f6 100644 --- a/source/isaaclab/isaaclab/utils/assets.py +++ b/source/isaaclab/isaaclab/utils/assets.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -13,14 +13,20 @@ .. _Omniverse Nucleus: https://docs.omniverse.nvidia.com/nucleus/latest/overview/overview.html """ +import asyncio import io +import logging import os import tempfile +import time from typing import Literal import carb import omni.client +# import logger +logger = logging.getLogger(__name__) + NUCLEUS_ASSET_ROOT_DIR = carb.settings.get_settings().get("/persistent/isaac/asset_root/cloud") """Path to the root directory on the Nucleus Server.""" @@ -97,7 +103,7 @@ def retrieve_file_path(path: str, download_dir: str | None = None, force_downloa # 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) + 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) @@ -127,3 +133,79 @@ def read_file(path: str) -> io.BytesIO: return io.BytesIO(memoryview(file_content).tobytes()) else: raise FileNotFoundError(f"Unable to find the file: {path}") + + +""" +Nucleus Connection. +""" + + +def check_usd_path_with_timeout(usd_path: str, timeout: float = 300, log_interval: float = 30) -> bool: + """Checks whether the given USD file path is available on the NVIDIA Nucleus server. + + This function synchronously runs an asynchronous USD path availability check, + logging progress periodically until it completes. The file is available on the server + if the HTTP status code is 200. Otherwise, the file is not available on the server. + + This is useful for checking server responsiveness before attempting to load a remote + asset. It will block execution until the check completes or times out. + + Args: + usd_path: The remote USD file path to check. + timeout: Maximum time (in seconds) to wait for the server check. + log_interval: Interval (in seconds) at which progress is logged. + + Returns: + Whether the given USD path is available on the server. + """ + start_time = time.time() + loop = asyncio.get_event_loop() + + coroutine = _is_usd_path_available(usd_path, timeout) + task = asyncio.ensure_future(coroutine) + + next_log_time = start_time + log_interval + + first_log = True + while not task.done(): + now = time.time() + if now >= next_log_time: + elapsed = int(now - start_time) + if first_log: + logger.warning(f"Checking server availability for USD path: {usd_path} (timeout: {timeout}s)") + first_log = False + logger.warning(f"Waiting for server response... ({elapsed}s elapsed)") + next_log_time += log_interval + loop.run_until_complete(asyncio.sleep(0.1)) # Yield to allow async work + + return task.result() + + +""" +Helper functions. +""" + + +async def _is_usd_path_available(usd_path: str, timeout: float) -> bool: + """Checks whether the given USD path is available on the Omniverse Nucleus server. + + This function is a asynchronous routine to check the availability of the given USD path on + the Omniverse Nucleus server. It will return True if the USD path is available on the server, + False otherwise. + + Args: + usd_path: The remote or local USD file path to check. + timeout: Timeout in seconds for the async stat call. + + Returns: + Whether the given USD path is available on the server. + """ + try: + result, _ = await asyncio.wait_for(omni.client.stat_async(usd_path), timeout=timeout) + return result == omni.client.Result.OK + except asyncio.TimeoutError: + logger.warning(f"Timed out after {timeout}s while checking for USD: {usd_path}") + return False + except Exception as ex: + logger.warning(f"Exception during USD file check: {type(ex).__name__}: {ex}") + return False diff --git a/source/isaaclab/isaaclab/utils/buffers/__init__.py b/source/isaaclab/isaaclab/utils/buffers/__init__.py index 1dd3dfb5dab4..64da4f6e6ae9 100644 --- a/source/isaaclab/isaaclab/utils/buffers/__init__.py +++ b/source/isaaclab/isaaclab/utils/buffers/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/utils/buffers/circular_buffer.py b/source/isaaclab/isaaclab/utils/buffers/circular_buffer.py index 11df3ddff988..c5c9fe9ff6ad 100644 --- a/source/isaaclab/isaaclab/utils/buffers/circular_buffer.py +++ b/source/isaaclab/isaaclab/utils/buffers/circular_buffer.py @@ -1,11 +1,12 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch from collections.abc import Sequence +import torch + class CircularBuffer: """Circular buffer for storing a history of batched tensor data. @@ -78,8 +79,11 @@ def current_length(self) -> torch.Tensor: @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:]]. + + The shape of the buffer is (batch_size, max_length, ...). + + Note: + The oldest entry is at the beginning of dimension 1. """ buf = self._buffer.clone() buf = torch.roll(buf, shifts=self.max_length - self._pointer - 1, dims=0) @@ -101,7 +105,8 @@ def reset(self, batch_ids: Sequence[int] | None = 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. + # 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): @@ -116,8 +121,10 @@ def append(self, data: torch.Tensor): """ # check the batch size if data.shape[0] != self.batch_size: - raise ValueError(f"The input data has {data.shape[0]} environments while expecting {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 @@ -125,12 +132,11 @@ def append(self, data: torch.Tensor): # 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.to(self._device) + self._buffer[self._pointer] = data # Check for batches with zero pushes and initialize all values in batch to first append - if 0 in self._num_pushes.tolist(): - fill_ids = [i for i, x in enumerate(self._num_pushes.tolist()) if x == 0] - self._num_pushes.tolist().index(0) if 0 in self._num_pushes.tolist() else None - self._buffer[:, fill_ids, :] = data.to(self._device)[fill_ids] + 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 diff --git a/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py b/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py index b0896b39c00e..dd1a1ef72684 100644 --- a/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py +++ b/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,9 +6,10 @@ # needed because we concatenate int and torch.Tensor in the type hints from __future__ import annotations -import torch from collections.abc import Sequence +import torch + from .circular_buffer import CircularBuffer @@ -46,10 +47,10 @@ def __init__(self, history_length: int, batch_size: int, device: str): # 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 environments. + # the minimum and maximum lags across all batch indices. self._min_time_lag = 0 self._max_time_lag = 0 - # the lags for each environment. + # the lags for each batch index. self._time_lags = torch.zeros(batch_size, dtype=torch.int, device=device) """ diff --git a/source/isaaclab/isaaclab/utils/buffers/timestamped_buffer.py b/source/isaaclab/isaaclab/utils/buffers/timestamped_buffer.py index f4d48157466c..30b824464ad1 100644 --- a/source/isaaclab/isaaclab/utils/buffers/timestamped_buffer.py +++ b/source/isaaclab/isaaclab/utils/buffers/timestamped_buffer.py @@ -1,11 +1,12 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch from dataclasses import dataclass +import torch + @dataclass class TimestampedBuffer: diff --git a/source/isaaclab/isaaclab/utils/configclass.py b/source/isaaclab/isaaclab/utils/configclass.py index 4bdecffb78d1..e46280a61ccf 100644 --- a/source/isaaclab/isaaclab/utils/configclass.py +++ b/source/isaaclab/isaaclab/utils/configclass.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -62,6 +62,7 @@ class EnvCfg: episode_length: int = 2000 viewer: ViewerCfg = ViewerCfg() + # create configuration instance env_cfg = EnvCfg(num_envs=24) @@ -153,6 +154,7 @@ class C: x: int y: int + c = C(1, 2) c1 = c.replace(x=3) assert c1.x == 3 and c1.y == 2 @@ -259,6 +261,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 @@ -268,7 +273,11 @@ def _validate(obj: object, prefix: str = "") -> list[str]: missing_fields.extend(_validate(item, prefix=current_path)) return missing_fields elif isinstance(obj, dict): - obj_dict = obj + # Convert any non-string keys to strings to allow validation of dict with non-string keys + if any(not isinstance(key, str) for key in obj.keys()): + obj_dict = {str(key): value for key, value in obj.items()} + else: + obj_dict = obj elif hasattr(obj, "__dict__"): obj_dict = obj.__dict__ else: @@ -294,12 +303,13 @@ def _validate(obj: object, prefix: str = "") -> list[str]: 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`. + 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. + 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 @@ -451,10 +461,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/datasets/__init__.py b/source/isaaclab/isaaclab/utils/datasets/__init__.py index fa70770135f0..fce0fa308fa7 100644 --- a/source/isaaclab/isaaclab/utils/datasets/__init__.py +++ b/source/isaaclab/isaaclab/utils/datasets/__init__.py @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Copyright (c) 2024-2025, The Isaac Lab Project Developers. # All rights reserved. # diff --git a/source/isaaclab/isaaclab/utils/datasets/dataset_file_handler_base.py b/source/isaaclab/isaaclab/utils/datasets/dataset_file_handler_base.py index c786bf416e56..201a0be370ec 100644 --- a/source/isaaclab/isaaclab/utils/datasets/dataset_file_handler_base.py +++ b/source/isaaclab/isaaclab/utils/datasets/dataset_file_handler_base.py @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Copyright (c) 2024-2025, The Isaac Lab Project Developers. # All rights reserved. # diff --git a/source/isaaclab/isaaclab/utils/datasets/episode_data.py b/source/isaaclab/isaaclab/utils/datasets/episode_data.py index 7ae5ce13505c..55df8ebbcbe7 100644 --- a/source/isaaclab/isaaclab/utils/datasets/episode_data.py +++ b/source/isaaclab/isaaclab/utils/datasets/episode_data.py @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Copyright (c) 2024-2025, The Isaac Lab Project Developers. # All rights reserved. # @@ -16,6 +21,7 @@ def __init__(self) -> None: self._data = dict() self._next_action_index = 0 self._next_state_index = 0 + self._next_joint_target_index = 0 self._seed = None self._env_id = None self._success = None @@ -105,12 +111,11 @@ def add(self, key: str, value: torch.Tensor | dict): for sub_key_index in range(len(sub_keys)): if sub_key_index == len(sub_keys) - 1: # Add value to the final dict layer + # Use lists to prevent slow tensor copy during concatenation if sub_keys[sub_key_index] not in current_dataset_pointer: - current_dataset_pointer[sub_keys[sub_key_index]] = value.unsqueeze(0).clone() + current_dataset_pointer[sub_keys[sub_key_index]] = [value.clone()] else: - current_dataset_pointer[sub_keys[sub_key_index]] = torch.cat( - (current_dataset_pointer[sub_keys[sub_key_index]], value.unsqueeze(0)) - ) + current_dataset_pointer[sub_keys[sub_key_index]].append(value.clone()) break # key index if sub_keys[sub_key_index] not in current_dataset_pointer: @@ -155,7 +160,7 @@ def get_state_helper(states, state_index) -> dict | torch.Tensor | None: elif isinstance(states, torch.Tensor): if state_index >= len(states): return None - output_state = states[state_index] + output_state = states[state_index, None] else: raise ValueError(f"Invalid state type: {type(states)}") return output_state @@ -169,3 +174,47 @@ def get_next_state(self) -> dict | None: if state is not None: self._next_state_index += 1 return state + + def get_joint_target(self, joint_target_index) -> dict | torch.Tensor | None: + """Get the joint target of the specified index from the dataset.""" + if "joint_targets" not in self._data: + return None + + joint_targets = self._data["joint_targets"] + + def get_joint_target_helper(joint_targets, joint_target_index) -> dict | torch.Tensor | None: + if isinstance(joint_targets, dict): + output_joint_targets = dict() + for key, value in joint_targets.items(): + output_joint_targets[key] = get_joint_target_helper(value, joint_target_index) + if output_joint_targets[key] is None: + return None + elif isinstance(joint_targets, torch.Tensor): + if joint_target_index >= len(joint_targets): + return None + output_joint_targets = joint_targets[joint_target_index] + else: + raise ValueError(f"Invalid joint target type: {type(joint_targets)}") + return output_joint_targets + + output_joint_targets = get_joint_target_helper(joint_targets, joint_target_index) + return output_joint_targets + + def get_next_joint_target(self) -> dict | torch.Tensor | None: + """Get the next joint target from the dataset.""" + joint_target = self.get_joint_target(self._next_joint_target_index) + if joint_target is not None: + self._next_joint_target_index += 1 + return joint_target + + def pre_export(self): + """Prepare data for export by converting lists to tensors.""" + + def pre_export_helper(data): + for key, value in data.items(): + if isinstance(value, list): + data[key] = torch.stack(value) + elif isinstance(value, dict): + pre_export_helper(value) + + pre_export_helper(self._data) diff --git a/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py b/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py index cf1f0c5c003b..46aeead2fd93 100644 --- a/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py +++ b/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py @@ -1,15 +1,21 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Copyright (c) 2024-2025, The Isaac Lab Project Developers. # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import h5py import json -import numpy as np import os -import torch from collections.abc import Iterable +import h5py +import numpy as np +import torch + from .dataset_file_handler_base import DatasetFileHandlerBase from .episode_data import EpisodeData @@ -131,18 +137,27 @@ def load_dataset_helper(group): return episode - def write_episode(self, episode: EpisodeData): + def write_episode(self, episode: EpisodeData, demo_id: int | None = None): """Add an episode to the dataset. Args: episode: The episode data to add. + demo_id: Custom index for the episode. If None, uses default index. """ self._raise_if_not_initialized() if episode.is_empty(): return - # create episode group based on demo count - h5_episode_group = self._hdf5_data_group.create_group(f"demo_{self._demo_count}") + # Use custom demo id if provided, otherwise use default naming + if demo_id is not None: + episode_group_name = f"demo_{demo_id}" + else: + episode_group_name = f"demo_{self._demo_count}" + + # create episode group with the specified name + if episode_group_name in self._hdf5_data_group: + raise ValueError(f"Episode group '{episode_group_name}' already exists in the dataset") + h5_episode_group = self._hdf5_data_group.create_group(episode_group_name) # store number of steps taken if "actions" in episode.data: @@ -163,7 +178,7 @@ def create_dataset_helper(group, key, value): for sub_key, sub_value in value.items(): create_dataset_helper(key_group, sub_key, sub_value) else: - group.create_dataset(key, data=value.cpu().numpy()) + group.create_dataset(key, data=value.cpu().numpy(), compression="gzip") for key, value in episode.data.items(): create_dataset_helper(h5_episode_group, key, value) @@ -171,8 +186,10 @@ def create_dataset_helper(group, key, value): # increment total step counts self._hdf5_data_group.attrs["total"] += h5_episode_group.attrs["num_samples"] - # increment total demo counts - self._demo_count += 1 + # Only increment demo count if using default indexing + if demo_id is None: + # increment total demo counts + self._demo_count += 1 def flush(self): """Flush the episode data to disk.""" diff --git a/source/isaaclab/isaaclab/utils/dict.py b/source/isaaclab/isaaclab/utils/dict.py index a6a40f06775f..de2062d66979 100644 --- a/source/isaaclab/isaaclab/utils/dict.py +++ b/source/isaaclab/isaaclab/utils/dict.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,10 +8,11 @@ import collections.abc import hashlib import json -import torch -from collections.abc import Iterable, Mapping +from collections.abc import Iterable, Mapping, Sized from typing import Any +import torch + from .array import TENSOR_TYPE_CONVERSIONS, TENSOR_TYPES from .string import callable_to_string, string_to_callable, string_to_slice @@ -90,47 +91,78 @@ def update_class_from_dict(obj, data: dict[str, Any], _ns: str = "") -> None: 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): + + # -- A) if key is present in the object ------------------------------------ + if hasattr(obj, key) or (isinstance(obj, dict) and key in obj): obj_mem = obj[key] if isinstance(obj, dict) else getattr(obj, key) + + # -- 1) nested mapping โ†’ recurse --------------------------- if isinstance(value, Mapping): # recursively call if it is a dictionary update_class_from_dict(obj_mem, value, _ns=key_ns) continue + + # -- 2) iterable (list / tuple / etc.) --------------------- 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: + # ---- 2a) flat iterable โ†’ replace wholesale ---------- + if all(not isinstance(el, Mapping) for el in value): + out_val = tuple(value) if isinstance(obj_mem, tuple) else value + if isinstance(obj, dict): + obj[key] = out_val + else: + setattr(obj, key, out_val) + continue + + # ---- 2b) existing value is None โ†’ abort ------------- + if obj_mem is None: + raise ValueError( + f"[Config]: Cannot merge list under namespace: {key_ns} because the existing value is None." + ) + + # ---- 2c) length mismatch โ†’ abort ------------------- + if isinstance(obj_mem, Sized) and isinstance(value, Sized) and len(obj_mem) != len(value): raise ValueError( f"[Config]: Incorrect length under namespace: {key_ns}." f" Expected: {len(obj_mem)}, Received: {len(value)}." ) + + # ---- 2d) keep tuple/list parity & recurse ---------- if isinstance(obj_mem, tuple): value = tuple(value) else: set_obj = True - # recursively call if iterable contains dictionaries + # recursively call if iterable contains Mappings for i in range(len(obj_mem)): - if isinstance(value[i], dict): + if isinstance(value[i], Mapping): 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 + + # -- 3) callable attribute โ†’ resolve string -------------- elif callable(obj_mem): # update function name value = string_to_callable(value) - elif isinstance(value, type(obj_mem)) or value is None: + + # -- 4) simple scalar / explicit None --------------------- + elif value is None or isinstance(value, type(obj_mem)): pass + + # -- 5) type mismatch โ†’ abort ----------------------------- else: raise ValueError( f"[Config]: Incorrect type under namespace: {key_ns}." f" Expected: {type(obj_mem)}, Received: {type(value)}." ) - # set value + + # -- 6) final assignment --------------------------------- if isinstance(obj, dict): obj[key] = value else: setattr(obj, key, value) + + # -- B) if key is not present ------------------------------------ else: raise KeyError(f"[Config]: Key not found under namespace: {key_ns}.") @@ -267,6 +299,8 @@ def replace_slices_with_strings(data: dict) -> dict: """ if isinstance(data, dict): return {k: replace_slices_with_strings(v) for k, v in data.items()} + elif isinstance(data, list): + return [replace_slices_with_strings(v) for v in data] elif isinstance(data, slice): return f"slice({data.start},{data.stop},{data.step})" else: @@ -284,6 +318,8 @@ def replace_strings_with_slices(data: dict) -> dict: """ if isinstance(data, dict): return {k: replace_strings_with_slices(v) for k, v in data.items()} + elif isinstance(data, list): + return [replace_strings_with_slices(v) for v in data] elif isinstance(data, str) and data.startswith("slice("): return string_to_slice(data) else: diff --git a/source/isaaclab/isaaclab/utils/interpolation/__init__.py b/source/isaaclab/isaaclab/utils/interpolation/__init__.py index 49d376bbbae7..25f6be5f0014 100644 --- a/source/isaaclab/isaaclab/utils/interpolation/__init__.py +++ b/source/isaaclab/isaaclab/utils/interpolation/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/utils/interpolation/linear_interpolation.py b/source/isaaclab/isaaclab/utils/interpolation/linear_interpolation.py index 19a57baf912e..84a371280427 100644 --- a/source/isaaclab/isaaclab/utils/interpolation/linear_interpolation.py +++ b/source/isaaclab/isaaclab/utils/interpolation/linear_interpolation.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/utils/io/__init__.py b/source/isaaclab/isaaclab/utils/io/__init__.py index cde83a391543..9a8b16ed157a 100644 --- a/source/isaaclab/isaaclab/utils/io/__init__.py +++ b/source/isaaclab/isaaclab/utils/io/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -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/pkl.py b/source/isaaclab/isaaclab/utils/io/pkl.py deleted file mode 100644 index f6679a9f3f3a..000000000000 --- a/source/isaaclab/isaaclab/utils/io/pkl.py +++ /dev/null @@ -1,50 +0,0 @@ -# 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 -import pickle -from typing import Any - - -def load_pickle(filename: str) -> Any: - """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, "rb") as f: - data = pickle.load(f) - return data - - -def dump_pickle(filename: str, data: Any): - """Saves data into a pickle 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. - """ - # check ending - if not filename.endswith("pkl"): - filename += ".pkl" - # create directory - if not os.path.exists(os.path.dirname(filename)): - os.makedirs(os.path.dirname(filename), exist_ok=True) - # save data - with open(filename, "wb") as f: - pickle.dump(data, f) diff --git a/source/isaaclab/isaaclab/utils/io/torchscript.py b/source/isaaclab/isaaclab/utils/io/torchscript.py new file mode 100644 index 000000000000..df96ebca1233 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/io/torchscript.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""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/io/yaml.py b/source/isaaclab/isaaclab/utils/io/yaml.py index 22204231af5e..0f2dbeeefb9c 100644 --- a/source/isaaclab/isaaclab/utils/io/yaml.py +++ b/source/isaaclab/isaaclab/utils/io/yaml.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,6 +6,7 @@ """Utilities for file I/O with yaml.""" import os + import yaml from isaaclab.utils import class_to_dict diff --git a/source/isaaclab/isaaclab/utils/logger.py b/source/isaaclab/isaaclab/utils/logger.py new file mode 100644 index 000000000000..c9293e931a72 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/logger.py @@ -0,0 +1,182 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module with logging utilities. + +To use the logger, you can use the :func:`logging.getLogger` function. + +Example: + >>> import logging + >>> + >>> # define logger for the current module (enables fine-control) + >>> logger = logging.getLogger(__name__) + >>> + >>> # log messages + >>> logger.info("This is an info message") + >>> logger.warning("This is a warning message") + >>> logger.error("This is an error message") + >>> logger.critical("This is a critical message") + >>> logger.debug("This is a debug message") +""" + +from __future__ import annotations + +import logging +import os +import sys +import tempfile +import time +from datetime import datetime +from typing import Literal + + +def configure_logging( + logging_level: Literal["DEBUG", "INFO", "WARNING", "ERROR", "CRITICAL"] = "WARNING", + save_logs_to_file: bool = True, + log_dir: str | None = None, +) -> logging.Logger: + """Setup the logger with a colored formatter and a rate limit filter. + + This function defines the default logger for IsaacLab. It adds a stream handler with a colored formatter + and a rate limit filter. If :attr:`save_logs_to_file` is True, it also adds a file handler to save the logs + to a file. The log directory can be specified using :attr:`log_dir`. If not provided, the logs will be saved + to the temp directory with the sub-directory "isaaclab/logs". + + The log file name is formatted as "isaaclab_{datetime.now().strftime('%Y-%m-%d_%H-%M-%S')}.log". + The log record format is "%(asctime)s [%(filename)s:%(lineno)d] %(levelname)s: %(message)s". + The date format is "%Y-%m-%d %H:%M:%S". + + Args: + logging_level: The logging level. + save_logs_to_file: Whether to save the logs to a file. + log_dir: The directory to save the logs to. Default is None, in which case the logs + will be saved to the temp directory with the sub-directory "isaaclab/logs". + + Returns: + The root logger. + """ + root_logger = logging.getLogger() + # the root logger must be the lowest level to ensure that all messages are logged + root_logger.setLevel(logging.DEBUG) + + # remove existing handlers + # Note: iterate over a copy [:] to avoid modifying list during iteration + for handler in root_logger.handlers[:]: + root_logger.removeHandler(handler) + + # add a stream handler with default level + handler = logging.StreamHandler(sys.stdout) + handler.setLevel(logging_level) + + # add a colored formatter + formatter = ColoredFormatter(fmt="%(asctime)s [%(filename)s] %(levelname)s: %(message)s", datefmt="%H:%M:%S") + handler.setFormatter(formatter) + handler.addFilter(RateLimitFilter(interval_seconds=5)) + root_logger.addHandler(handler) + + # add a file handler + if save_logs_to_file: + # if log_dir is not provided, use the temp directory + if log_dir is None: + log_dir = os.path.join(tempfile.gettempdir(), "isaaclab", "logs") + # create the log directory if it does not exist + os.makedirs(log_dir, exist_ok=True) + # create the log file path + log_file_path = os.path.join(log_dir, f"isaaclab_{datetime.now().strftime('%Y-%m-%d_%H-%M-%S')}.log") + + # create the file handler + file_handler = logging.FileHandler(log_file_path, mode="w", encoding="utf-8") + file_handler.setLevel(logging.DEBUG) + file_formatter = logging.Formatter( + fmt="%(asctime)s [%(filename)s:%(lineno)d] %(levelname)s: %(message)s", datefmt="%Y-%m-%d %H:%M:%S" + ) + file_handler.setFormatter(file_formatter) + root_logger.addHandler(file_handler) + + # print the log file path once at startup with nice formatting + cyan = "\033[36m" # cyan color + bold = "\033[1m" # bold text + reset = "\033[0m" # reset formatting + message = f"[INFO][IsaacLab]: Logging to file: {log_file_path}" + border = "=" * len(message) + print(f"\n{cyan}{border}{reset}") + print(f"{cyan}{bold}{message}{reset}") + print(f"{cyan}{border}{reset}\n") + + # return the root logger + return root_logger + + +class ColoredFormatter(logging.Formatter): + """Colored formatter for logging. + + This formatter colors the log messages based on the log level. + """ + + COLORS = { + "WARNING": "\033[33m", # orange/yellow + "ERROR": "\033[31m", # red + "CRITICAL": "\033[1;31m", # bold red + "INFO": "\033[0m", # reset + "DEBUG": "\033[0m", + } + """Colors for different log levels.""" + + RESET = "\033[0m" + """Reset color.""" + + def format(self, record: logging.LogRecord) -> str: + """Format the log record. + + Args: + record: The log record to format. + + Returns: + The formatted log record. + """ + color = self.COLORS.get(record.levelname, self.RESET) + message = super().format(record) + return f"{color}{message}{self.RESET}" + + +class RateLimitFilter(logging.Filter): + """Custom rate-limited warning filter. + + This filter allows warning-level messages only once every few seconds per message. + This is useful to avoid flooding the log with the same message multiple times. + """ + + def __init__(self, interval_seconds: int = 5): + """Initialize the rate limit filter. + + Args: + interval_seconds: The interval in seconds to limit the warnings. + Defaults to 5 seconds. + """ + super().__init__() + self.interval = interval_seconds + self.last_emitted = {} + + def filter(self, record: logging.LogRecord) -> bool: + """Allow warning-level messages only once every few seconds per message. + + Args: + record: The log record to filter. + + Returns: + True if the message should be logged, False otherwise. + """ + # only filter warning-level messages + if record.levelno != logging.WARNING: + return True + # check if the message has been logged in the last interval + now = time.time() + msg_key = record.getMessage() + if msg_key not in self.last_emitted or (now - self.last_emitted[msg_key]) > self.interval: + # if the message has not been logged in the last interval, log it + self.last_emitted[msg_key] = now + return True + # if the message has been logged in the last interval, do not log it + return False diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index 73ee4cae35d0..96314abfbd09 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/source/isaaclab/isaaclab/utils/math.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,11 +8,16 @@ # needed to import for allowing type-hinting: torch.Tensor | np.ndarray from __future__ import annotations +import logging import math +from typing import Literal + import numpy as np import torch import torch.nn.functional -from typing import Literal + +# import logger +logger = logging.getLogger(__name__) """ General @@ -131,8 +136,8 @@ def copysign(mag: float, other: torch.Tensor) -> torch.Tensor: 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) + mag_torch = abs(mag) * torch.ones_like(other) + return torch.copysign(mag_torch, other) """ @@ -140,6 +145,22 @@ def copysign(mag: float, other: torch.Tensor) -> torch.Tensor: """ +@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. @@ -232,20 +253,21 @@ def quat_conjugate(q: torch.Tensor) -> torch.Tensor: """ shape = q.shape q = q.reshape(-1, 4) - return torch.cat((q[:, 0:1], -q[:, 1:]), dim=-1).view(shape) + 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. +def quat_inv(q: torch.Tensor, eps: float = 1e-9) -> torch.Tensor: + """Computes the inverse of a quaternion. Args: q: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + eps: A small value to avoid division by zero. Defaults to 1e-9. Returns: The inverse quaternion in (w, x, y, z). Shape is (N, 4). """ - return normalize(quat_conjugate(q)) + return quat_conjugate(q) / q.pow(2).sum(dim=-1, keepdim=True).clamp(min=eps) @torch.jit.script @@ -382,7 +404,7 @@ def _axis_angle_rotation(axis: Literal["X", "Y", "Z"], angle: torch.Tensor) -> t def matrix_from_euler(euler_angles: torch.Tensor, convention: str) -> torch.Tensor: """ - Convert rotations given as Euler angles in radians to rotation matrices. + Convert rotations given as Euler angles (intrinsic) in radians to rotation matrices. Args: euler_angles: Euler angles in radians. Shape is (..., 3). @@ -411,14 +433,19 @@ def matrix_from_euler(euler_angles: torch.Tensor, convention: str) -> torch.Tens @torch.jit.script -def euler_xyz_from_quat(quat: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: +def euler_xyz_from_quat( + quat: torch.Tensor, wrap_to_2pi: bool = False +) -> 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. + The euler angles are assumed in XYZ extrinsic convention. Args: quat: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + wrap_to_2pi (bool): Whether to wrap output Euler angles into [0, 2ฯ€). If + False, angles are returned in the default range (โˆ’ฯ€, ฯ€]. Defaults to + False. Returns: A tuple containing roll-pitch-yaw. Each element is a tensor of shape (N,). @@ -441,23 +468,58 @@ def euler_xyz_from_quat(quat: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, 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 ? + if wrap_to_2pi: + return roll % (2 * torch.pi), pitch % (2 * torch.pi), yaw % (2 * torch.pi) + return roll, pitch, yaw @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. +def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tensor: + """Convert rotations given as quaternions to axis/angle. - 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: + 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: - q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + 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: - Standardized quaternions. Shape is (..., 4). + The quaternion in (w, x, y, z). Shape is (N, 4). """ - return torch.where(q[..., 0:1] < 0, -q, q) + theta = (angle / 2).unsqueeze(-1) + xyz = normalize(axis) * theta.sin() + w = theta.cos() + return normalize(torch.cat([w, xyz], dim=-1)) @torch.jit.script @@ -499,25 +561,6 @@ def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: return torch.stack([w, x, y, z], dim=-1).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). - """ - quat_diff = quat_mul(q1, quat_conjugate(q2)) # q1 * q2^-1 - re = quat_diff[:, 0] # real part, q = [w, x, y, z] = [re, im] - im = quat_diff[:, 1:] # imaginary part - norm_im = torch.norm(im, dim=1) - scale = 2.0 * torch.where(norm_im > 1.0e-7, torch.atan2(norm_im, re) / norm_im, torch.sign(re)) - return scale.unsqueeze(-1) * im - - @torch.jit.script def yaw_quat(quat: torch.Tensor) -> torch.Tensor: """Extract the yaw component of a quaternion. @@ -529,19 +572,58 @@ def yaw_quat(quat: torch.Tensor) -> torch.Tensor: A quaternion with only yaw component. """ shape = quat.shape - quat_yaw = quat.clone().view(-1, 4) + 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[:] = 0.0 + 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. @@ -564,6 +646,28 @@ def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: 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. @@ -579,10 +683,12 @@ def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: return quat_apply(quat_yaw, vec) -@torch.jit.script 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). @@ -590,22 +696,21 @@ def quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: Returns: The rotated vector in (x, y, z). Shape is (..., 3). """ - q_w = q[..., 0] - q_vec = q[..., 1:] - a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) - b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 - # for two-dimensional tensors, bmm is faster than einsum - if q_vec.dim() == 2: - c = q_vec * torch.bmm(q_vec.view(q.shape[0], 1, 3), v.view(q.shape[0], 3, 1)).squeeze(-1) * 2.0 - else: - c = q_vec * torch.einsum("...i,...i->...", q_vec, v).unsqueeze(-1) * 2.0 - return a + b + c + # deprecation + logger.warning( + "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) -@torch.jit.script 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). @@ -613,65 +718,11 @@ def quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: Returns: The rotated vector in (x, y, z). Shape is (..., 3). """ - q_w = q[..., 0] - q_vec = q[..., 1:] - a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) - b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 - # for two-dimensional tensors, bmm is faster than einsum - if q_vec.dim() == 2: - c = q_vec * torch.bmm(q_vec.view(q.shape[0], 1, 3), v.view(q.shape[0], 3, 1)).squeeze(-1) * 2.0 - else: - c = q_vec * torch.einsum("...i,...i->...", q_vec, v).unsqueeze(-1) * 2.0 - return a - b + c - - -@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 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 + logger.warning( + "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[..., 1:4] / sin_half_angles_over_angles.unsqueeze(-1) + return quat_apply_inverse(q, v) @torch.jit.script @@ -685,8 +736,8 @@ def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: Returns: Angular error between input quaternions in radians. """ - quat_diff = quat_mul(q1, quat_conjugate(q2)) - return torch.norm(axis_angle_from_quat(quat_diff), dim=-1) + axis_angle_error = quat_box_minus(q1, q2) + return torch.norm(axis_angle_error, dim=-1) @torch.jit.script @@ -781,6 +832,43 @@ def combine_frame_transforms( 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 @@ -1440,7 +1528,10 @@ def convert_camera_frame_orientation_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} + 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}` @@ -1448,7 +1539,10 @@ def convert_camera_frame_orientation_convention( .. 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} + 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. @@ -1561,16 +1655,15 @@ def create_rotation_matrix_from_view( return R.transpose(1, 2) -def make_pose(pos, rot): - """ - Make homogeneous pose matrices from a set of translation vectors and rotation matrices. +def make_pose(pos: torch.Tensor, rot: torch.Tensor) -> torch.Tensor: + """Creates transformation matrices from positions and rotation matrices. Args: - pos (torch.Tensor): batch of position vectors with last dimension of 3 - rot (torch.Tensor): batch of rotation matrices with last 2 dimensions of (3, 3) + pos: Batch of position vectors with last dimension of 3. + rot: Batch of rotation matrices with last 2 dimensions of (3, 3). Returns: - pose (torch.Tensor): batch of pose matrices with last 2 dimensions of (4, 4) + 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" @@ -1583,33 +1676,31 @@ def make_pose(pos, rot): return pose -def unmake_pose(pose): - """ - Split homogeneous pose matrices back into translation vectors and rotation matrices. +def unmake_pose(pose: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Splits transformation matrices into positions and rotation matrices. Args: - pose (torch.Tensor): batch of pose matrices with last 2 dimensions of (4, 4) + pose: Batch of pose matrices with last 2 dimensions of (4, 4). Returns: - pos (torch.Tensor): batch of position vectors with last dimension of 3 - rot (torch.Tensor): batch of rotation matrices with last 2 dimensions of (3, 3) + 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): - """ - Computes the inverse of homogeneous pose matrices. +def pose_inv(pose: torch.Tensor) -> torch.Tensor: + """Computes the inverse of transformation matrices. - Note that the inverse of a pose matrix is the following: - [R t; 0 1]^-1 = [R.T -R.T*t; 0 1] + The inverse of a pose matrix [R t; 0 1] is [R.T -R.T*t; 0 1]. Args: - pose (torch.Tensor): batch of pose matrices with last 2 dimensions of (4, 4) + pose: Batch of pose matrices with last 2 dimensions of (4, 4). Returns: - inv_pose (torch.Tensor): batch of inverse pose matrices with last 2 dimensions of (4, 4) + 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) @@ -1617,44 +1708,50 @@ def pose_inv(pose): inv_pose = torch.zeros_like(pose) - # take transpose of last 2 dimensions + # 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 + # 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, pose_A_in_B): - """ - Converts homogeneous matrices corresponding to a point C in frame A - to homogeneous matrices corresponding to the same point C in frame B. +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 (torch.Tensor): batch of homogeneous matrices corresponding to the pose of C in frame A - pose_A_in_B (torch.Tensor): batch of homogeneous matrices corresponding to the pose of A in frame B + 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: - pose_in_B (torch.Tensor): batch of homogeneous matrices corresponding to the pose of C in frame B + 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, q2, tau): - """ - Spherical linear interpolation (SLERP) between two quaternions. - This function does NOT support batch processing. +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 (torch.Tensor): The first quaternion (w, x, y, z) format. - q2 (torch.Tensor): The second quaternion (w, x, y, z) format. - tau (float): Interpolation coefficient between 0 (q1) and 1 (q2). + 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: - torch.Tensor: The interpolated quaternion (w, x, y, z) format. + 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" @@ -1666,7 +1763,7 @@ def quat_slerp(q1, q2, tau): if abs(abs(d) - 1.0) < torch.finfo(q1.dtype).eps * 4.0: return q1 if d < 0.0: - # invert rotation + # Invert rotation d = -d q2 *= -1.0 angle = torch.acos(torch.clamp(d, -1, 1)) @@ -1679,24 +1776,24 @@ def quat_slerp(q1, q2, tau): return q1 -def interpolate_rotations(R1, R2, num_steps, axis_angle=True): - """ - Interpolate between two rotation matrices. +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 (torch.Tensor): The first rotation matrix (4x4). - R2 (torch.Tensor): The second rotation matrix (4x4). - num_steps (int): The number of desired interpolated rotations (excluding start and end). - axis_angle (bool, optional): If True, interpolate in axis-angle representation. Else, use slerp. Defaults to True. + 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: - torch.Tensor: A stack of interpolated rotation matrices (shape: (num_steps + 1, 4, 4)), - including the start and end rotations. + 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 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) @@ -1704,15 +1801,15 @@ def interpolate_rotations(R1, R2, num_steps, axis_angle=True): # Grab angle delta_angle = torch.linalg.norm(delta_axis_angle) - # fix the axis, and chunk the angle up into steps + # 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 + # Convert into delta rotation matrices, and then convert to absolute rotations if delta_angle < 0.05: - # small angle - don't bother with interpolation + # 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 + # 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) @@ -1725,29 +1822,33 @@ def interpolate_rotations(R1, R2, num_steps, axis_angle=True): [matrix_from_quat(quat_slerp(q1, q2, tau=float(i) / num_steps)) for i in range(num_steps)] ) - # add in endpoint + # Add in endpoint rot_steps = torch.cat([rot_steps, R2[None]], dim=0) return rot_steps -def interpolate_poses(pose_1, pose_2, num_steps=None, step_size=None, perturb=False): - """ - Linear interpolation between two poses. +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 (torch.tensor): 4x4 start pose - pose_2 (torch.tensor): 4x4 end pose - num_steps (int): if provided, specifies the number of desired interpolated points (not excluding - the start and end points). Passing 0 corresponds to no interpolation, and passing None - means that @step_size must be provided to determine the number of interpolated points. - step_size (float): if provided, will be used to infer the number of steps, by taking the norm - of the delta position vector, and dividing it by the step size - perturb (bool): if True, randomly move all the interpolated position points in a uniform, non-overlapping grid. + 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: - pose_steps (torch.tensor): array of shape (N + 2, 3) corresponding to the interpolated pose path, where N is @num_steps - num_steps (int): the number of interpolated points (N) in the path + 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" @@ -1757,7 +1858,7 @@ def interpolate_poses(pose_1, pose_2, num_steps=None, step_size=None, perturb=Fa pos2, rot2 = unmake_pose(pose_2) if num_steps == 0: - # skip interpolation + # Skip interpolation return ( torch.cat([pos1[None], pos2[None]], dim=0), torch.cat([rot1[None], rot2[None]], dim=0), @@ -1769,51 +1870,48 @@ def interpolate_poses(pose_1, pose_2, num_steps=None, step_size=None, perturb=Fa assert torch.norm(delta_pos) > 0 num_steps = math.ceil(torch.norm(delta_pos) / step_size) - num_steps += 1 # include starting pose + num_steps += 1 # Include starting pose assert num_steps >= 2 - # linear interpolation of positions + # Linear interpolation of positions pos_step_size = delta_pos / num_steps grid = torch.arange(num_steps, dtype=torch.float32) if perturb: - # move the interpolation grid points by up to a half-size forward or backward + # 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 + # Add in endpoint pos_steps = torch.cat([pos_steps, pos2[None]], dim=0) - # interpolate the rotations too + # 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, frame_A, frame_B): - """ - 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. +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 (torch.tensor): Input pose sequence (shape [T, 4, 4]) from the source demonstration - frame_A (torch.tensor): 4x4 frame A pose - frame_B (torch.tensor): 4x4 frame B pose + 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_eef_poses (torch.tensor): transformed pose sequence (shape [T, 4, 4]) + Transformed pose sequence of shape [T, 4, 4]. """ - - # transform source end effector poses to be relative to source object frame + # 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 + # 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], @@ -1821,15 +1919,14 @@ def transform_poses_from_frame_A_to_frame_B(src_poses, frame_A, frame_B): return transformed_poses -def generate_random_rotation(rot_boundary=(2 * math.pi)): - """ - Generates a random rotation matrix using Euler angles. +def generate_random_rotation(rot_boundary: float = (2 * math.pi)) -> torch.Tensor: + """Generates a random rotation matrix using Euler angles. Args: - rot_boundary (float): The range for random rotation angles around each axis (x, y, z). + rot_boundary: Range for random rotation angles around each axis (x, y, z). Returns: - torch.tensor: A 3x3 rotation matrix. + 3x3 rotation matrix. """ angles = torch.rand(3) * rot_boundary Rx = torch.tensor( @@ -1849,29 +1946,27 @@ def generate_random_rotation(rot_boundary=(2 * math.pi)): return R -def generate_random_translation(pos_boundary=1): - """ - Generates a random translation vector. +def generate_random_translation(pos_boundary: float = 1) -> torch.Tensor: + """Generates a random translation vector. Args: - pos_boundary (float): The range for random translation values in 3D space. + pos_boundary: Range for random translation values in 3D space. Returns: - torch.tensor: A 3-element translation vector. + 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=1, rot_boundary=(2 * math.pi)): - """ - Generates a random transformation matrix combining rotation and translation. +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 (float): The range for random translation values. - rot_boundary (float): The range for random rotation angles. + pos_boundary: Range for random translation values. + rot_boundary: Range for random rotation angles. Returns: - torch.tensor: A 4x4 transformation matrix. + 4x4 transformation matrix. """ R = generate_random_rotation(rot_boundary) translation = generate_random_translation(pos_boundary) diff --git a/source/isaaclab/isaaclab/utils/mesh.py b/source/isaaclab/isaaclab/utils/mesh.py new file mode 100644 index 000000000000..9e6315cc83c7 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/mesh.py @@ -0,0 +1,183 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Utility functions for working with meshes.""" + +from collections.abc import Callable + +import numpy as np +import trimesh + +from pxr import Usd, UsdGeom + +__all__ = [ + "create_trimesh_from_geom_mesh", + "create_trimesh_from_geom_shape", + "convert_faces_to_triangles", + "PRIMITIVE_MESH_TYPES", +] + + +def create_trimesh_from_geom_mesh(mesh_prim: Usd.Prim) -> trimesh.Trimesh: + """Reads the vertices and faces of a mesh prim. + + The function reads the vertices and faces of a mesh prim and returns it. If the underlying mesh is a quad mesh, + it converts it to a triangle mesh. + + Args: + mesh_prim: The mesh prim to read the vertices and faces from. + + Returns: + A trimesh.Trimesh object containing the mesh geometry. + """ + if mesh_prim.GetTypeName() != "Mesh": + raise ValueError(f"Prim at path '{mesh_prim.GetPath()}' is not a mesh.") + # cast into UsdGeomMesh + mesh = UsdGeom.Mesh(mesh_prim) + + # read the vertices and faces + points = np.asarray(mesh.GetPointsAttr().Get()).copy() + + # Load faces and convert to triangle if needed. (Default is quads) + num_vertex_per_face = np.asarray(mesh.GetFaceVertexCountsAttr().Get()) + indices = np.asarray(mesh.GetFaceVertexIndicesAttr().Get()) + return trimesh.Trimesh(points, convert_faces_to_triangles(indices, num_vertex_per_face)) + + +def create_trimesh_from_geom_shape(prim: Usd.Prim) -> trimesh.Trimesh: + """Converts a primitive object to a trimesh. + + Args: + prim: The prim that should be converted to a trimesh. + + Returns: + A trimesh object representing the primitive. + + Raises: + ValueError: If the prim is not a supported primitive. Check PRIMITIVE_MESH_TYPES for supported primitives. + """ + + if prim.GetTypeName() not in PRIMITIVE_MESH_TYPES: + raise ValueError(f"Prim at path '{prim.GetPath()}' is not a primitive mesh. Cannot convert to trimesh.") + + return _MESH_CONVERTERS_CALLBACKS[prim.GetTypeName()](prim) + + +def convert_faces_to_triangles(faces: np.ndarray, point_counts: np.ndarray) -> np.ndarray: + """Converts quad mesh face indices into triangle face indices. + + This function expects an array of faces (indices) and the number of points per face. It then converts potential + quads into triangles and returns the new triangle face indices as a numpy array of shape (n_faces_new, 3). + + Args: + faces: The faces of the quad mesh as a one-dimensional array. Shape is (N,). + point_counts: The number of points per face. Shape is (N,). + + Returns: + The new face ids with triangles. Shape is (n_faces_new, 3). + """ + # check if the mesh is already triangulated + if (point_counts == 3).all(): + return faces.reshape(-1, 3) # already triangulated + all_faces = [] + + vertex_counter = 0 + # Iterates over all faces of the mesh to triangulate them. + # could be very slow for large meshes + for num_points in point_counts: + # Triangulate n-gons (n>4) using fan triangulation + for i in range(num_points - 2): + triangle = np.array([faces[vertex_counter], faces[vertex_counter + 1 + i], faces[vertex_counter + 2 + i]]) + all_faces.append(triangle) + + vertex_counter += num_points + return np.asarray(all_faces) + + +""" +Internal USD Shape Handlers. +""" + + +def _create_plane_trimesh(prim: Usd.Prim) -> trimesh.Trimesh: + """Creates a trimesh for a plane primitive.""" + size = (2e6, 2e6) + vertices = np.array([[size[0], size[1], 0], [size[0], 0.0, 0], [0.0, size[1], 0], [0.0, 0.0, 0]]) - np.array( + [size[0] / 2.0, size[1] / 2.0, 0.0] + ) + faces = np.array([[1, 0, 2], [2, 3, 1]]) + return trimesh.Trimesh(vertices=vertices, faces=faces) + + +def _create_cube_trimesh(prim: Usd.Prim) -> trimesh.Trimesh: + """Creates a trimesh for a cube primitive.""" + size = prim.GetAttribute("size").Get() + extends = [size, size, size] + return trimesh.creation.box(extends) + + +def _create_sphere_trimesh(prim: Usd.Prim, subdivisions: int = 2) -> trimesh.Trimesh: + """Creates a trimesh for a sphere primitive.""" + radius = prim.GetAttribute("radius").Get() + mesh = trimesh.creation.icosphere(radius=radius, subdivisions=subdivisions) + return mesh + + +def _create_cylinder_trimesh(prim: Usd.Prim) -> trimesh.Trimesh: + """Creates a trimesh for a cylinder primitive.""" + radius = prim.GetAttribute("radius").Get() + height = prim.GetAttribute("height").Get() + mesh = trimesh.creation.cylinder(radius=radius, height=height) + axis = prim.GetAttribute("axis").Get() + if axis == "X": + # rotate โˆ’90ยฐ about Y to point the length along +X + R = trimesh.transformations.rotation_matrix(np.radians(-90), [0, 1, 0]) + mesh.apply_transform(R) + elif axis == "Y": + # rotate +90ยฐ about X to point the length along +Y + R = trimesh.transformations.rotation_matrix(np.radians(90), [1, 0, 0]) + mesh.apply_transform(R) + return mesh + + +def _create_capsule_trimesh(prim: Usd.Prim) -> trimesh.Trimesh: + """Creates a trimesh for a capsule primitive.""" + radius = prim.GetAttribute("radius").Get() + height = prim.GetAttribute("height").Get() + mesh = trimesh.creation.capsule(radius=radius, height=height) + axis = prim.GetAttribute("axis").Get() + if axis == "X": + # rotate โˆ’90ยฐ about Y to point the length along +X + R = trimesh.transformations.rotation_matrix(np.radians(-90), [0, 1, 0]) + mesh.apply_transform(R) + elif axis == "Y": + # rotate +90ยฐ about X to point the length along +Y + R = trimesh.transformations.rotation_matrix(np.radians(90), [1, 0, 0]) + mesh.apply_transform(R) + return mesh + + +def _create_cone_trimesh(prim: Usd.Prim) -> trimesh.Trimesh: + """Creates a trimesh for a cone primitive.""" + radius = prim.GetAttribute("radius").Get() + height = prim.GetAttribute("height").Get() + mesh = trimesh.creation.cone(radius=radius, height=height) + # shift all vertices down by height/2 for usd / trimesh cone primitive definition discrepancy + mesh.apply_translation((0.0, 0.0, -height / 2.0)) + return mesh + + +_MESH_CONVERTERS_CALLBACKS: dict[str, Callable[[Usd.Prim], trimesh.Trimesh]] = { + "Plane": _create_plane_trimesh, + "Cube": _create_cube_trimesh, + "Sphere": _create_sphere_trimesh, + "Cylinder": _create_cylinder_trimesh, + "Capsule": _create_capsule_trimesh, + "Cone": _create_cone_trimesh, +} + +PRIMITIVE_MESH_TYPES = list(_MESH_CONVERTERS_CALLBACKS.keys()) +"""List of supported primitive mesh types that can be converted to a trimesh.""" diff --git a/source/isaaclab/isaaclab/utils/modifiers/__init__.py b/source/isaaclab/isaaclab/utils/modifiers/__init__.py index 62aea1cbe9e4..b79a5140a7a8 100644 --- a/source/isaaclab/isaaclab/utils/modifiers/__init__.py +++ b/source/isaaclab/isaaclab/utils/modifiers/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -43,7 +43,7 @@ # create a modifier configuration # a digital filter with a simple delay of 1 timestep - cfg = modifiers.DigitalFilter(A=[0.0], B=[0.0, 1.0]) + cfg = modifiers.DigitalFilterCfg(A=[0.0], B=[0.0, 1.0]) # create the modifier instance my_modifier = modifiers.DigitalFilter(cfg, my_tensor.shape, "cuda") diff --git a/source/isaaclab/isaaclab/utils/modifiers/modifier.py b/source/isaaclab/isaaclab/utils/modifiers/modifier.py index 6abf6cec61a7..182a606565ad 100644 --- a/source/isaaclab/isaaclab/utils/modifiers/modifier.py +++ b/source/isaaclab/isaaclab/utils/modifiers/modifier.py @@ -1,14 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + from .modifier_base import ModifierBase if TYPE_CHECKING: @@ -123,11 +124,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/isaaclab/utils/modifiers/modifier_base.py b/source/isaaclab/isaaclab/utils/modifiers/modifier_base.py index e75c152b848c..65a7fe0bb8a2 100644 --- a/source/isaaclab/isaaclab/utils/modifiers/modifier_base.py +++ b/source/isaaclab/isaaclab/utils/modifiers/modifier_base.py @@ -1,15 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from abc import ABC, abstractmethod from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + if TYPE_CHECKING: from .modifier_cfg import ModifierCfg @@ -31,7 +32,7 @@ class ModifierBase(ABC): from isaaclab.utils import modifiers # define custom keyword arguments to pass to ModifierCfg - kwarg_dict = {"arg_1" : VAL_1, "arg_2" : VAL_2} + 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 diff --git a/source/isaaclab/isaaclab/utils/modifiers/modifier_cfg.py b/source/isaaclab/isaaclab/utils/modifiers/modifier_cfg.py index 00e25fa938fd..cf018fc07165 100644 --- a/source/isaaclab/isaaclab/utils/modifiers/modifier_cfg.py +++ b/source/isaaclab/isaaclab/utils/modifiers/modifier_cfg.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import torch from collections.abc import Callable from dataclasses import MISSING from typing import Any +import torch + from isaaclab.utils import configclass from . import modifier diff --git a/source/isaaclab/isaaclab/utils/noise/__init__.py b/source/isaaclab/isaaclab/utils/noise/__init__.py index e57b4a2c1b0f..7f91067fd005 100644 --- a/source/isaaclab/isaaclab/utils/noise/__init__.py +++ b/source/isaaclab/isaaclab/utils/noise/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/utils/noise/noise_cfg.py b/source/isaaclab/isaaclab/utils/noise/noise_cfg.py index 6bb0378aae36..b3275643fd29 100644 --- a/source/isaaclab/isaaclab/utils/noise/noise_cfg.py +++ b/source/isaaclab/isaaclab/utils/noise/noise_cfg.py @@ -1,15 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from collections.abc import Callable from dataclasses import MISSING from typing import Literal +import torch + from isaaclab.utils import configclass from . import noise_model @@ -78,6 +79,19 @@ class NoiseModelCfg: noise_cfg: NoiseCfg = MISSING """The noise configuration to use.""" + func: Callable[[torch.Tensor], torch.Tensor] | None = None + """Function or callable class used by this noise model. + + The function must take a single `torch.Tensor` (the batch of observations) as input + and return a `torch.Tensor` of the same shape with noise applied. + + It also supports `callable classes `_, + i.e. classes that implement the ``__call__()`` method. In this case, the class should inherit from the + :class:`NoiseModel` class and implement the required methods. + + This field is used internally by :class:ObservationManager and is not meant to be set directly. + """ + @configclass class NoiseModelWithAdditiveBiasCfg(NoiseModelCfg): @@ -90,3 +104,9 @@ class NoiseModelWithAdditiveBiasCfg(NoiseModelCfg): Based on this configuration, the bias is sampled at every reset of the noise model. """ + + sample_bias_per_component: bool = True + """Whether to sample a separate bias for each data component. + + Defaults to True. + """ diff --git a/source/isaaclab/isaaclab/utils/noise/noise_model.py b/source/isaaclab/isaaclab/utils/noise/noise_model.py index f197450534b5..78b93c9f099b 100644 --- a/source/isaaclab/isaaclab/utils/noise/noise_model.py +++ b/source/isaaclab/isaaclab/utils/noise/noise_model.py @@ -1,14 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + if TYPE_CHECKING: from . import noise_cfg @@ -130,7 +131,7 @@ def reset(self, env_ids: Sequence[int] | None = None): """ pass - def apply(self, data: torch.Tensor) -> torch.Tensor: + def __call__(self, data: torch.Tensor) -> torch.Tensor: """Apply the noise to the data. Args: @@ -154,6 +155,8 @@ def __init__(self, noise_model_cfg: noise_cfg.NoiseModelWithAdditiveBiasCfg, num # 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) + self._num_components: int | None = None + self._sample_bias_per_component = noise_model_cfg.sample_bias_per_component def reset(self, env_ids: Sequence[int] | None = None): """Reset the noise model. @@ -170,7 +173,7 @@ def reset(self, env_ids: Sequence[int] | None = 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: + def __call__(self, data: torch.Tensor) -> torch.Tensor: """Apply bias noise to the data. Args: @@ -179,4 +182,11 @@ def apply(self, data: torch.Tensor) -> torch.Tensor: Returns: The data with the noise applied. Shape is the same as the input data. """ - return super().apply(data) + self._bias + # if sample_bias_per_component, on first apply, expand bias to match last dim of data + if self._sample_bias_per_component and self._num_components is None: + *_, self._num_components = data.shape + # expand bias from (num_envs,1) to (num_envs, num_components) + self._bias = self._bias.repeat(1, self._num_components) + # now re-sample that expanded bias in-place + self.reset() + return super().__call__(data) + self._bias diff --git a/source/isaaclab/isaaclab/utils/seed.py b/source/isaaclab/isaaclab/utils/seed.py new file mode 100644 index 000000000000..6b2a8ff97adf --- /dev/null +++ b/source/isaaclab/isaaclab/utils/seed.py @@ -0,0 +1,45 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import random + +import numpy as np +import torch +import warp as wp + + +def configure_seed(seed: int | None, torch_deterministic: bool = False) -> int: + """Set seed across all random number generators (torch, numpy, random, warp). + + Args: + seed: The random seed value. If None, generates a random seed. + torch_deterministic: If True, enables deterministic mode for torch operations. + + Returns: + The seed value that was set. + """ + if seed is None or seed == -1: + seed = 42 if torch_deterministic else random.randint(0, 10000) + + random.seed(seed) + np.random.seed(seed) + torch.manual_seed(seed) + os.environ["PYTHONHASHSEED"] = str(seed) + torch.cuda.manual_seed(seed) + torch.cuda.manual_seed_all(seed) + wp.rand_init(seed) + + if torch_deterministic: + # refer to https://docs.nvidia.com/cuda/cublas/index.html#cublasApi_reproducibility + os.environ["CUBLAS_WORKSPACE_CONFIG"] = ":4096:8" + torch.backends.cudnn.benchmark = False + torch.backends.cudnn.deterministic = True + torch.use_deterministic_algorithms(True) + else: + torch.backends.cudnn.benchmark = True + torch.backends.cudnn.deterministic = False + + return seed diff --git a/source/isaaclab/isaaclab/utils/sensors.py b/source/isaaclab/isaaclab/utils/sensors.py new file mode 100644 index 000000000000..d9016c2f885a --- /dev/null +++ b/source/isaaclab/isaaclab/utils/sensors.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import logging + +# import logger +logger = logging.getLogger(__name__) + + +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, + in which case, the focal length will be calculated as 1 / width. + + Returns: + A dictionary of USD camera parameters for focal_length, horizontal_aperture, vertical_aperture, + horizontal_aperture_offset, and vertical_aperture_offset. + """ + # 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: + logger.warning("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): + logger.warning( + "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/source/isaaclab/isaaclab/utils/string.py b/source/isaaclab/isaaclab/utils/string.py index af92bca38abf..dc1cdaf53477 100644 --- a/source/isaaclab/isaaclab/utils/string.py +++ b/source/isaaclab/isaaclab/utils/string.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -272,7 +272,10 @@ def resolve_matching_names( def resolve_matching_names_values( - data: dict[str, Any], list_of_strings: Sequence[str], preserve_order: bool = False + data: dict[str, Any], + list_of_strings: Sequence[str], + preserve_order: bool = False, + strict: bool = True, ) -> 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. @@ -287,12 +290,14 @@ def resolve_matching_names_values( 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]). + :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. + strict: Whether to require that all keys in the dictionary get matched. Defaults to True. Returns: A tuple of lists containing the matched indices, names, and values. @@ -300,7 +305,7 @@ def resolve_matching_names_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. + ValueError: When not all regular expressions in the data keys are matched (if strict is True). """ # check valid input if not isinstance(data, dict): @@ -354,7 +359,7 @@ def resolve_matching_names_values( names_list = names_list_reorder values_list = values_list_reorder # check that all regular expressions are matched - if not all(keys_match_found): + if strict and not all(keys_match_found): # make this print nicely aligned for debugging msg = "\n" for key, value in zip(data.keys(), keys_match_found): @@ -366,3 +371,46 @@ def resolve_matching_names_values( ) # return return index_list, names_list, values_list + + +def find_unique_string_name(initial_name: str, is_unique_fn: Callable[[str], bool]) -> str: + """Find a unique string name based on the predicate function provided. + The string is appended with "_N", where N is a natural number till the resultant string + is unique. + Args: + initial_name (str): The initial string name. + is_unique_fn (Callable[[str], bool]): The predicate function to validate against. + Returns: + str: A unique string based on input function. + """ + if is_unique_fn(initial_name): + return initial_name + iterator = 1 + result = initial_name + "_" + str(iterator) + while not is_unique_fn(result): + result = initial_name + "_" + str(iterator) + iterator += 1 + return result + + +def find_root_prim_path_from_regex(prim_path_regex: str) -> tuple[str, int]: + """Find the first prim above the regex pattern prim and its position. + Args: + prim_path_regex (str): full prim path including the regex pattern prim. + Returns: + Tuple[str, int]: First position is the prim path to the parent of the regex prim. + Second position represents the level of the regex prim in the USD stage tree representation. + """ + prim_paths_list = str(prim_path_regex).split("/") + root_idx = None + for prim_path_idx in range(len(prim_paths_list)): + chars = set("[]*|^") + if any((c in chars) for c in prim_paths_list[prim_path_idx]): + root_idx = prim_path_idx + break + root_prim_path = None + tree_level = None + if root_idx is not None: + root_prim_path = "/".join(prim_paths_list[:root_idx]) + tree_level = root_idx + return root_prim_path, tree_level diff --git a/source/isaaclab/isaaclab/utils/timer.py b/source/isaaclab/isaaclab/utils/timer.py index be312d45e727..4d9951db60c8 100644 --- a/source/isaaclab/isaaclab/utils/timer.py +++ b/source/isaaclab/isaaclab/utils/timer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/utils/types.py b/source/isaaclab/isaaclab/utils/types.py index a22322640897..321c361792a4 100644 --- a/source/isaaclab/isaaclab/utils/types.py +++ b/source/isaaclab/isaaclab/utils/types.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,10 +7,11 @@ from __future__ import annotations -import torch from collections.abc import Sequence from dataclasses import dataclass +import torch + @dataclass class ArticulationActions: diff --git a/source/isaaclab/isaaclab/utils/version.py b/source/isaaclab/isaaclab/utils/version.py new file mode 100644 index 000000000000..358a5550aa1c --- /dev/null +++ b/source/isaaclab/isaaclab/utils/version.py @@ -0,0 +1,94 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utility functions for versioning.""" + +from __future__ import annotations + +import functools + +from packaging.version import Version + + +@functools.lru_cache(maxsize=1) +def get_isaac_sim_version() -> Version: + """Get the Isaac Sim version as a Version object, cached for performance. + + This function wraps :func:`isaacsim.core.version.get_version()` and caches the result + to avoid repeated file I/O operations. The underlying Isaac Sim function reads from + a file each time it's called, which can be slow when called frequently. + + Returns: + A :class:`packaging.version.Version` object representing the Isaac Sim version. + This object supports rich comparison operators (<, <=, >, >=, ==, !=). + + Example: + >>> from isaaclab.utils import get_isaac_sim_version + >>> from packaging.version import Version + >>> + >>> isaac_version = get_isaac_sim_version() + >>> print(isaac_version) + 5.0.0 + >>> + >>> # Natural version comparisons + >>> if isaac_version >= Version("5.0.0"): + ... print("Using Isaac Sim 5.0 or later") + >>> + >>> # Access components + >>> print(isaac_version.major, isaac_version.minor, isaac_version.micro) + 5 0 0 + """ + from isaacsim.core.version import get_version + + version_tuple = get_version() + # version_tuple[2] = major (year), [3] = minor (release), [4] = micro (patch) + return Version(f"{version_tuple[2]}.{version_tuple[3]}.{version_tuple[4]}") + + +def compare_versions(v1: str, v2: str) -> int: + """Compare two version strings and return the comparison result. + + The version strings are expected to be in the format "x.y.z" where x, y, + and z are integers. The version strings are compared lexicographically. + + .. note:: + This function is provided for backward compatibility. For new code, + prefer using :class:`packaging.version.Version` objects directly with + comparison operators (``<``, ``<=``, ``>``, ``>=``, ``==``, ``!=``). + + Args: + v1: The first version string. + v2: The second version string. + + Returns: + An integer indicating the comparison result: + + - :attr:`1` if v1 is greater + - :attr:`-1` if v2 is greater + - :attr:`0` if v1 and v2 are equal + + Example: + >>> from isaaclab.utils import compare_versions + >>> compare_versions("5.0.0", "4.5.0") + 1 + >>> compare_versions("4.5.0", "5.0.0") + -1 + >>> compare_versions("5.0.0", "5.0.0") + 0 + >>> + >>> # Better: use Version objects directly + >>> from packaging.version import Version + >>> Version("5.0.0") > Version("4.5.0") + True + """ + ver1 = Version(v1) + ver2 = Version(v2) + + if ver1 > ver2: + return 1 + elif ver1 < ver2: + return -1 + else: + return 0 diff --git a/source/isaaclab/isaaclab/utils/warp/__init__.py b/source/isaaclab/isaaclab/utils/warp/__init__.py index 35b4e389b99f..12c9a20f8bb3 100644 --- a/source/isaaclab/isaaclab/utils/warp/__init__.py +++ b/source/isaaclab/isaaclab/utils/warp/__init__.py @@ -1,8 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Sub-module containing operations based on warp.""" -from .ops import convert_to_warp_mesh, raycast_mesh +from . import fabric # noqa: F401 +from .ops import convert_to_warp_mesh, raycast_dynamic_meshes, raycast_mesh, raycast_single_mesh diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py new file mode 100644 index 000000000000..3fc42ff94236 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -0,0 +1,207 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# pyright: ignore +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # noqa: E501 +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Warp kernels for GPU-accelerated Fabric operations.""" + +from typing import TYPE_CHECKING, Any + +import warp as wp + +if TYPE_CHECKING: + FabricArrayUInt32 = Any + FabricArrayMat44d = Any + ArrayUInt32 = Any + ArrayUInt32_1d = Any + ArrayFloat32_2d = Any +else: + FabricArrayUInt32 = wp.fabricarray(dtype=wp.uint32) + FabricArrayMat44d = wp.fabricarray(dtype=wp.mat44d) + ArrayUInt32 = wp.array(ndim=1, dtype=wp.uint32) + ArrayUInt32_1d = wp.array(dtype=wp.uint32) + ArrayFloat32_2d = wp.array(ndim=2, dtype=wp.float32) + + +@wp.kernel(enable_backward=False) +def set_view_to_fabric_array(fabric_to_view: FabricArrayUInt32, view_to_fabric: ArrayUInt32): + """Create bidirectional mapping from view indices to fabric indices.""" + fabric_idx = int(wp.tid()) + view_idx = int(fabric_to_view[fabric_idx]) + view_to_fabric[view_idx] = wp.uint32(fabric_idx) + + +@wp.kernel(enable_backward=False) +def arange_k(a: ArrayUInt32_1d): + """Fill array with sequential indices.""" + tid = int(wp.tid()) + a[tid] = wp.uint32(tid) + + +@wp.kernel(enable_backward=False) +def decompose_fabric_transformation_matrix_to_warp_arrays( + fabric_matrices: FabricArrayMat44d, + array_positions: ArrayFloat32_2d, + array_orientations: ArrayFloat32_2d, + array_scales: ArrayFloat32_2d, + indices: ArrayUInt32, + mapping: ArrayUInt32, +): + """Decompose Fabric transformation matrices into position, orientation, and scale arrays. + + This kernel extracts transform components from Fabric's omni:fabric:worldMatrix attribute + and stores them in separate arrays. It handles the quaternion convention conversion + (Warp uses xyzw, Isaac Lab uses wxyz). + + Args: + fabric_matrices: Fabric array containing 4x4 transformation matrices + array_positions: Output array for positions (N, 3) + array_orientations: Output array for quaternions in wxyz format (N, 4) + array_scales: Output array for scales (N, 3) + indices: View indices to process + mapping: Mapping from view indices to fabric indices + """ + # Thread index is the output array index (0, 1, 2, ... for N elements) + output_index = wp.tid() + # View index is which prim in the view we're reading from (e.g., 0, 2, 4 from indices=[0,2,4]) + view_index = indices[output_index] + # Fabric index is where that prim is stored in Fabric + fabric_index = mapping[view_index] + + # decompose transform matrix + position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[fabric_index])) + # extract position - write to sequential output array (check if array has elements) + if array_positions.shape[0] > 0: + array_positions[output_index, 0] = position[0] + array_positions[output_index, 1] = position[1] + array_positions[output_index, 2] = position[2] + # extract orientation (Warp quaternion is xyzw, convert to wxyz) + if array_orientations.shape[0] > 0: + array_orientations[output_index, 0] = rotation[3] # w + array_orientations[output_index, 1] = rotation[0] # x + array_orientations[output_index, 2] = rotation[1] # y + array_orientations[output_index, 3] = rotation[2] # z + # extract scale + if array_scales.shape[0] > 0: + array_scales[output_index, 0] = scale[0] + array_scales[output_index, 1] = scale[1] + array_scales[output_index, 2] = scale[2] + + +@wp.kernel(enable_backward=False) +def compose_fabric_transformation_matrix_from_warp_arrays( + fabric_matrices: FabricArrayMat44d, + array_positions: ArrayFloat32_2d, + array_orientations: ArrayFloat32_2d, + array_scales: ArrayFloat32_2d, + broadcast_positions: bool, + broadcast_orientations: bool, + broadcast_scales: bool, + indices: ArrayUInt32, + mapping: ArrayUInt32, +): + """Compose Fabric transformation matrices from position, orientation, and scale arrays. + + This kernel updates Fabric's omni:fabric:worldMatrix attribute from separate component arrays. + It handles the quaternion convention conversion (Isaac Lab uses wxyz, Warp uses xyzw). + + After calling this kernel, IFabricHierarchy.updateWorldXforms() should be called to + propagate changes through the hierarchy. + + Args: + fabric_matrices: Fabric array containing 4x4 transformation matrices to update + array_positions: Input array for positions (N, 3) or None + array_orientations: Input array for quaternions in wxyz format (N, 4) or None + array_scales: Input array for scales (N, 3) or None + broadcast_positions: If True, use first position for all prims + broadcast_orientations: If True, use first orientation for all prims + broadcast_scales: If True, use first scale for all prims + indices: View indices to process + mapping: Mapping from view indices to fabric indices + """ + i = wp.tid() + # resolve fabric index + fabric_index = mapping[indices[i]] + # decompose current transform matrix to get existing values + position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[fabric_index])) + # update position (check if array has elements, not just if it exists) + if array_positions.shape[0] > 0: + if broadcast_positions: + index = 0 + else: + index = i + position[0] = array_positions[index, 0] + position[1] = array_positions[index, 1] + position[2] = array_positions[index, 2] + # update orientation (convert from wxyz to xyzw for Warp) + if array_orientations.shape[0] > 0: + if broadcast_orientations: + index = 0 + else: + index = i + rotation[0] = array_orientations[index, 1] # x + rotation[1] = array_orientations[index, 2] # y + rotation[2] = array_orientations[index, 3] # z + rotation[3] = array_orientations[index, 0] # w + # update scale + if array_scales.shape[0] > 0: + if broadcast_scales: + index = 0 + else: + index = i + scale[0] = array_scales[index, 0] + scale[1] = array_scales[index, 1] + scale[2] = array_scales[index, 2] + # set transform matrix (need transpose for column-major ordering) + # Using transform_compose as wp.matrix() is deprecated + fabric_matrices[fabric_index] = wp.mat44d( # type: ignore[arg-type] + wp.transpose(wp.transform_compose(position, rotation, scale)) # type: ignore[arg-type] + ) + + +@wp.func +def _decompose_transformation_matrix(m: Any): # -> tuple[wp.vec3f, wp.quatf, wp.vec3f] + """Decompose a 4x4 transformation matrix into position, orientation, and scale. + + Args: + m: 4x4 transformation matrix + + Returns: + Tuple of (position, rotation_quaternion, scale) + """ + # extract position from translation column + position = wp.vec3f(m[3, 0], m[3, 1], m[3, 2]) + # extract rotation matrix components + r00, r01, r02 = m[0, 0], m[0, 1], m[0, 2] + r10, r11, r12 = m[1, 0], m[1, 1], m[1, 2] + r20, r21, r22 = m[2, 0], m[2, 1], m[2, 2] + # get scale magnitudes from column vectors + sx = wp.sqrt(r00 * r00 + r01 * r01 + r02 * r02) + sy = wp.sqrt(r10 * r10 + r11 * r11 + r12 * r12) + sz = wp.sqrt(r20 * r20 + r21 * r21 + r22 * r22) + # normalize rotation matrix components by scale + if sx != 0.0: + r00 /= sx + r01 /= sx + r02 /= sx + if sy != 0.0: + r10 /= sy + r11 /= sy + r12 /= sy + if sz != 0.0: + r20 /= sz + r21 /= sz + r22 /= sz + # extract rotation quaternion from normalized rotation matrix + rotation = wp.quat_from_matrix( # type: ignore[arg-type] + wp.transpose(wp.mat33f(r00, r01, r02, r10, r11, r12, r20, r21, r22)) # type: ignore[arg-type] + ) + # extract scale + scale = wp.vec3f(sx, sy, sz) + return position, rotation, scale diff --git a/source/isaaclab/isaaclab/utils/warp/kernels.py b/source/isaaclab/isaaclab/utils/warp/kernels.py index 882282fdff26..cf56e34ed45a 100644 --- a/source/isaaclab/isaaclab/utils/warp/kernels.py +++ b/source/isaaclab/isaaclab/utils/warp/kernels.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,6 +9,10 @@ import warp as wp +## +# Raycasting +## + @wp.kernel(enable_backward=False) def raycast_mesh_kernel( @@ -75,6 +79,171 @@ def raycast_mesh_kernel( ray_face_id[tid] = f +@wp.kernel(enable_backward=False) +def raycast_static_meshes_kernel( + mesh: wp.array2d(dtype=wp.uint64), + ray_starts: wp.array2d(dtype=wp.vec3), + ray_directions: wp.array2d(dtype=wp.vec3), + ray_hits: wp.array2d(dtype=wp.vec3), + ray_distance: wp.array2d(dtype=wp.float32), + ray_normal: wp.array2d(dtype=wp.vec3), + ray_face_id: wp.array2d(dtype=wp.int32), + ray_mesh_id: wp.array2d(dtype=wp.int16), + max_dist: float = 1e6, + return_normal: int = False, + return_face_id: int = False, + return_mesh_id: int = False, +): + """Performs ray-casting against multiple static meshes. + + This function performs ray-casting against the given meshes using the provided ray start positions + and directions. The resulting ray hit positions are stored in the :obj:`ray_hits` array. + + 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. + + .. 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. + + This kernel differs from the :meth:`raycast_dynamic_meshes_kernel` in that it does not take into + account the mesh's position and rotation. This kernel is useful for ray-casting against static meshes + that are not expected to move. + + 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 (B, N, 3). + ray_directions: The input ray directions. Shape is (B, N, 3). + ray_hits: The output ray hit positions. Shape is (B, N, 3). + ray_distance: The output ray hit distances. Shape is (B, N,), if ``return_distance`` is True. Otherwise, + this array is not used. + ray_normal: The output ray hit normals. Shape is (B, N, 3), if ``return_normal`` is True. Otherwise, + this array is not used. + ray_face_id: The output ray hit face ids. Shape is (B, N,), if ``return_face_id`` is True. Otherwise, + this array is not used. + ray_mesh_id: The output ray hit mesh ids. Shape is (B, N,), if ``return_mesh_id`` is True. Otherwise, + this array is not used. + max_dist: The maximum ray-cast distance. Defaults to 1e6. + 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. + return_mesh_id: Whether to return the mesh id. Defaults to False. + """ + # get the thread id + tid_mesh_id, tid_env, tid_ray = wp.tid() + + direction = ray_directions[tid_env, tid_ray] + start_pos = ray_starts[tid_env, tid_ray] + + # ray cast against the mesh and store the hit position + mesh_query_ray_t = wp.mesh_query_ray(mesh[tid_env, tid_mesh_id], start_pos, direction, max_dist) + + # if the ray hit, store the hit data + if mesh_query_ray_t.result: + wp.atomic_min(ray_distance, tid_env, tid_ray, mesh_query_ray_t.t) + # check if hit distance is less than the current hit distance, only then update the memory + # TODO, in theory we could use the output of atomic_min to avoid the non-thread safe next comparison + # however, warp atomic_min is returning the wrong values on gpu currently. + # FIXME https://github.com/NVIDIA/warp/issues/1058 + if mesh_query_ray_t.t == ray_distance[tid_env, tid_ray]: + # convert back to world space and update the hit data + ray_hits[tid_env, tid_ray] = start_pos + mesh_query_ray_t.t * direction + + # update the normal and face id if requested + if return_normal == 1: + ray_normal[tid_env, tid_ray] = mesh_query_ray_t.normal + if return_face_id == 1: + ray_face_id[tid_env, tid_ray] = mesh_query_ray_t.face + if return_mesh_id == 1: + ray_mesh_id[tid_env, tid_ray] = wp.int16(tid_mesh_id) + + +@wp.kernel(enable_backward=False) +def raycast_dynamic_meshes_kernel( + mesh: wp.array2d(dtype=wp.uint64), + ray_starts: wp.array2d(dtype=wp.vec3), + ray_directions: wp.array2d(dtype=wp.vec3), + ray_hits: wp.array2d(dtype=wp.vec3), + ray_distance: wp.array2d(dtype=wp.float32), + ray_normal: wp.array2d(dtype=wp.vec3), + ray_face_id: wp.array2d(dtype=wp.int32), + ray_mesh_id: wp.array2d(dtype=wp.int16), + mesh_positions: wp.array2d(dtype=wp.vec3), + mesh_rotations: wp.array2d(dtype=wp.quat), + max_dist: float = 1e6, + return_normal: int = False, + return_face_id: int = False, + return_mesh_id: int = False, +): + """Performs ray-casting against multiple meshes. + + This function performs ray-casting against the given meshes using the provided ray start positions + and directions. The resulting ray hit positions are stored in the :obj:`ray_hits` array. + + 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. + + + 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. + + All arguments are expected to be batched with the first dimension (B, batch) being the number of envs + and the second dimension (N, num_rays) being the number of rays. For Meshes, W is the number of meshes. + + 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 (B, N, 3). + ray_directions: The input ray directions. Shape is (B, N, 3). + ray_hits: The output ray hit positions. Shape is (B, N, 3). + ray_distance: The output ray hit distances. Shape is (B, N,), if ``return_distance`` is True. Otherwise, + this array is not used. + ray_normal: The output ray hit normals. Shape is (B, N, 3), if ``return_normal`` is True. Otherwise, + this array is not used. + ray_face_id: The output ray hit face ids. Shape is (B, N,), if ``return_face_id`` is True. Otherwise, + this array is not used. + ray_mesh_id: The output ray hit mesh ids. Shape is (B, N,), if ``return_mesh_id`` is True. Otherwise, + this array is not used. + mesh_positions: The input mesh positions in world frame. Shape is (W, 3). + mesh_rotations: The input mesh rotations in world frame. Shape is (W, 4). + max_dist: The maximum ray-cast distance. Defaults to 1e6. + 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. + return_mesh_id: Whether to return the mesh id. Defaults to False. + """ + # get the thread id + tid_mesh_id, tid_env, tid_ray = wp.tid() + + mesh_pose = wp.transform(mesh_positions[tid_env, tid_mesh_id], mesh_rotations[tid_env, tid_mesh_id]) + mesh_pose_inv = wp.transform_inverse(mesh_pose) + direction = wp.transform_vector(mesh_pose_inv, ray_directions[tid_env, tid_ray]) + start_pos = wp.transform_point(mesh_pose_inv, ray_starts[tid_env, tid_ray]) + + # ray cast against the mesh and store the hit position + mesh_query_ray_t = wp.mesh_query_ray(mesh[tid_env, tid_mesh_id], start_pos, direction, max_dist) + # if the ray hit, store the hit data + if mesh_query_ray_t.result: + wp.atomic_min(ray_distance, tid_env, tid_ray, mesh_query_ray_t.t) + # check if hit distance is less than the current hit distance, only then update the memory + # TODO, in theory we could use the output of atomic_min to avoid the non-thread safe next comparison + # however, warp atomic_min is returning the wrong values on gpu currently. + # FIXME https://github.com/NVIDIA/warp/issues/1058 + if mesh_query_ray_t.t == ray_distance[tid_env, tid_ray]: + # convert back to world space and update the hit data + hit_pos = start_pos + mesh_query_ray_t.t * direction + ray_hits[tid_env, tid_ray] = wp.transform_point(mesh_pose, hit_pos) + + # update the normal and face id if requested + if return_normal == 1: + n = wp.transform_vector(mesh_pose, mesh_query_ray_t.normal) + ray_normal[tid_env, tid_ray] = n + if return_face_id == 1: + ray_face_id[tid_env, tid_ray] = mesh_query_ray_t.face + if return_mesh_id == 1: + ray_mesh_id[tid_env, tid_ray] = wp.int16(tid_mesh_id) + + @wp.kernel(enable_backward=False) def reshape_tiled_image( tiled_image_buffer: Any, @@ -131,3 +300,170 @@ def reshape_tiled_image( reshape_tiled_image, {"tiled_image_buffer": wp.array(dtype=wp.float32), "batched_image": wp.array(dtype=wp.float32, ndim=4)}, ) + +## +# Wrench Composer +## + + +@wp.func +def cast_to_link_frame(position: wp.vec3f, link_position: wp.vec3f, is_global: bool) -> wp.vec3f: + """Casts a position to the link frame of the body. + + Args: + position: The position to cast. + link_position: The link frame position. + is_global: Whether the position is in the global frame. + + Returns: + The position in the link frame of the body. + """ + if is_global: + return position - link_position + else: + return position + + +@wp.func +def cast_force_to_link_frame(force: wp.vec3f, link_quat: wp.quatf, is_global: bool) -> wp.vec3f: + """Casts a force to the link frame of the body. + + Args: + force: The force to cast. + link_quat: The link frame quaternion. + is_global: Whether the force is applied in the global frame. + Returns: + The force in the link frame of the body. + """ + if is_global: + return wp.quat_rotate_inv(link_quat, force) + else: + return force + + +@wp.func +def cast_torque_to_link_frame(torque: wp.vec3f, link_quat: wp.quatf, is_global: bool) -> wp.vec3f: + """Casts a torque to the link frame of the body. + + Args: + torque: The torque to cast. + link_quat: The link frame quaternion. + is_global: Whether the torque is applied in the global frame. + + Returns: + The torque in the link frame of the body. + """ + if is_global: + return wp.quat_rotate_inv(link_quat, torque) + else: + return torque + + +@wp.kernel +def add_forces_and_torques_at_position( + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + forces: wp.array2d(dtype=wp.vec3f), + torques: wp.array2d(dtype=wp.vec3f), + positions: wp.array2d(dtype=wp.vec3f), + link_positions: wp.array2d(dtype=wp.vec3f), + link_quaternions: wp.array2d(dtype=wp.quatf), + composed_forces_b: wp.array2d(dtype=wp.vec3f), + composed_torques_b: wp.array2d(dtype=wp.vec3f), + is_global: bool, +): + """Adds forces and torques to the composed force and torque at the user-provided positions. + When is_global is False, the user-provided positions are offsetting the application of the force relatively to the + link frame of the body. When is_global is True, the user-provided positions are the global positions of the force + application. + + Args: + env_ids: The environment ids. + body_ids: The body ids. + forces: The forces. + torques: The torques. + positions: The positions. + link_positions: The link frame positions. + link_quaternions: The link frame quaternions. + composed_forces_b: The composed forces. + composed_torques_b: The composed torques. + is_global: Whether the forces and torques are applied in the global frame. + """ + # get the thread id + tid_env, tid_body = wp.tid() + + # add the forces to the composed force, if the positions are provided, also adds a torque to the composed torque. + if forces: + # add the forces to the composed force + composed_forces_b[env_ids[tid_env], body_ids[tid_body]] += cast_force_to_link_frame( + forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global + ) + # if there is a position offset, add a torque to the composed torque. + if positions: + composed_torques_b[env_ids[tid_env], body_ids[tid_body]] += wp.skew( + cast_to_link_frame( + positions[tid_env, tid_body], link_positions[env_ids[tid_env], body_ids[tid_body]], is_global + ) + ) @ cast_force_to_link_frame( + forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global + ) + if torques: + composed_torques_b[env_ids[tid_env], body_ids[tid_body]] += cast_torque_to_link_frame( + torques[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global + ) + + +@wp.kernel +def set_forces_and_torques_at_position( + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + forces: wp.array2d(dtype=wp.vec3f), + torques: wp.array2d(dtype=wp.vec3f), + positions: wp.array2d(dtype=wp.vec3f), + link_positions: wp.array2d(dtype=wp.vec3f), + link_quaternions: wp.array2d(dtype=wp.quatf), + composed_forces_b: wp.array2d(dtype=wp.vec3f), + composed_torques_b: wp.array2d(dtype=wp.vec3f), + is_global: bool, +): + """Sets forces and torques to the composed force and torque at the user-provided positions. + When is_global is False, the user-provided positions are offsetting the application of the force relatively + to the link frame of the body. When is_global is True, the user-provided positions are the global positions + of the force application. + + Args: + env_ids: The environment ids. + body_ids: The body ids. + forces: The forces. + torques: The torques. + positions: The positions. + link_positions: The link frame positions. + link_quaternions: The link frame quaternions. + composed_forces_b: The composed forces. + composed_torques_b: The composed torques. + is_global: Whether the forces and torques are applied in the global frame. + """ + # get the thread id + tid_env, tid_body = wp.tid() + + # set the torques to the composed torque + if torques: + composed_torques_b[env_ids[tid_env], body_ids[tid_body]] = cast_torque_to_link_frame( + torques[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global + ) + # set the forces to the composed force, if the positions are provided, adds a torque to the composed torque + # from the force at that position. + if forces: + # set the forces to the composed force + composed_forces_b[env_ids[tid_env], body_ids[tid_body]] = cast_force_to_link_frame( + forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global + ) + # if there is a position offset, set the torque from the force at that position. + if positions: + composed_torques_b[env_ids[tid_env], body_ids[tid_body]] = wp.skew( + cast_to_link_frame( + positions[tid_env, tid_body], link_positions[env_ids[tid_env], body_ids[tid_body]], is_global + ) + ) @ cast_force_to_link_frame( + forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global + ) diff --git a/source/isaaclab/isaaclab/utils/warp/ops.py b/source/isaaclab/isaaclab/utils/warp/ops.py index a1817b507c1f..f7cc8ac01def 100644 --- a/source/isaaclab/isaaclab/utils/warp/ops.py +++ b/source/isaaclab/isaaclab/utils/warp/ops.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,7 +10,6 @@ import numpy as np import torch - import warp as wp # disable warp module initialization messages @@ -18,6 +17,8 @@ # initialize the warp module wp.init() +from isaaclab.utils.math import convert_quat + from . import kernels @@ -127,6 +128,257 @@ def raycast_mesh( return ray_hits.to(device).view(shape), ray_distance, ray_normal, ray_face_id +def raycast_single_mesh( + ray_starts: torch.Tensor, + ray_directions: torch.Tensor, + mesh_id: int, + 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 :attr:`ray_starts` and :attr:`ray_directions`, and :attr:`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 (B, N, 3). + ray_directions: The ray directions for each ray. Shape (B, N, 3). + mesh_id: The warp mesh id 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 (B, N, 3). + The returned tensor contains :obj:`float('inf')` for missed hits. + The ray hit distance. Shape (B, 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 (B, 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 (B, N,). + Will only return if :attr:`return_face_id` is True else returns None. + The returned tensor contains :obj:`int(-1)` for missed hits. + """ + # cast mesh id into array + mesh_ids = wp.array2d( + [[mesh_id] for _ in range(ray_starts.shape[0])], dtype=wp.uint64, device=wp.device_from_torch(ray_starts.device) + ) + ray_hits, ray_distance, ray_normal, ray_face_id, ray_mesh_id = raycast_dynamic_meshes( + ray_starts=ray_starts, + ray_directions=ray_directions, + mesh_ids_wp=mesh_ids, + max_dist=max_dist, + return_distance=return_distance, + return_normal=return_normal, + return_face_id=return_face_id, + ) + + return ray_hits, ray_distance, ray_normal, ray_face_id + + +def raycast_dynamic_meshes( + ray_starts: torch.Tensor, + ray_directions: torch.Tensor, + mesh_ids_wp: wp.Array, + mesh_positions_w: torch.Tensor | None = None, + mesh_orientations_w: torch.Tensor | None = None, + max_dist: float = 1e6, + return_distance: bool = False, + return_normal: bool = False, + return_face_id: bool = False, + return_mesh_id: bool = False, +) -> tuple[torch.Tensor, torch.Tensor | None, torch.Tensor | None, torch.Tensor | None, torch.Tensor | None]: + """Performs ray-casting against multiple, dynamic meshes. + + Note that the :attr:`ray_starts` and :attr:`ray_directions`, and :attr:`ray_hits` should have compatible shapes + and data types to ensure proper execution. Additionally, they all must be in the same frame. + + If mesh positions and rotations are provided, they need to have to have the same shape as the + number of meshes. + + Args: + ray_starts: The starting position of the rays. Shape (B, N, 3). + ray_directions: The ray directions for each ray. Shape (B, N, 3). + mesh_ids_wp: The warp mesh ids to ray-cast against. Length (B, M). + mesh_positions_w: The world positions of the meshes. Shape (B, M, 3). + mesh_orientations_w: The world orientation as quaternion (wxyz) format. Shape (B, M, 4). + 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. + return_mesh_id: Whether to return the mesh id of the mesh face the ray hits. Defaults to False. + NOTE: the type of the returned tensor is torch.int16, so you can't have more than 32767 meshes. + + Returns: + The ray hit position. Shape (B, N, 3). + The returned tensor contains :obj:`float('inf')` for missed hits. + The ray hit distance. Shape (B, 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 (B, 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 (B, N,). + Will only return if :attr:`return_face_id` is True else returns None. + The returned tensor contains :obj:`int(-1)` for missed hits. + The ray hit mesh id. Shape (B, N,). + Will only return if :attr:`return_mesh_id` is True else returns None. + The returned tensor contains :obj:`-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_ids_wp.device) + n_meshes = mesh_ids_wp.shape[1] + + n_envs = ray_starts.shape[0] + n_rays_per_env = ray_starts.shape[1] + + # reshape the tensors + ray_starts = ray_starts.to(torch_device).view(n_envs, n_rays_per_env, 3).contiguous() + ray_directions = ray_directions.to(torch_device).view(n_envs, n_rays_per_env, 3).contiguous() + + # create output tensor for the ray hits + ray_hits = torch.full((n_envs, n_rays_per_env, 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) + # required to check if a closer hit is reported, returned only if return_distance is true + ray_distance = torch.full( + ( + n_envs, + n_rays_per_env, + ), + float("inf"), + device=torch_device, + ).contiguous() + ray_distance_wp = wp.from_torch(ray_distance, dtype=wp.float32) + + if return_normal: + ray_normal = torch.full((n_envs, n_rays_per_env, 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, 1), dtype=wp.vec3, device=torch_device) + + if return_face_id: + ray_face_id = torch.ones( + ( + n_envs, + n_rays_per_env, + ), + 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, 1), dtype=wp.int32, device=torch_device) + + if return_mesh_id: + ray_mesh_id = -torch.ones((n_envs, n_rays_per_env), dtype=torch.int16, device=torch_device).contiguous() + ray_mesh_id_wp = wp.from_torch(ray_mesh_id, dtype=wp.int16) + else: + ray_mesh_id = None + ray_mesh_id_wp = wp.empty((1, 1), dtype=wp.int16, device=torch_device) + + ## + # Call the warp kernels + ### + if mesh_positions_w is None and mesh_orientations_w is None: + # Static mesh case, no need to pass in positions and rotations. + # launch the warp kernel + wp.launch( + kernel=kernels.raycast_static_meshes_kernel, + dim=[n_meshes, n_envs, n_rays_per_env], + inputs=[ + mesh_ids_wp, + ray_starts_wp, + ray_directions_wp, + ray_hits_wp, + ray_distance_wp, + ray_normal_wp, + ray_face_id_wp, + ray_mesh_id_wp, + float(max_dist), + int(return_normal), + int(return_face_id), + int(return_mesh_id), + ], + device=torch_device, + ) + else: + # dynamic mesh case + if mesh_positions_w is None: + mesh_positions_wp_w = wp.zeros((n_envs, n_meshes), dtype=wp.vec3, device=torch_device) + else: + mesh_positions_w = mesh_positions_w.to(torch_device).view(n_envs, n_meshes, 3).contiguous() + mesh_positions_wp_w = wp.from_torch(mesh_positions_w, dtype=wp.vec3) + + if mesh_orientations_w is None: + # Note (zrene): This is a little bit ugly, since it requires to initialize torch memory first + # But I couldn't find a better way to initialize a quaternion identity in warp + # wp.zeros(1, dtype=wp.quat, device=torch_device) gives all zero quaternion + quat_identity = torch.tensor([0, 0, 0, 1], dtype=torch.float32, device=torch_device).repeat( + n_envs, n_meshes, 1 + ) + mesh_quat_wp_w = wp.from_torch(quat_identity, dtype=wp.quat) + else: + mesh_orientations_w = convert_quat( + mesh_orientations_w.to(dtype=torch.float32, device=torch_device), "xyzw" + ).contiguous() + mesh_quat_wp_w = wp.from_torch(mesh_orientations_w, dtype=wp.quat) + + # launch the warp kernel + wp.launch( + kernel=kernels.raycast_dynamic_meshes_kernel, + dim=[n_meshes, n_envs, n_rays_per_env], + inputs=[ + mesh_ids_wp, + ray_starts_wp, + ray_directions_wp, + ray_hits_wp, + ray_distance_wp, + ray_normal_wp, + ray_face_id_wp, + ray_mesh_id_wp, + mesh_positions_wp_w, + mesh_quat_wp_w, + float(max_dist), + int(return_normal), + int(return_face_id), + int(return_mesh_id), + ], + device=torch_device, + ) + ## + # Cleanup and convert back to torch tensors + ## + + # 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[:2]) + 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[:2]) + if return_mesh_id: + ray_mesh_id = ray_mesh_id.to(device).view(shape[:2]) + + return ray_hits.to(device).view(shape), ray_distance, ray_normal, ray_face_id, ray_mesh_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. diff --git a/source/isaaclab/isaaclab/utils/wrench_composer.py b/source/isaaclab/isaaclab/utils/wrench_composer.py new file mode 100644 index 000000000000..8bd42f81e9e6 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/wrench_composer.py @@ -0,0 +1,349 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +import warp as wp + +from isaaclab.utils.math import convert_quat +from isaaclab.utils.warp.kernels import add_forces_and_torques_at_position, set_forces_and_torques_at_position + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection + + +class WrenchComposer: + def __init__(self, asset: Articulation | RigidObject | RigidObjectCollection) -> None: + """Wrench composer. + + This class is used to compose forces and torques at the body's link frame. + It can compose global wrenches and local wrenches. The result is always in the link frame of the body. + + Args: + asset: Asset to use. Defaults to None. + """ + self.num_envs = asset.num_instances + # Avoid isinstance to prevent circular import issues, use attribute presence instead. + if hasattr(asset, "num_bodies"): + self.num_bodies = asset.num_bodies + else: + self.num_bodies = asset.num_objects + self.device = asset.device + self._asset = asset + self._active = False + + # Avoid isinstance here due to potential circular import issues; check by attribute presence instead. + if hasattr(self._asset.data, "body_link_pos_w") and hasattr(self._asset.data, "body_link_quat_w"): + self._get_link_position_fn = lambda a=self._asset: a.data.body_link_pos_w[..., :3] + self._get_link_quaternion_fn = lambda a=self._asset: a.data.body_link_quat_w[..., :4] + elif hasattr(self._asset.data, "object_link_pos_w") and hasattr(self._asset.data, "object_link_quat_w"): + self._get_link_position_fn = lambda a=self._asset: a.data.object_link_pos_w[..., :3] + self._get_link_quaternion_fn = lambda a=self._asset: a.data.object_link_quat_w[..., :4] + else: + raise ValueError(f"Unsupported asset type: {self._asset.__class__.__name__}") + + # Create buffers + self._composed_force_b = wp.zeros((self.num_envs, self.num_bodies), dtype=wp.vec3f, device=self.device) + self._composed_torque_b = wp.zeros((self.num_envs, self.num_bodies), dtype=wp.vec3f, device=self.device) + self._ALL_ENV_INDICES_WP = wp.from_torch( + torch.arange(self.num_envs, dtype=torch.int32, device=self.device), dtype=wp.int32 + ) + self._ALL_BODY_INDICES_WP = wp.from_torch( + torch.arange(self.num_bodies, dtype=torch.int32, device=self.device), dtype=wp.int32 + ) + + # Pinning the composed force and torque to the torch tensor to avoid copying the data to the torch tensor + self._composed_force_b_torch = wp.to_torch(self._composed_force_b) + self._composed_torque_b_torch = wp.to_torch(self._composed_torque_b) + # Pinning the environment and body indices to the torch tensor to allow for slicing. + self._ALL_ENV_INDICES_TORCH = wp.to_torch(self._ALL_ENV_INDICES_WP) + self._ALL_BODY_INDICES_TORCH = wp.to_torch(self._ALL_BODY_INDICES_WP) + + # Flag to check if the link poses have been updated. + self._link_poses_updated = False + + @property + def active(self) -> bool: + """Whether the wrench composer is active.""" + return self._active + + @property + def composed_force(self) -> wp.array: + """Composed force at the body's link frame. + + .. note:: If some of the forces are applied in the global frame, the composed force will be in the link frame + of the body. + + Returns: + wp.array: Composed force at the body's link frame. (num_envs, num_bodies, 3) + """ + return self._composed_force_b + + @property + def composed_torque(self) -> wp.array: + """Composed torque at the body's link frame. + + .. note:: If some of the torques are applied in the global frame, the composed torque will be in the link frame + of the body. + + Returns: + wp.array: Composed torque at the body's link frame. (num_envs, num_bodies, 3) + """ + return self._composed_torque_b + + @property + def composed_force_as_torch(self) -> torch.Tensor: + """Composed force at the body's link frame as torch tensor. + + .. note:: If some of the forces are applied in the global frame, the composed force will be in the link frame + of the body. + + Returns: + torch.Tensor: Composed force at the body's link frame. (num_envs, num_bodies, 3) + """ + return self._composed_force_b_torch + + @property + def composed_torque_as_torch(self) -> torch.Tensor: + """Composed torque at the body's link frame as torch tensor. + + .. note:: If some of the torques are applied in the global frame, the composed torque will be in the link frame + of the body. + + Returns: + torch.Tensor: Composed torque at the body's link frame. (num_envs, num_bodies, 3) + """ + return self._composed_torque_b_torch + + def add_forces_and_torques( + self, + forces: wp.array | torch.Tensor | None = None, + torques: wp.array | torch.Tensor | None = None, + positions: wp.array | torch.Tensor | None = None, + body_ids: wp.array | torch.Tensor | None = None, + env_ids: wp.array | torch.Tensor | None = None, + is_global: bool = False, + ): + """Add forces and torques to the composed force and torque. + + Composed force and torque are the sum of all the forces and torques applied to the body. + It can compose global wrenches and local wrenches. The result is always in the link frame of the body. + + The user can provide any combination of forces, torques, and positions. + + .. note:: Users may want to call `reset` function after every simulation step to ensure no force is carried + over to the next step. However, this may not necessary if the user calls `set_forces_and_torques` function + instead of `add_forces_and_torques`. + + Args: + forces: Forces. (num_envs, num_bodies, 3). Defaults to None. + torques: Torques. (num_envs, num_bodies, 3). Defaults to None. + positions: Positions. (num_envs, num_bodies, 3). Defaults to None. + body_ids: Body ids. (num_envs, num_bodies). Defaults to None (all bodies). + env_ids: Environment ids. (num_envs). Defaults to None (all environments). + is_global: Whether the forces and torques are applied in the global frame. Defaults to False. + + Raises: + ValueError: If the type of the input is not supported. + ValueError: If the input is a slice and it is not None. + """ + # Resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES_WP + elif isinstance(env_ids, torch.Tensor): + env_ids = wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) + elif isinstance(env_ids, list): + env_ids = wp.array(env_ids, dtype=wp.int32, device=self.device) + elif isinstance(env_ids, slice): + if env_ids == slice(None): + env_ids = self._ALL_ENV_INDICES_WP + else: + raise ValueError(f"Doesn't support slice input for env_ids: {env_ids}") + # -- body_ids + if body_ids is None: + body_ids = self._ALL_BODY_INDICES_WP + elif isinstance(body_ids, torch.Tensor): + body_ids = wp.from_torch(body_ids.to(torch.int32), dtype=wp.int32) + elif isinstance(body_ids, list): + body_ids = wp.array(body_ids, dtype=wp.int32, device=self.device) + elif isinstance(body_ids, slice): + if body_ids == slice(None): + body_ids = self._ALL_BODY_INDICES_WP + else: + raise ValueError(f"Doesn't support slice input for body_ids: {body_ids}") + + # Resolve remaining inputs + # -- don't launch if no forces or torques are provided + if forces is None and torques is None: + return + if isinstance(forces, torch.Tensor): + forces = wp.from_torch(forces, dtype=wp.vec3f) + if isinstance(torques, torch.Tensor): + torques = wp.from_torch(torques, dtype=wp.vec3f) + if isinstance(positions, torch.Tensor): + positions = wp.from_torch(positions, dtype=wp.vec3f) + + # Get the link positions and quaternions + if not self._link_poses_updated: + self._link_positions = wp.from_torch(self._get_link_position_fn().clone(), dtype=wp.vec3f) + self._link_quaternions = wp.from_torch( + convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf + ) + self._link_poses_updated = True + + # Set the active flag to true + self._active = True + + wp.launch( + add_forces_and_torques_at_position, + dim=(env_ids.shape[0], body_ids.shape[0]), + inputs=[ + env_ids, + body_ids, + forces, + torques, + positions, + self._link_positions, + self._link_quaternions, + self._composed_force_b, + self._composed_torque_b, + is_global, + ], + device=self.device, + ) + + def set_forces_and_torques( + self, + forces: wp.array | torch.Tensor | None = None, + torques: wp.array | torch.Tensor | None = None, + positions: wp.array | torch.Tensor | None = None, + body_ids: wp.array | torch.Tensor | None = None, + env_ids: wp.array | torch.Tensor | None = None, + is_global: bool = False, + ): + """Set forces and torques to the composed force and torque. + + Composed force and torque are the sum of all the forces and torques applied to the body. + It can compose global wrenches and local wrenches. The result is always in the link frame of the body. + + The user can provide any combination of forces, torques, and positions. + + Args: + forces: Forces. (num_envs, num_bodies, 3). Defaults to None. + torques: Torques. (num_envs, num_bodies, 3). Defaults to None. + positions: Positions. (num_envs, num_bodies, 3). Defaults to None. + body_ids: Body ids. (num_envs, num_bodies). Defaults to None (all bodies). + env_ids: Environment ids. (num_envs). Defaults to None (all environments). + is_global: Whether the forces and torques are applied in the global frame. Defaults to False. + + Raises: + ValueError: If the type of the input is not supported. + ValueError: If the input is a slice and it is not None. + """ + # Resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES_WP + elif isinstance(env_ids, torch.Tensor): + env_ids = wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) + elif isinstance(env_ids, list): + env_ids = wp.array(env_ids, dtype=wp.int32, device=self.device) + elif isinstance(env_ids, slice): + if env_ids == slice(None): + env_ids = self._ALL_ENV_INDICES_WP + else: + raise ValueError(f"Doesn't support slice input for env_ids: {env_ids}") + # -- body_ids + if body_ids is None: + body_ids = self._ALL_BODY_INDICES_WP + elif isinstance(body_ids, torch.Tensor): + body_ids = wp.from_torch(body_ids.to(torch.int32), dtype=wp.int32) + elif isinstance(body_ids, list): + body_ids = wp.array(body_ids, dtype=wp.int32, device=self.device) + elif isinstance(body_ids, slice): + if body_ids == slice(None): + body_ids = self._ALL_BODY_INDICES_WP + else: + raise ValueError(f"Doesn't support slice input for body_ids: {body_ids}") + # Resolve remaining inputs + # -- don't launch if no forces or torques are provided + if forces is None and torques is None: + return + if forces is None: + forces = wp.empty((0, 0), dtype=wp.vec3f, device=self.device) + elif isinstance(forces, torch.Tensor): + forces = wp.from_torch(forces, dtype=wp.vec3f) + if torques is None: + torques = wp.empty((0, 0), dtype=wp.vec3f, device=self.device) + elif isinstance(torques, torch.Tensor): + torques = wp.from_torch(torques, dtype=wp.vec3f) + if positions is None: + positions = wp.empty((0, 0), dtype=wp.vec3f, device=self.device) + elif isinstance(positions, torch.Tensor): + positions = wp.from_torch(positions, dtype=wp.vec3f) + + # Get the link positions and quaternions + if not self._link_poses_updated: + self._link_positions = wp.from_torch(self._get_link_position_fn().clone(), dtype=wp.vec3f) + self._link_quaternions = wp.from_torch( + convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf + ) + self._link_poses_updated = True + + # Set the active flag to true + self._active = True + + wp.launch( + set_forces_and_torques_at_position, + dim=(env_ids.shape[0], body_ids.shape[0]), + inputs=[ + env_ids, + body_ids, + forces, + torques, + positions, + self._link_positions, + self._link_quaternions, + self._composed_force_b, + self._composed_torque_b, + is_global, + ], + device=self.device, + ) + + def reset(self, env_ids: wp.array | torch.Tensor | None = None): + """Reset the composed force and torque. + + This function will reset the composed force and torque to zero. + It will also make sure the link positions and quaternions are updated in the next call of the + `add_forces_and_torques` or `set_forces_and_torques` functions. + + .. note:: This function should be called after every simulation step / reset to ensure no force is carried + over to the next step. + """ + if env_ids is None: + self._composed_force_b.zero_() + self._composed_torque_b.zero_() + self._active = False + else: + indices = env_ids + if isinstance(env_ids, torch.Tensor): + indices = wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) + elif isinstance(env_ids, list): + indices = wp.array(env_ids, dtype=wp.int32, device=self.device) + elif isinstance(env_ids, slice): + if env_ids == slice(None): + indices = self._ALL_ENV_INDICES_WP + else: + indices = env_ids + + self._composed_force_b[indices].zero_() + self._composed_torque_b[indices].zero_() + + self._link_poses_updated = False diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index 86e9e4e88cb4..369bf5bdf8a9 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,8 +6,8 @@ """Installation script for the 'isaaclab' python package.""" import os -import toml +import toml from setuptools import setup # Obtain the extension data from the extension.toml file @@ -19,28 +19,46 @@ INSTALL_REQUIRES = [ # generic "numpy<2", - "torch==2.5.1", - "onnx==1.16.1", # 1.16.2 throws access violation on Windows + "torch>=2.7", + "onnx>=1.18.0", # 1.16.2 throws access violation on Windows "prettytable==3.3.0", "toml", # devices "hidapi==0.14.0.post2", # reinforcement learning - "gymnasium", + "gymnasium==1.2.1", # procedural-generation "trimesh", "pyglet<2", # image processing - "transformers", + "transformers==4.57.6", "einops", # needed for transformers, doesn't always auto-install "warp-lang", # make sure this is consistent with isaac sim version - "pillow==11.0.0", + "pillow==11.3.0", # livestream - "starlette==0.46.0", + "starlette==0.49.1", + # testing + "pytest", + "pytest-mock", + "junitparser", + "flatdict==4.0.0", + "flaky", + "packaging", +] + +# 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 ({SUPPORTED_ARCHS_ARM})", + f"daqp==0.7.2 ; 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 ({SUPPORTED_ARCHS})", ] -PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu118"] +PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu128"] # Installation operation setup( @@ -60,7 +78,10 @@ classifiers=[ "Natural Language :: English", "Programming Language :: Python :: 3.10", + "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/actuators/test_dc_motor.py b/source/isaaclab/test/actuators/test_dc_motor.py new file mode 100644 index 000000000000..26ad2de0526d --- /dev/null +++ b/source/isaaclab/test/actuators/test_dc_motor.py @@ -0,0 +1,192 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.app import AppLauncher + +HEADLESS = True + +# if not AppLauncher.instance(): +simulation_app = AppLauncher(headless=HEADLESS).app + +"""Rest of imports follows""" + +import pytest +import torch + +from isaaclab.actuators import DCMotorCfg + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_dc_motor_init_minimum(num_envs, num_joints, device): + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = 200 + damping = 10 + effort_limit = 60.0 + saturation_effort = 100.0 + velocity_limit = 50 + + actuator_cfg = DCMotorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + effort_limit=effort_limit, + saturation_effort=saturation_effort, + velocity_limit=velocity_limit, + ) + # assume Articulation class: + # - finds joints (names and ids) associate with the provided joint_names_expr + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + ) + + # check device and shape + torch.testing.assert_close(actuator.computed_effort, torch.zeros(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.applied_effort, torch.zeros(num_envs, num_joints, device=device)) + torch.testing.assert_close( + actuator.effort_limit, + effort_limit * torch.ones(num_envs, num_joints, device=device), + ) + torch.testing.assert_close( + actuator.velocity_limit, velocity_limit * torch.ones(num_envs, num_joints, device=device) + ) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda", "cpu"]) +@pytest.mark.parametrize("test_point", range(20)) +def test_dc_motor_clip(num_envs, num_joints, device, test_point): + r"""Test the computation of the dc motor actuator 4 quadrant torque speed curve. + torque_speed_pairs of interest: + + 0 - fully inside torque speed curve and effort limit (quadrant 1) + 1 - greater than effort limit but under torque-speed curve (quadrant 1) + 2 - greater than effort limit and outside torque-speed curve (quadrant 1) + 3 - less than effort limit but outside torque speed curve (quadrant 1) + 4 - less than effort limit but outside torque speed curve and outside corner velocity(quadrant 4) + 5 - fully inside torque speed curve and effort limit (quadrant 4) + 6 - fully outside torque speed curve and -effort limit (quadrant 4) + 7 - fully inside torque speed curve, outside -effort limit, and inside corner velocity (quadrant 4) + 8 - fully inside torque speed curves, outside -effort limit, and outside corner velocity (quadrant 4) + 9 - less than effort limit but outside torque speed curve and inside corner velocity (quadrant 4) + e - effort_limit + s - saturation_effort + v - velocity_limit + c - corner velocity + \ - torque-speed linear boundary between v and s + each torque_speed_point will be tested in quadrant 3 and 4 + =========================================================== + Torque + \ (+) + \ | + Q2 s Q1 + | \ 2 + \ | 1 \ + c ---------------------e-----\ + \ | \ + \ | 0 \ 3 + \ | \ + (-)-----------v -------------o-------------v --------------(+) Speed + \ | \ 9 4 + \ | 5 \ + \ | \ + \ -----e---------------------c + \ | \ 6 + Q3 \ | 7 Q4 \ + \s \ + |\ 8 \ + (-) \ + ============================================================ + """ + effort_lim = 60 + saturation_effort = 100.0 + velocity_limit = 50 + + torque_speed_pairs = [ + (30.0, 10.0), # 0 + (70.0, 10.0), # 1 + (80.0, 40.0), # 2 + (30.0, 40.0), # 3 + (-20.0, 90.0), # 4 + (-30.0, 10.0), # 5 + (-80.0, 110.0), # 6 + (-80.0, 50.0), # 7 + (-120.0, 90.0), # 8 + (-10.0, 70.0), # 9 + (-30.0, -10.0), # -0 + (-70.0, -10.0), # -1 + (-80.0, -40.0), # -2 + (-30.0, -40.0), # -3 + (20.0, -90.0), # -4 + (30.0, -10.0), # -5 + (80.0, -110.0), # -6 + (80.0, -50.0), # -7 + (120.0, -90.0), # -8 + (10.0, -70.0), # -9 + ] + expected_clipped_effort = [ + 30.0, # 0 + 60.0, # 1 + 20.0, # 2 + 20.0, # 3 + -60.0, # 4 + -30.0, # 5 + -60.0, # 6 + -60.0, # 7 + -60.0, # 8 + -40.0, # 9 + -30.0, # -0 + -60.0, # -1 + -20, # -2 + -20, # -3 + 60.0, # -4 + 30.0, # -5 + 60.0, # -6 + 60.0, # -7 + 60.0, # -8 + 40.0, # -9 + ] + + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = 200 + damping = 10 + actuator_cfg = DCMotorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + effort_limit=effort_lim, + velocity_limit=velocity_limit, + saturation_effort=saturation_effort, + ) + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + ) + + ts = torque_speed_pairs[test_point] + torque = ts[0] + speed = ts[1] + actuator._joint_vel[:] = speed * torch.ones(num_envs, num_joints, device=device) + effort = torque * torch.ones(num_envs, num_joints, device=device) + clipped_effort = actuator._clip_effort(effort) + torch.testing.assert_close( + expected_clipped_effort[test_point] * torch.ones(num_envs, num_joints, device=device), + clipped_effort, + ) diff --git a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py new file mode 100644 index 000000000000..d77e5e12c34a --- /dev/null +++ b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py @@ -0,0 +1,271 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.app import AppLauncher + +HEADLESS = True + +# if not AppLauncher.instance(): +simulation_app = AppLauncher(headless=HEADLESS).app + +"""Rest of imports follows""" + +import pytest +import torch + +from isaaclab.actuators import IdealPDActuatorCfg +from isaaclab.utils.types import ArticulationActions + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("usd_default", [False, True]) +def test_ideal_pd_actuator_init_minimum(num_envs, num_joints, device, usd_default): + """Test initialization of ideal pd actuator with minimum configuration.""" + + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = None if usd_default else 200 + damping = None if usd_default else 10 + friction = None if usd_default else 0.1 + armature = None if usd_default else 0.2 + + actuator_cfg = IdealPDActuatorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + armature=armature, + friction=friction, + ) + # assume Articulation class: + # - finds joints (names and ids) associate with the provided joint_names_expr + + # faux usd defaults + stiffness_default = 300 + damping_default = 20 + friction_default = 0.0 + armature_default = 0.0 + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=stiffness_default, + damping=damping_default, + friction=friction_default, + armature=armature_default, + ) + + # check initialized actuator + assert actuator.is_implicit_model is False + # check device and shape + torch.testing.assert_close(actuator.computed_effort, torch.zeros(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.applied_effort, torch.zeros(num_envs, num_joints, device=device)) + + torch.testing.assert_close(actuator.effort_limit, torch.inf * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close( + actuator.effort_limit_sim, actuator._DEFAULT_MAX_EFFORT_SIM * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close(actuator.velocity_limit, torch.inf * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.velocity_limit_sim, torch.inf * torch.ones(num_envs, num_joints, device=device)) + + if not usd_default: + torch.testing.assert_close(actuator.stiffness, stiffness * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.damping, damping * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.armature, armature * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.friction, friction * torch.ones(num_envs, num_joints, device=device)) + else: + torch.testing.assert_close( + actuator.stiffness, stiffness_default * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close(actuator.damping, damping_default * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close( + actuator.armature, armature_default * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.friction, friction_default * torch.ones(num_envs, num_joints, device=device) + ) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("effort_lim", [None, 300]) +@pytest.mark.parametrize("effort_lim_sim", [None, 400]) +def test_ideal_pd_actuator_init_effort_limits(num_envs, num_joints, device, effort_lim, effort_lim_sim): + """Test initialization of ideal pd actuator with effort limits.""" + # used as a standin for the usd default value read in by articulation. + # This value should not be propagated for ideal pd actuators + effort_lim_default = 5000 + + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + + actuator_cfg = IdealPDActuatorCfg( + joint_names_expr=joint_names, + stiffness=200, + damping=10, + effort_limit=effort_lim, + effort_limit_sim=effort_lim_sim, + ) + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + effort_limit=effort_lim_default, + ) + + if effort_lim is not None and effort_lim_sim is None: + effort_lim_expected = effort_lim + effort_lim_sim_expected = actuator._DEFAULT_MAX_EFFORT_SIM + + elif effort_lim is None and effort_lim_sim is not None: + effort_lim_expected = effort_lim_default + effort_lim_sim_expected = effort_lim_sim + + elif effort_lim is None and effort_lim_sim is None: + effort_lim_expected = effort_lim_default + effort_lim_sim_expected = actuator._DEFAULT_MAX_EFFORT_SIM + + elif effort_lim is not None and effort_lim_sim is not None: + effort_lim_expected = effort_lim + effort_lim_sim_expected = effort_lim_sim + + torch.testing.assert_close( + actuator.effort_limit, effort_lim_expected * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.effort_limit_sim, effort_lim_sim_expected * torch.ones(num_envs, num_joints, device=device) + ) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("velocity_lim", [None, 300]) +@pytest.mark.parametrize("velocity_lim_sim", [None, 400]) +def test_ideal_pd_actuator_init_velocity_limits(num_envs, num_joints, device, velocity_lim, velocity_lim_sim): + """Test initialization of ideal pd actuator with velocity limits. + + Note Ideal PD actuator does not use velocity limits in computation, they are passed to physics via articulations. + """ + velocity_limit_default = 1000 + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + + actuator_cfg = IdealPDActuatorCfg( + joint_names_expr=joint_names, + stiffness=200, + damping=10, + velocity_limit=velocity_lim, + velocity_limit_sim=velocity_lim_sim, + ) + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + velocity_limit=velocity_limit_default, + ) + if velocity_lim is not None and velocity_lim_sim is None: + vel_lim_expected = velocity_lim + vel_lim_sim_expected = velocity_limit_default + elif velocity_lim is None and velocity_lim_sim is not None: + vel_lim_expected = velocity_lim_sim + vel_lim_sim_expected = velocity_lim_sim + elif velocity_lim is None and velocity_lim_sim is None: + vel_lim_expected = velocity_limit_default + vel_lim_sim_expected = velocity_limit_default + elif velocity_lim is not None and velocity_lim_sim is not None: + vel_lim_expected = velocity_lim + vel_lim_sim_expected = velocity_lim_sim + + torch.testing.assert_close( + actuator.velocity_limit, vel_lim_expected * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.velocity_limit_sim, vel_lim_sim_expected * torch.ones(num_envs, num_joints, device=device) + ) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("effort_lim", [None, 300]) +def test_ideal_pd_compute(num_envs, num_joints, device, effort_lim): + """Test the computation of the ideal pd actuator.""" + + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = 200 + damping = 10 + actuator_cfg = IdealPDActuatorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + effort_limit=effort_lim, + ) + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + ) + desired_pos = 10.0 + desired_vel = 0.1 + measured_joint_pos = 1.0 + measured_joint_vel = -0.1 + + desired_control_action = ArticulationActions() + desired_control_action.joint_positions = desired_pos * torch.ones(num_envs, num_joints, device=device) + desired_control_action.joint_velocities = desired_vel * torch.ones(num_envs, num_joints, device=device) + desired_control_action.joint_efforts = torch.zeros(num_envs, num_joints, device=device) + + expected_comp_joint_effort = stiffness * (desired_pos - measured_joint_pos) + damping * ( + desired_vel - measured_joint_vel + ) + + computed_control_action = actuator.compute( + desired_control_action, + measured_joint_pos * torch.ones(num_envs, num_joints, device=device), + measured_joint_vel * torch.ones(num_envs, num_joints, device=device), + ) + + torch.testing.assert_close( + expected_comp_joint_effort * torch.ones(num_envs, num_joints, device=device), actuator.computed_effort + ) + + if effort_lim is None: + torch.testing.assert_close( + expected_comp_joint_effort * torch.ones(num_envs, num_joints, device=device), actuator.applied_effort + ) + else: + torch.testing.assert_close( + effort_lim * torch.ones(num_envs, num_joints, device=device), actuator.applied_effort + ) + torch.testing.assert_close( + actuator.applied_effort, + computed_control_action.joint_efforts, + ) + + +if __name__ == "__main__": + pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab/test/actuators/test_implicit_actuator.py b/source/isaaclab/test/actuators/test_implicit_actuator.py new file mode 100644 index 000000000000..c4a26f2f9533 --- /dev/null +++ b/source/isaaclab/test/actuators/test_implicit_actuator.py @@ -0,0 +1,242 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.app import AppLauncher + +HEADLESS = True + +# if not AppLauncher.instance(): +simulation_app = AppLauncher(headless=HEADLESS).app + +"""Rest of imports follows""" + +import pytest +import torch + +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sim import build_simulation_context + + +@pytest.fixture +def sim(request): + """Create simulation context with the specified device.""" + device = request.getfixturevalue("device") + with build_simulation_context(device=device) as sim: + sim._app_control_on_stop_handle = None + yield sim + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("usd_default", [False, True]) +def test_implicit_actuator_init_minimum(sim, num_envs, num_joints, device, usd_default): + """Test initialization of implicit actuator with minimum configuration.""" + + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = None if usd_default else 200 + damping = None if usd_default else 10 + friction = None if usd_default else 0.1 + armature = None if usd_default else 0.2 + + actuator_cfg = ImplicitActuatorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + armature=armature, + friction=friction, + ) + # assume Articulation class: + # - finds joints (names and ids) associate with the provided joint_names_expr + + # faux usd defaults + stiffness_default = 300 + damping_default = 20 + friction_default = 0.0 + armature_default = 0.0 + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=stiffness_default, + damping=damping_default, + friction=friction_default, + armature=armature_default, + ) + + # check initialized actuator + assert actuator.is_implicit_model is True + # check device and shape + torch.testing.assert_close(actuator.computed_effort, torch.zeros(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.applied_effort, torch.zeros(num_envs, num_joints, device=device)) + + torch.testing.assert_close(actuator.effort_limit, torch.inf * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.effort_limit_sim, torch.inf * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.velocity_limit, torch.inf * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.velocity_limit_sim, torch.inf * torch.ones(num_envs, num_joints, device=device)) + + if not usd_default: + torch.testing.assert_close(actuator.stiffness, stiffness * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.damping, damping * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.armature, armature * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.friction, friction * torch.ones(num_envs, num_joints, device=device)) + else: + torch.testing.assert_close( + actuator.stiffness, stiffness_default * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close(actuator.damping, damping_default * torch.ones(num_envs, num_joints, device=device)) + torch.testing.assert_close( + actuator.armature, armature_default * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.friction, friction_default * torch.ones(num_envs, num_joints, device=device) + ) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("effort_lim", [None, 300, 200]) +@pytest.mark.parametrize("effort_lim_sim", [None, 400, 200]) +def test_implicit_actuator_init_effort_limits(sim, num_envs, num_joints, device, effort_lim, effort_lim_sim): + """Test initialization of implicit actuator with effort limits.""" + effort_limit_default = 5000 + + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + + actuator_cfg = ImplicitActuatorCfg( + joint_names_expr=joint_names, + stiffness=200, + damping=10, + effort_limit=effort_lim, + effort_limit_sim=effort_lim_sim, + ) + + if effort_lim is not None and effort_lim_sim is not None and effort_lim != effort_lim_sim: + with pytest.raises(ValueError): + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + effort_limit=effort_limit_default, + ) + else: + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + effort_limit=effort_limit_default, + ) + if effort_lim is not None and effort_lim_sim is None: + assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit + effort_lim_expected = effort_lim + effort_lim_sim_expected = effort_lim + + elif effort_lim is None and effort_lim_sim is not None: + assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit + effort_lim_expected = effort_lim_sim + effort_lim_sim_expected = effort_lim_sim + + elif effort_lim is None and effort_lim_sim is None: + assert actuator.cfg.effort_limit_sim is None + assert actuator.cfg.effort_limit is None + effort_lim_expected = effort_limit_default + effort_lim_sim_expected = effort_limit_default + + elif effort_lim is not None and effort_lim_sim is not None: + assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit + effort_lim_expected = effort_lim + effort_lim_sim_expected = effort_lim_sim + + torch.testing.assert_close( + actuator.effort_limit, effort_lim_expected * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.effort_limit_sim, effort_lim_sim_expected * torch.ones(num_envs, num_joints, device=device) + ) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("velocity_lim", [None, 300, 200]) +@pytest.mark.parametrize("velocity_lim_sim", [None, 400, 200]) +def test_implicit_actuator_init_velocity_limits(sim, num_envs, num_joints, device, velocity_lim, velocity_lim_sim): + """Test initialization of implicit actuator with velocity limits. + + Note implicit actuators do no use velocity limits in computation, they are passed to physics via articulations. + """ + velocity_limit_default = 1000 + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + + actuator_cfg = ImplicitActuatorCfg( + joint_names_expr=joint_names, + stiffness=200, + damping=10, + velocity_limit=velocity_lim, + velocity_limit_sim=velocity_lim_sim, + ) + + if velocity_lim is not None and velocity_lim_sim is not None and velocity_lim != velocity_lim_sim: + with pytest.raises(ValueError): + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + velocity_limit=velocity_limit_default, + ) + else: + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + velocity_limit=velocity_limit_default, + ) + if velocity_lim is not None and velocity_lim_sim is None: + assert actuator.cfg.velocity_limit is None + vel_lim_expected = velocity_limit_default + elif velocity_lim is None and velocity_lim_sim is not None: + assert actuator.cfg.velocity_limit == actuator.cfg.velocity_limit_sim + vel_lim_expected = velocity_lim_sim + elif velocity_lim is None and velocity_lim_sim is None: + assert actuator.cfg.velocity_limit is None + assert actuator.cfg.velocity_limit_sim is None + vel_lim_expected = velocity_limit_default + else: + assert actuator.cfg.velocity_limit == actuator.cfg.velocity_limit_sim + vel_lim_expected = velocity_lim_sim + + torch.testing.assert_close( + actuator.velocity_limit, vel_lim_expected * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.velocity_limit_sim, vel_lim_expected * torch.ones(num_envs, num_joints, device=device) + ) + + +if __name__ == "__main__": + pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab/test/app/test_argparser_launch.py b/source/isaaclab/test/app/test_argparser_launch.py index 6c38bcbb1699..683409dd190c 100644 --- a/source/isaaclab/test/app/test_argparser_launch.py +++ b/source/isaaclab/test/app/test_argparser_launch.py @@ -1,47 +1,42 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import argparse -import unittest -from unittest import mock - -from isaaclab.app import AppLauncher, run_tests - - -class TestAppLauncher(unittest.TestCase): - """Test launching of the simulation app using AppLauncher.""" - - @mock.patch("argparse.ArgumentParser.parse_args", return_value=argparse.Namespace(livestream=1)) - def test_livestream_launch_with_argparser(self, mock_args): - """Test launching with argparser arguments.""" - # create argparser - parser = argparse.ArgumentParser() - # add app launcher arguments - AppLauncher.add_app_launcher_args(parser) - # check that argparser has the mandatory arguments - for name in AppLauncher._APPLAUNCHER_CFG_INFO: - self.assertTrue(parser._option_string_actions[f"--{name}"]) - # parse args - mock_args = parser.parse_args() - # everything defaults to None - app = AppLauncher(mock_args).app - - # import settings - import carb - - # acquire settings interface - carb_settings_iface = carb.settings.get_settings() - # check settings - # -- no-gui mode - self.assertEqual(carb_settings_iface.get("/app/window/enabled"), False) - # -- livestream - self.assertEqual(carb_settings_iface.get("/app/livestream/enabled"), True) - - # close the app on exit - app.close() - - -if __name__ == "__main__": - run_tests() + +import pytest + +from isaaclab.app import AppLauncher + + +@pytest.mark.usefixtures("mocker") +def test_livestream_launch_with_argparser(mocker): + """Test launching with argparser arguments.""" + # Mock the parse_args method + mocker.patch("argparse.ArgumentParser.parse_args", return_value=argparse.Namespace(livestream=1, headless=True)) + # create argparser + parser = argparse.ArgumentParser() + # add app launcher arguments + AppLauncher.add_app_launcher_args(parser) + # check that argparser has the mandatory arguments + for name in AppLauncher._APPLAUNCHER_CFG_INFO: + assert parser._option_string_actions[f"--{name}"] + # parse args + mock_args = parser.parse_args() + # everything defaults to None + app = AppLauncher(mock_args).app + + # import settings + import carb + + # acquire settings interface + carb_settings_iface = carb.settings.get_settings() + # check settings + # -- no-gui mode + assert carb_settings_iface.get("/app/window/enabled") is False + # -- livestream + assert carb_settings_iface.get("/app/livestream/enabled") is True + + # close the app on exit + app.close() diff --git a/source/isaaclab/test/app/test_env_var_launch.py b/source/isaaclab/test/app/test_env_var_launch.py index bf7a34d9a85a..9ec07f932749 100644 --- a/source/isaaclab/test/app/test_env_var_launch.py +++ b/source/isaaclab/test/app/test_env_var_launch.py @@ -1,38 +1,33 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import os -import unittest -from isaaclab.app import AppLauncher, run_tests +import pytest +from isaaclab.app import AppLauncher -class TestAppLauncher(unittest.TestCase): - """Test launching of the simulation app using AppLauncher.""" - def test_livestream_launch_with_env_var(self): - """Test launching with no-keyword args but environment variables.""" - # manually set the settings as well to make sure they are set correctly - os.environ["LIVESTREAM"] = "1" - # everything defaults to None - app = AppLauncher().app +@pytest.mark.usefixtures("mocker") +def test_livestream_launch_with_env_vars(mocker): + """Test launching with environment variables.""" + # Mock the environment variables + mocker.patch.dict(os.environ, {"LIVESTREAM": "1", "HEADLESS": "1"}) + # everything defaults to None + app = AppLauncher().app - # import settings - import carb + # import settings + import carb - # acquire settings interface - carb_settings_iface = carb.settings.get_settings() - # check settings - # -- no-gui mode - self.assertEqual(carb_settings_iface.get("/app/window/enabled"), False) - # -- livestream - self.assertEqual(carb_settings_iface.get("/app/livestream/enabled"), True) + # acquire settings interface + carb_settings_iface = carb.settings.get_settings() + # check settings + # -- no-gui mode + assert carb_settings_iface.get("/app/window/enabled") is False + # -- livestream + assert carb_settings_iface.get("/app/livestream/enabled") is True - # close the app on exit - app.close() - - -if __name__ == "__main__": - run_tests() + # close the app on exit + app.close() diff --git a/source/isaaclab/test/app/test_kwarg_launch.py b/source/isaaclab/test/app/test_kwarg_launch.py index 0b5af98cbb8b..b2781637b722 100644 --- a/source/isaaclab/test/app/test_kwarg_launch.py +++ b/source/isaaclab/test/app/test_kwarg_launch.py @@ -1,35 +1,29 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import unittest +import pytest -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher -class TestAppLauncher(unittest.TestCase): - """Test launching of the simulation app using AppLauncher.""" +@pytest.mark.usefixtures("mocker") +def test_livestream_launch_with_kwargs(mocker): + """Test launching with keyword arguments.""" + # everything defaults to None + app = AppLauncher(headless=True, livestream=1).app - def test_livestream_launch_with_kwarg(self): - """Test launching with headless and livestreaming arguments.""" - # everything defaults to None - app = AppLauncher(headless=True, livestream=1).app + # import settings + import carb - # import settings - import carb + # acquire settings interface + carb_settings_iface = carb.settings.get_settings() + # check settings + # -- no-gui mode + assert carb_settings_iface.get("/app/window/enabled") is False + # -- livestream + assert carb_settings_iface.get("/app/livestream/enabled") is True - # acquire settings interface - carb_settings_iface = carb.settings.get_settings() - # check settings - # -- no-gui mode - self.assertEqual(carb_settings_iface.get("/app/window/enabled"), False) - # -- livestream - self.assertEqual(carb_settings_iface.get("/app/livestream/enabled"), True) - - # close the app on exit - app.close() - - -if __name__ == "__main__": - run_tests() + # close the app on exit + app.close() 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 000000000000..eb8544b995cc --- /dev/null +++ b/source/isaaclab/test/app/test_non_headless_launch.py @@ -0,0 +1,65 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +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/check_external_force.py b/source/isaaclab/test/assets/check_external_force.py index 4e1131fe5f93..f038b907a1f0 100644 --- a/source/isaaclab/test/assets/check_external_force.py +++ b/source/isaaclab/test/assets/check_external_force.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -106,7 +106,7 @@ def main(): robot.write_joint_state_to_sim(joint_pos, joint_vel) robot.reset() # apply force - robot.set_external_force_and_torque( + robot.permanent_wrench_composer.set_forces_and_torques( external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids ) # reset command diff --git a/source/isaaclab/test/assets/check_fixed_base_assets.py b/source/isaaclab/test/assets/check_fixed_base_assets.py index 5a4482326f76..c62c5a3334da 100644 --- a/source/isaaclab/test/assets/check_fixed_base_assets.py +++ b/source/isaaclab/test/assets/check_fixed_base_assets.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,8 +35,6 @@ import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils from isaaclab.assets import Articulation @@ -75,12 +73,12 @@ def design_scene() -> tuple[dict, list[list[float]]]: origins = define_origins(num_origins=4, spacing=2.0) # Origin 1 with Franka Panda - prim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) + sim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0]) # -- Robot franka = Articulation(FRANKA_PANDA_CFG.replace(prim_path="/World/Origin1/Robot")) # Origin 2 with Anymal C - prim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) + sim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1]) # -- Robot robot_cfg = ANYMAL_C_CFG.replace(prim_path="/World/Origin2/Robot") robot_cfg.spawn.articulation_props.fix_root_link = True diff --git a/source/isaaclab/test/assets/check_ridgeback_franka.py b/source/isaaclab/test/assets/check_ridgeback_franka.py index 35794056e4af..724cd8b2a080 100644 --- a/source/isaaclab/test/assets/check_ridgeback_franka.py +++ b/source/isaaclab/test/assets/check_ridgeback_franka.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/assets/test_articulation.py b/source/isaaclab/test/assets/test_articulation.py index 97b06740fb99..9a983ab34c1d 100644 --- a/source/isaaclab/test/assets/test_articulation.py +++ b/source/isaaclab/test/assets/test_articulation.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,29 +8,30 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher HEADLESS = True # launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import ctypes -import torch -import unittest -import isaacsim.core.utils.prims as prim_utils +import pytest +import torch 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, IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit +from isaaclab.managers import SceneEntityCfg from isaaclab.sim import build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.version import get_isaac_sim_version ## # Pre-defined configs @@ -72,7 +73,9 @@ def generate_articulation_cfg( """ if articulation_type == "humanoid": articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Humanoid/humanoid_instanceable.usd"), + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/Humanoid/humanoid_instanceable.usd" + ), init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.34)), actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, ) @@ -86,7 +89,7 @@ def generate_articulation_cfg( articulation_cfg = ArticulationCfg( # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), ), actuators={ @@ -96,16 +99,21 @@ def generate_articulation_cfg( velocity_limit_sim=velocity_limit_sim, effort_limit=effort_limit, velocity_limit=velocity_limit, - stiffness=0.0, - damping=10.0, + stiffness=2000.0, + damping=100.0, ), }, + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.0), + joint_pos=({"RevoluteJoint": 1.5708}), + rot=(0.7071055, 0.7071081, 0, 0), + ), ) elif articulation_type == "single_joint_explicit": # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. articulation_cfg = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), ), actuators={ @@ -120,10 +128,24 @@ def generate_articulation_cfg( ), }, ) + elif articulation_type == "spatial_tendon_test_asset": + # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. + articulation_cfg = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/IsaacLab/Tests/spatial_tendons.usd", + ), + actuators={ + "joint": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=2000.0, + damping=100.0, + ), + }, + ) else: raise ValueError( f"Invalid articulation type: {articulation_type}, valid options are 'humanoid', 'panda', 'anymal'," - " 'shadow_hand', 'single_joint_implicit' or 'single_joint_explicit'." + " 'shadow_hand', 'single_joint_implicit', 'single_joint_explicit' or 'spatial_tendon_test_asset'." ) return articulation_cfg @@ -152,1363 +174,1994 @@ def generate_articulation( # Create Top-level Xforms, one for each articulation for i in range(num_articulations): - prim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) + sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) articulation = Articulation(articulation_cfg.replace(prim_path="/World/Env_.*/Robot")) return articulation, translations -class TestArticulation(unittest.TestCase): - """Test for articulation class.""" +@pytest.fixture +def sim(request): + """Create simulation context with the specified device.""" + device = request.getfixturevalue("device") + if "gravity_enabled" in request.fixturenames: + gravity_enabled = request.getfixturevalue("gravity_enabled") + else: + gravity_enabled = True # default to gravity enabled + if "add_ground_plane" in request.fixturenames: + add_ground_plane = request.getfixturevalue("add_ground_plane") + else: + add_ground_plane = False # default to no ground plane + with build_simulation_context( + device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane + ) as sim: + sim._app_control_on_stop_handle = None + yield sim + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_initialization_floating_base_non_root(sim, num_articulations, device, add_ground_plane): + """Test initialization for a floating-base with articulation root on a rigid body. + + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation is not fixed base + 3. All buffers have correct shapes + 4. The articulation can be simulated + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + articulation_cfg = generate_articulation_cfg(articulation_type="humanoid", stiffness=0.0, damping=0.0) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + + # Check if articulation is initialized + assert articulation.is_initialized + # Check that is fixed base + assert not articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 21) + + # Check some internal physx data for debugging + # -- joint related + assert articulation.root_physx_view.max_dofs == articulation.root_physx_view.shared_metatype.dof_count + # -- link related + assert articulation.root_physx_view.max_links == articulation.root_physx_view.shared_metatype.link_count + # -- link names (check within articulation ordering is correct) + prim_path_body_names = [path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]] + assert prim_path_body_names == articulation.body_names + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert actuator.is_implicit_model == is_implicit_model_cfg + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_initialization_floating_base(sim, num_articulations, device, add_ground_plane): + """Test initialization for a floating-base with articulation root on provided prim path. + + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation is not fixed base + 3. All buffers have correct shapes + 4. The articulation can be simulated + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + articulation_cfg = generate_articulation_cfg(articulation_type="anymal", stiffness=0.0, damping=0.0) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + # Check that floating base + assert not articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 12) + assert articulation.data.default_mass.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.default_inertia.shape == (num_articulations, articulation.num_bodies, 9) + + # Check some internal physx data for debugging + # -- joint related + assert articulation.root_physx_view.max_dofs == articulation.root_physx_view.shared_metatype.dof_count + # -- link related + assert articulation.root_physx_view.max_links == articulation.root_physx_view.shared_metatype.link_count + # -- link names (check within articulation ordering is correct) + prim_path_body_names = [path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]] + assert prim_path_body_names == articulation.body_names + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert actuator.is_implicit_model == is_implicit_model_cfg + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_fixed_base(sim, num_articulations, device): + """Test initialization for fixed base. + + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation is fixed base + 3. All buffers have correct shapes + 4. The articulation maintains its default state + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + articulation_cfg = generate_articulation_cfg(articulation_type="panda") + articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + # Check that fixed base + assert articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 9) + assert articulation.data.default_mass.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.default_inertia.shape == (num_articulations, articulation.num_bodies, 9) + + # Check some internal physx data for debugging + # -- joint related + assert articulation.root_physx_view.max_dofs == articulation.root_physx_view.shared_metatype.dof_count + # -- link related + assert articulation.root_physx_view.max_links == articulation.root_physx_view.shared_metatype.link_count + # -- link names (check within articulation ordering is correct) + prim_path_body_names = [path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]] + assert prim_path_body_names == articulation.body_names + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert actuator.is_implicit_model == is_implicit_model_cfg + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + # check that the root is at the correct state - its default state as it is fixed base + default_root_state = articulation.data.default_root_state.clone() + default_root_state[:, :3] = default_root_state[:, :3] + translations + + torch.testing.assert_close(articulation.data.root_state_w, default_root_state) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_initialization_fixed_base_single_joint(sim, num_articulations, device, add_ground_plane): + """Test initialization for fixed base articulation with a single joint. + + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation is fixed base + 3. All buffers have correct shapes + 4. The articulation maintains its default state + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") + articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + # Check that fixed base + assert articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 1) + assert articulation.data.default_mass.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.default_inertia.shape == (num_articulations, articulation.num_bodies, 9) + + # Check some internal physx data for debugging + # -- joint related + assert articulation.root_physx_view.max_dofs == articulation.root_physx_view.shared_metatype.dof_count + # -- link related + assert articulation.root_physx_view.max_links == articulation.root_physx_view.shared_metatype.link_count + # -- link names (check within articulation ordering is correct) + prim_path_body_names = [path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]] + assert prim_path_body_names == articulation.body_names + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert actuator.is_implicit_model == is_implicit_model_cfg + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + # check that the root is at the correct state - its default state as it is fixed base + default_root_state = articulation.data.default_root_state.clone() + default_root_state[:, :3] = default_root_state[:, :3] + translations + + torch.testing.assert_close(articulation.data.root_state_w, default_root_state) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_hand_with_tendons(sim, num_articulations, device): + """Test initialization for fixed base articulated hand with tendons. + + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation is fixed base + 3. All buffers have correct shapes + 4. The articulation can be simulated + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + articulation_cfg = generate_articulation_cfg(articulation_type="shadow_hand") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + # Check that fixed base + assert articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 24) + assert articulation.data.default_mass.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.default_inertia.shape == (num_articulations, articulation.num_bodies, 9) + + # Check some internal physx data for debugging + # -- joint related + assert articulation.root_physx_view.max_dofs == articulation.root_physx_view.shared_metatype.dof_count + # -- link related + assert articulation.root_physx_view.max_links == articulation.root_physx_view.shared_metatype.link_count + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert actuator.is_implicit_model == is_implicit_model_cfg + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_initialization_floating_base_made_fixed_base(sim, num_articulations, device, add_ground_plane): + """Test initialization for a floating-base articulation made fixed-base using schema properties. + + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation is fixed base after modification + 3. All buffers have correct shapes + 4. The articulation maintains its default state + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="anymal").copy() + # Fix root link by making it kinematic + articulation_cfg.spawn.articulation_props.fix_root_link = True + articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + # Check that is fixed base + assert articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 12) + + # Check some internal physx data for debugging + # -- joint related + assert articulation.root_physx_view.max_dofs == articulation.root_physx_view.shared_metatype.dof_count + # -- link related + assert articulation.root_physx_view.max_links == articulation.root_physx_view.shared_metatype.link_count + # -- link names (check within articulation ordering is correct) + prim_path_body_names = [path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]] + assert prim_path_body_names == articulation.body_names + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + # check that the root is at the correct state - its default state as it is fixed base + default_root_state = articulation.data.default_root_state.clone() + default_root_state[:, :3] = default_root_state[:, :3] + translations + + torch.testing.assert_close(articulation.data.root_state_w, default_root_state) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_initialization_fixed_base_made_floating_base(sim, num_articulations, device, add_ground_plane): + """Test initialization for fixed base made floating-base using schema properties. + + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation is floating base after modification + 3. All buffers have correct shapes + 4. The articulation can be simulated + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="panda") + # Unfix root link by making it non-kinematic + articulation_cfg.spawn.articulation_props.fix_root_link = False + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + # Check that is floating base + assert not articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 9) + + # Check some internal physx data for debugging + # -- joint related + assert articulation.root_physx_view.max_dofs == articulation.root_physx_view.shared_metatype.dof_count + # -- link related + assert articulation.root_physx_view.max_links == articulation.root_physx_view.shared_metatype.link_count + # -- link names (check within articulation ordering is correct) + prim_path_body_names = [path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]] + assert prim_path_body_names == articulation.body_names + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_out_of_range_default_joint_pos(sim, num_articulations, device, add_ground_plane): + """Test that the default joint position from configuration is out of range. + + This test verifies that: + 1. The articulation fails to initialize when joint positions are out of range + 2. The error is properly handled + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + # Create articulation + articulation_cfg = generate_articulation_cfg(articulation_type="panda").copy() + articulation_cfg.init_state.joint_pos = { + "panda_joint1": 10.0, + "panda_joint[2, 4]": -20.0, + } + + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + with pytest.raises(ValueError): + sim.reset() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_out_of_range_default_joint_vel(sim, device): + """Test that the default joint velocity from configuration is out of range. + + This test verifies that: + 1. The articulation fails to initialize when joint velocities are out of range + 2. The error is properly handled + """ + articulation_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Robot") + articulation_cfg.init_state.joint_vel = { + "panda_joint1": 100.0, + "panda_joint[2, 4]": -60.0, + } + articulation = Articulation(articulation_cfg) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + with pytest.raises(ValueError): + sim.reset() + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): + """Test write_joint_limits_to_sim API and when default pos falls outside of the new limits. + + This test verifies that: + 1. Joint limits can be set correctly + 2. Default positions are preserved when setting new limits + 3. Joint limits can be set with indexing + 4. Invalid joint positions are properly handled + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + # Create articulation + articulation_cfg = generate_articulation_cfg(articulation_type="panda") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + + # Get current default joint pos + default_joint_pos = articulation._data.default_joint_pos.clone() + + # Set new joint limits + limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) + limits[..., 0] = (torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0) * -1.0 + limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 + articulation.write_joint_position_limit_to_sim(limits) + + # Check new limits are in place + torch.testing.assert_close(articulation._data.joint_pos_limits, limits) + torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) + + # Set new joint limits with indexing + env_ids = torch.arange(1, device=device) + joint_ids = torch.arange(2, device=device) + limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) + limits[..., 0] = (torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0) * -1.0 + limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0 + articulation.write_joint_position_limit_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids) + + # Check new limits are in place + torch.testing.assert_close(articulation._data.joint_pos_limits[env_ids][:, joint_ids], limits) + torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) + + # Set new joint limits that invalidate default joint pos + limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) + limits[..., 0] = torch.rand(num_articulations, articulation.num_joints, device=device) * -0.1 + limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1 + articulation.write_joint_position_limit_to_sim(limits) + + # Check if all values are within the bounds + within_bounds = (articulation._data.default_joint_pos >= limits[..., 0]) & ( + articulation._data.default_joint_pos <= limits[..., 1] + ) + assert torch.all(within_bounds) + + # Set new joint limits that invalidate default joint pos with indexing + limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) + limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 + limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 + articulation.write_joint_position_limit_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids) + + # Check if all values are within the bounds + within_bounds = (articulation._data.default_joint_pos[env_ids][:, joint_ids] >= limits[..., 0]) & ( + articulation._data.default_joint_pos[env_ids][:, joint_ids] <= limits[..., 1] + ) + assert torch.all(within_bounds) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +def test_joint_effort_limits(sim, num_articulations, device, add_ground_plane): + """Validate joint effort limits via joint_effort_out_of_limit().""" + # Create articulation + articulation_cfg = generate_articulation_cfg(articulation_type="panda") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) + + # Minimal env wrapper exposing scene["robot"] + class _Env: + def __init__(self, art): + self.scene = {"robot": art} + + env = _Env(articulation) + robot_all = SceneEntityCfg(name="robot") + + sim.reset() + assert articulation.is_initialized + + # Case A: no clipping โ†’ should NOT terminate + articulation._data.computed_torque.zero_() + articulation._data.applied_torque.zero_() + out = joint_effort_out_of_limit(env, robot_all) # [N] + assert torch.all(~out) + + # Case B: simulate clipping โ†’ should terminate + articulation._data.computed_torque.fill_(100.0) # pretend controller commanded 100 + articulation._data.applied_torque.fill_(50.0) # pretend actuator clipped to 50 + out = joint_effort_out_of_limit(env, robot_all) # [N] + assert torch.all(out) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_buffer(sim, num_articulations, device): + """Test if external force buffer correctly updates in the force value is zero case. + + This test verifies that: + 1. External forces can be applied correctly + 2. Force buffers are updated properly + 3. Zero forces are handled correctly + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="anymal") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + + # play the simulator + sim.reset() + + # find bodies to apply the force + body_ids, _ = articulation.find_bodies("base") + + # reset root state + root_state = articulation.data.default_root_state.clone() + articulation.write_root_state_to_sim(root_state) + + # reset dof state + joint_pos, joint_vel = ( + articulation.data.default_joint_pos, + articulation.data.default_joint_vel, + ) + articulation.write_joint_state_to_sim(joint_pos, joint_vel) + + # reset articulation + articulation.reset() + + # perform simulation + for step in range(5): + # initiate force tensor + external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) + + if step == 0 or step == 3: + # set a non-zero force + force = 1 + else: + # set a zero force + force = 0 + + # set force value + external_wrench_b[:, :, 0] = force + external_wrench_b[:, :, 3] = force + + # apply force + # TODO: Replace with wrench composer once the deprecation is complete + articulation.set_external_force_and_torque( + external_wrench_b[..., :3], + external_wrench_b[..., 3:], + body_ids=body_ids, + ) + + # check if the articulation's force and torque buffers are correctly updated + for i in range(num_articulations): + assert articulation.permanent_wrench_composer.composed_force_as_torch[i, 0, 0].item() == force + assert articulation.permanent_wrench_composer.composed_torque_as_torch[i, 0, 0].item() == force + + # Check if the instantaneous wrench is correctly added to the permanent wrench + articulation.instantaneous_wrench_composer.add_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + body_ids=body_ids, + ) + + # apply action to the articulation + articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) + articulation.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + articulation.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_on_single_body(sim, num_articulations, device): + """Test application of external force on the base of the articulation. + + This test verifies that: + 1. External forces can be applied to specific bodies + 2. The forces affect the articulation's motion correctly + 3. The articulation responds to the forces as expected + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="anymal") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, _ = articulation.find_bodies("base") + # Sample a large force + external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_b[..., 1] = 1000.0 + + # Now we are ready! + for _ in range(5): + # reset root state + root_state = articulation.data.default_root_state.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_root_velocity_to_sim(root_state[:, 7:]) + # reset dof state + joint_pos, joint_vel = ( + articulation.data.default_joint_pos, + articulation.data.default_joint_vel, + ) + articulation.write_joint_state_to_sim(joint_pos, joint_vel) + # reset articulation + articulation.reset() + # apply force + # TODO: Replace with wrench composer once the deprecation is complete + articulation.set_external_force_and_torque( + external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids + ) + # perform simulation + for _ in range(100): + # apply action to the articulation + articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) + articulation.write_data_to_sim() + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + # check condition that the articulations have fallen down + for i in range(num_articulations): + assert articulation.data.root_pos_w[i, 2].item() < 0.2 + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_on_single_body_at_position(sim, num_articulations, device): + """Test application of external force on the base of the articulation at a given position. + + This test verifies that: + 1. External forces can be applied to specific bodies at a given position + 2. External forces can be applied to specific bodies in the global frame + 3. External forces are calculated and composed correctly + 4. The forces affect the articulation's motion correctly + 5. The articulation responds to the forces as expected + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="anymal") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, _ = articulation.find_bodies("base") + # Sample a large force + external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_b[..., 2] = 500.0 + external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + external_wrench_positions_b[..., 1] = 1.0 + + desired_force = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + desired_force[..., 2] = 1000.0 + desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + desired_torque[..., 0] = 1000.0 + + # Now we are ready! + for i in range(5): + # reset root state + root_state = articulation.data.default_root_state.clone() + root_state[0, 0] = 2.5 # space them apart by 2.5m + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_root_velocity_to_sim(root_state[:, 7:]) + # reset dof state + joint_pos, joint_vel = ( + articulation.data.default_joint_pos, + articulation.data.default_joint_vel, + ) + articulation.write_joint_state_to_sim(joint_pos, joint_vel) + # reset articulation + articulation.reset() + # apply force + is_global = False + + if i % 2 == 0: + body_com_pos_w = articulation.data.body_com_pos_w[:, body_ids, :3] + # is_global = True + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + external_wrench_positions_b += body_com_pos_w + else: + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + + articulation.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + articulation.permanent_wrench_composer.add_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + # perform simulation + for _ in range(100): + # apply action to the articulation + articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) + articulation.write_data_to_sim() + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + # check condition that the articulations have fallen down + for i in range(num_articulations): + assert articulation.data.root_pos_w[i, 2].item() < 0.2 + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_on_multiple_bodies(sim, num_articulations, device): + """Test application of external force on the legs of the articulation. + + This test verifies that: + 1. External forces can be applied to multiple bodies + 2. The forces affect the articulation's motion correctly + 3. The articulation responds to the forces as expected + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="anymal") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, _ = articulation.find_bodies(".*_SHANK") + # Sample a large force + external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_b[..., 1] = 100.0 + + # Now we are ready! + for _ in range(5): + # reset root state + articulation.write_root_pose_to_sim(articulation.data.default_root_state.clone()[:, :7]) + articulation.write_root_velocity_to_sim(articulation.data.default_root_state.clone()[:, 7:]) + # reset dof state + joint_pos, joint_vel = ( + articulation.data.default_joint_pos, + articulation.data.default_joint_vel, + ) + articulation.write_joint_state_to_sim(joint_pos, joint_vel) + # reset articulation + articulation.reset() + # apply force + # TODO: Replace with wrench composer once the deprecation is complete + articulation.set_external_force_and_torque( + external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids + ) + # perform simulation + for _ in range(100): + # apply action to the articulation + articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) + articulation.write_data_to_sim() + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + # check condition + for i in range(num_articulations): + # since there is a moment applied on the articulation, the articulation should rotate + assert articulation.data.root_ang_vel_w[i, 2].item() > 0.1 + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device): + """Test application of external force on the legs of the articulation at a given position. + + This test verifies that: + 1. External forces can be applied to multiple bodies at a given position + 2. External forces can be applied to multiple bodies in the global frame + 3. External forces are calculated and composed correctly + 4. The forces affect the articulation's motion correctly + 5. The articulation responds to the forces as expected + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="anymal") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, _ = articulation.find_bodies(".*_SHANK") + # Sample a large force + external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_b[..., 2] = 500.0 + external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + external_wrench_positions_b[..., 1] = 1.0 + + desired_force = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + desired_force[..., 2] = 1000.0 + desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + desired_torque[..., 0] = 1000.0 + + # Now we are ready! + for i in range(5): + # reset root state + articulation.write_root_pose_to_sim(articulation.data.default_root_state.clone()[:, :7]) + articulation.write_root_velocity_to_sim(articulation.data.default_root_state.clone()[:, 7:]) + # reset dof state + joint_pos, joint_vel = ( + articulation.data.default_joint_pos, + articulation.data.default_joint_vel, + ) + articulation.write_joint_state_to_sim(joint_pos, joint_vel) + # reset articulation + articulation.reset() + + is_global = False + if i % 2 == 0: + body_com_pos_w = articulation.data.body_com_pos_w[:, body_ids, :3] + is_global = True + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + external_wrench_positions_b += body_com_pos_w + else: + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + + # apply force + articulation.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + articulation.permanent_wrench_composer.add_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + # perform simulation + for _ in range(100): + # apply action to the articulation + articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) + articulation.write_data_to_sim() + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + # check condition + for i in range(num_articulations): + # since there is a moment applied on the articulation, the articulation should rotate + assert torch.abs(articulation.data.root_ang_vel_w[i, 2]).item() > 0.1 + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_loading_gains_from_usd(sim, num_articulations, device): + """Test that gains are loaded from USD file if actuator model has them as None. + + This test verifies that: + 1. Gains are loaded correctly from USD file + 2. Default gains are applied when not specified + 3. The gains match the expected values + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ + articulation_cfg = generate_articulation_cfg(articulation_type="humanoid", stiffness=None, damping=None) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + + # Play sim + sim.reset() + + # Expected gains + # -- Stiffness values + expected_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, + } + indices_list, _, values_list = string_utils.resolve_matching_names_values( + expected_stiffness, articulation.joint_names + ) + expected_stiffness = torch.zeros(articulation.num_instances, articulation.num_joints, device=articulation.device) + expected_stiffness[:, indices_list] = torch.tensor(values_list, device=articulation.device) + # -- Damping values + expected_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, + } + indices_list, _, values_list = string_utils.resolve_matching_names_values( + expected_damping, articulation.joint_names + ) + expected_damping = torch.zeros_like(expected_stiffness) + expected_damping[:, indices_list] = torch.tensor(values_list, device=articulation.device) + + # Check that gains are loaded from USD file + torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) + torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane): + """Test that gains are loaded from the configuration correctly. + + This test verifies that: + 1. Gains are loaded correctly from configuration + 2. The gains match the expected values + 3. The gains are applied correctly to the actuators + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test """ - Tests + articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device + ) + + # Play sim + sim.reset() + + # Expected gains + expected_stiffness = torch.full( + (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device + ) + expected_damping = torch.full_like(expected_stiffness, 2.0) + + # Check that gains are loaded from USD file + torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) + torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_setting_gains_from_cfg_dict(sim, num_articulations, device): + """Test that gains are loaded from the configuration dictionary correctly. + + This test verifies that: + 1. Gains are loaded correctly from configuration dictionary + 2. The gains match the expected values + 3. The gains are applied correctly to the actuators + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test """ + articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device + ) + # Play sim + sim.reset() + + # Expected gains + expected_stiffness = torch.full( + (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device + ) + expected_damping = torch.full_like(expected_stiffness, 2.0) + + # Check that gains are loaded from USD file + torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) + torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("vel_limit_sim", [1e5, None]) +@pytest.mark.parametrize("vel_limit", [1e2, None]) +@pytest.mark.parametrize("add_ground_plane", [False]) +@pytest.mark.isaacsim_ci +def test_setting_velocity_limit_implicit(sim, num_articulations, device, vel_limit_sim, vel_limit, add_ground_plane): + """Test setting of velocity limit for implicit actuators. + + This test verifies that: + 1. Velocity limits can be set correctly for implicit actuators + 2. The limits are applied correctly to the simulation + 3. The limits are handled correctly when both sim and non-sim limits are set - def test_initialization_floating_base_non_root(self): - """Test initialization for a floating-base with articulation root on a rigid body. - under the provided prim path.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg( - articulation_type="humanoid", stiffness=0.0, damping=0.0 - ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - - # # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that is fixed base - self.assertFalse(articulation.is_fixed_base) - # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) - self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 21)) - - # Check some internal physx data for debugging - # -- joint related - self.assertEqual( - articulation.root_physx_view.max_dofs, - articulation.root_physx_view.shared_metatype.dof_count, - ) - # -- link related - self.assertEqual( - articulation.root_physx_view.max_links, - articulation.root_physx_view.shared_metatype.link_count, - ) - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [ - path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0] - ] - self.assertListEqual(prim_path_body_names, articulation.body_names) - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance( - articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg - ) - self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - def test_initialization_floating_base(self): - """Test initialization for a floating-base with articulation root on provided prim path.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg( - articulation_type="anymal", stiffness=0.0, damping=0.0 - ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that floating base - self.assertFalse(articulation.is_fixed_base) - # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) - self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 12)) - self.assertEqual( - articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) - ) - self.assertEqual( - articulation.data.default_inertia.shape, (num_articulations, articulation.num_bodies, 9) - ) - - # Check some internal physx data for debugging - # -- joint related - self.assertEqual( - articulation.root_physx_view.max_dofs, - articulation.root_physx_view.shared_metatype.dof_count, - ) - # -- link related - self.assertEqual( - articulation.root_physx_view.max_links, - articulation.root_physx_view.shared_metatype.link_count, - ) - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [ - path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0] - ] - self.assertListEqual(prim_path_body_names, articulation.body_names) - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance( - articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg - ) - self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - def test_initialization_fixed_base(self): - """Test initialization for fixed base.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that fixed base - self.assertTrue(articulation.is_fixed_base) - # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) - self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 9)) - self.assertEqual( - articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) - ) - self.assertEqual( - articulation.data.default_inertia.shape, (num_articulations, articulation.num_bodies, 9) - ) - - # Check some internal physx data for debugging - # -- joint related - self.assertEqual( - articulation.root_physx_view.max_dofs, - articulation.root_physx_view.shared_metatype.dof_count, - ) - # -- link related - self.assertEqual( - articulation.root_physx_view.max_links, - articulation.root_physx_view.shared_metatype.link_count, - ) - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [ - path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0] - ] - self.assertListEqual(prim_path_body_names, articulation.body_names) - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance( - articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg - ) - self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - # check that the root is at the correct state - its default state as it is fixed base - default_root_state = articulation.data.default_root_state.clone() - default_root_state[:, :3] = default_root_state[:, :3] + translations - - torch.testing.assert_close(articulation.data.root_state_w, default_root_state) - - def test_initialization_fixed_base_single_joint(self): - """Test initialization for fixed base articulation with a single joint.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that fixed base - self.assertTrue(articulation.is_fixed_base) - # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) - self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 1)) - self.assertEqual( - articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) - ) - self.assertEqual( - articulation.data.default_inertia.shape, (num_articulations, articulation.num_bodies, 9) - ) - - # Check some internal physx data for debugging - # -- joint related - self.assertEqual( - articulation.root_physx_view.max_dofs, - articulation.root_physx_view.shared_metatype.dof_count, - ) - # -- link related - self.assertEqual( - articulation.root_physx_view.max_links, - articulation.root_physx_view.shared_metatype.link_count, - ) - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [ - path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0] - ] - self.assertListEqual(prim_path_body_names, articulation.body_names) - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance( - articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg - ) - self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - # check that the root is at the correct state - its default state as it is fixed base - default_root_state = articulation.data.default_root_state.clone() - default_root_state[:, :3] = default_root_state[:, :3] + translations - - torch.testing.assert_close(articulation.data.root_state_w, default_root_state) - - # check that the max force is what we set - physx_effort_limit = articulation.root_physx_view.get_dof_max_forces().to(device) - expected_joint_effort_limit = torch.full_like( - physx_effort_limit, articulation_cfg.spawn.joint_drive_props.max_effort - ) - torch.testing.assert_close(physx_effort_limit, expected_joint_effort_limit) - # check that the max velocity is what we set - physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device) - expected_joint_vel_limit = torch.full_like( - physx_vel_limit, articulation_cfg.spawn.joint_drive_props.max_velocity - ) - torch.testing.assert_close(physx_vel_limit, expected_joint_vel_limit) - - def test_initialization_hand_with_tendons(self): - """Test initialization for fixed base articulated hand with tendons.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="shadow_hand") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that fixed base - self.assertTrue(articulation.is_fixed_base) - # Check buffers that exists and have correct shapes - self.assertTrue(articulation.data.root_pos_w.shape == (num_articulations, 3)) - self.assertTrue(articulation.data.root_quat_w.shape == (num_articulations, 4)) - self.assertTrue(articulation.data.joint_pos.shape == (num_articulations, 24)) - self.assertEqual( - articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) - ) - self.assertEqual( - articulation.data.default_inertia.shape, (num_articulations, articulation.num_bodies, 9) - ) - - # Check some internal physx data for debugging - # -- joint related - self.assertEqual( - articulation.root_physx_view.max_dofs, - articulation.root_physx_view.shared_metatype.dof_count, - ) - # -- link related - self.assertEqual( - articulation.root_physx_view.max_links, - articulation.root_physx_view.shared_metatype.link_count, - ) - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance( - articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg - ) - self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - def test_initialization_floating_base_made_fixed_base(self): - """Test initialization for a floating-base articulation made fixed-base using schema properties.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - # Fix root link - articulation_cfg.spawn.articulation_props.fix_root_link = True - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that is fixed base - self.assertTrue(articulation.is_fixed_base) - # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) - self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 12)) - - # Check some internal physx data for debugging - # -- joint related - self.assertEqual( - articulation.root_physx_view.max_dofs, - articulation.root_physx_view.shared_metatype.dof_count, - ) - # -- link related - self.assertEqual( - articulation.root_physx_view.max_links, - articulation.root_physx_view.shared_metatype.link_count, - ) - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [ - path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0] - ] - self.assertListEqual(prim_path_body_names, articulation.body_names) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - # check that the root is at the correct state - its default state as it is fixed base - default_root_state = articulation.data.default_root_state.clone() - default_root_state[:, :3] = default_root_state[:, :3] + translations - - torch.testing.assert_close(articulation.data.root_state_w, default_root_state) - - def test_initialization_fixed_base_made_floating_base(self): - """Test initialization for fixed base made floating-base using schema properties.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - # Unfix root link - articulation_cfg.spawn.articulation_props.fix_root_link = False - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that is floating base - self.assertFalse(articulation.is_fixed_base) - # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) - self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 9)) - - # Check some internal physx data for debugging - # -- joint related - self.assertEqual( - articulation.root_physx_view.max_dofs, - articulation.root_physx_view.shared_metatype.dof_count, - ) - # -- link related - self.assertEqual( - articulation.root_physx_view.max_links, - articulation.root_physx_view.shared_metatype.link_count, - ) - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [ - path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0] - ] - self.assertListEqual(prim_path_body_names, articulation.body_names) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - def test_out_of_range_default_joint_pos(self): - """Test that the default joint position from configuration is out of range.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation_cfg.init_state.joint_pos = { - "panda_joint1": 10.0, - "panda_joint[2, 4]": -20.0, - } - - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertFalse(articulation._is_initialized) - - def test_out_of_range_default_joint_vel(self): - """Test that the default joint velocity from configuration is out of range.""" - with build_simulation_context(device="cuda:0", add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Robot") - articulation_cfg.init_state.joint_vel = { - "panda_joint1": 100.0, - "panda_joint[2, 4]": -60.0, - } - articulation = Articulation(articulation_cfg) - - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - - # Play sim + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + vel_limit_sim: The velocity limit to set in simulation + vel_limit: The velocity limit to set in actuator + """ + # create simulation + articulation_cfg = generate_articulation_cfg( + articulation_type="single_joint_implicit", + velocity_limit_sim=vel_limit_sim, + velocity_limit=vel_limit, + ) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, + num_articulations=num_articulations, + device=device, + ) + # Play sim + if vel_limit_sim is not None and vel_limit is not None: + with pytest.raises(ValueError): sim.reset() - # Check if articulation is initialized - self.assertFalse(articulation._is_initialized) - - def test_joint_pos_limits(self): - """Test write_joint_position_limit_to_sim API and when default position falls outside of the new limits.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation._is_initialized) - - # Get current default joint pos - default_joint_pos = articulation._data.default_joint_pos.clone() - - # Set new joint limits - limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) - limits[..., 0] = ( - torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 - ) * -1.0 - limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 - articulation.write_joint_position_limit_to_sim(limits) - - # Check new limits are in place - torch.testing.assert_close(articulation._data.joint_pos_limits, limits) - torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) - - # Set new joint limits with indexing - env_ids = torch.arange(1, device=device) - joint_ids = torch.arange(2, device=device) - limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) - limits[..., 0] = (torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0) * -1.0 - limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0 - articulation.write_joint_position_limit_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids) - - # Check new limits are in place - torch.testing.assert_close(articulation._data.joint_pos_limits[env_ids][:, joint_ids], limits) - torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) - - # Set new joint limits that invalidate default joint pos - limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) - limits[..., 0] = torch.rand(num_articulations, articulation.num_joints, device=device) * -0.1 - limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1 - articulation.write_joint_position_limit_to_sim(limits) - - # Check if all values are within the bounds - within_bounds = (articulation._data.default_joint_pos >= limits[..., 0]) & ( - articulation._data.default_joint_pos <= limits[..., 1] - ) - self.assertTrue(torch.all(within_bounds)) - - # Set new joint limits that invalidate default joint pos with indexing - limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) - limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 - limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 - articulation.write_joint_position_limit_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids) - - # Check if all values are within the bounds - within_bounds = ( - articulation._data.default_joint_pos[env_ids][:, joint_ids] >= limits[..., 0] - ) & (articulation._data.default_joint_pos[env_ids][:, joint_ids] <= limits[..., 1]) - self.assertTrue(torch.all(within_bounds)) - - def test_external_force_buffer(self): - """Test if external force buffer correctly updates in the force value is zero case.""" - - num_articulations = 2 - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # play the simulator - sim.reset() - - # find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") - - # reset root state - root_state = articulation.data.default_root_state.clone() - articulation.write_root_state_to_sim(root_state) - - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos, - articulation.data.default_joint_vel, - ) - articulation.write_joint_state_to_sim(joint_pos, joint_vel) - - # reset articulation - articulation.reset() - - # perform simulation - for step in range(5): - # initiate force tensor - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - - if step == 0 or step == 3: - # set a non-zero force - force = 1 - else: - # set a zero force - force = 0 - - # set force value - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - # apply force - articulation.set_external_force_and_torque( - external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids - ) - - # check if the articulation's force and torque buffers are correctly updated - for i in range(num_articulations): - self.assertTrue(articulation._external_force_b[i, 0, 0].item() == force) - self.assertTrue(articulation._external_torque_b[i, 0, 0].item() == force) - - # apply action to the articulation - articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) - articulation.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - articulation.update(sim.cfg.dt) - - def test_external_force_on_single_body(self): - """Test application of external force on the base of the articulation.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - # Play the simulator - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 1] = 1000.0 - - # Now we are ready! - for _ in range(5): - # reset root state - root_state = articulation.data.default_root_state.clone() - - articulation.write_root_pose_to_sim(root_state[:, :7]) - articulation.write_root_velocity_to_sim(root_state[:, 7:]) - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos, - articulation.data.default_joint_vel, - ) - articulation.write_joint_state_to_sim(joint_pos, joint_vel) - # reset articulation - articulation.reset() - # apply force - articulation.set_external_force_and_torque( - external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition that the articulations have fallen down - for i in range(num_articulations): - self.assertLess(articulation.data.root_pos_w[i, 2].item(), 0.2) - - def test_external_force_on_multiple_bodies(self): - """Test application of external force on the legs of the articulation.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Play the simulator - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies(".*_SHANK") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 1] = 100.0 - - # Now we are ready! - for _ in range(5): - # reset root state - articulation.write_root_pose_to_sim(articulation.data.default_root_state.clone()[:, :7]) - articulation.write_root_velocity_to_sim(articulation.data.default_root_state.clone()[:, 7:]) - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos, - articulation.data.default_joint_vel, - ) - articulation.write_joint_state_to_sim(joint_pos, joint_vel) - # reset articulation - articulation.reset() - # apply force - articulation.set_external_force_and_torque( - external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target(articulation.data.default_joint_pos.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition - for i in range(num_articulations): - # since there is a moment applied on the articulation, the articulation should rotate - self.assertTrue(articulation.data.root_ang_vel_w[i, 2].item() > 0.1) - - def test_loading_gains_from_usd(self): - """Test that gains are loaded from USD file if actuator model has them as None.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg( - articulation_type="humanoid", stiffness=None, damping=None - ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Play sim - sim.reset() - - # Expected gains - # -- Stiffness values - expected_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, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_stiffness, articulation.joint_names - ) - expected_stiffness = torch.zeros( - articulation.num_instances, articulation.num_joints, device=articulation.device - ) - expected_stiffness[:, indices_list] = torch.tensor(values_list, device=articulation.device) - # -- Damping values - expected_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, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_damping, articulation.joint_names - ) - expected_damping = torch.zeros_like(expected_stiffness) - expected_damping[:, indices_list] = torch.tensor(values_list, device=articulation.device) - - # Check that gains are loaded from USD file - torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) - torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) - - def test_setting_gains_from_cfg(self): - """Test that gains are loaded from the configuration correctly. - - Note: We purposefully give one argument as int and other as float to check that it is handled correctly. - """ - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play sim - sim.reset() - - # Expected gains - expected_stiffness = torch.full( - (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device - ) - expected_damping = torch.full_like(expected_stiffness, 2.0) - - # Check that gains are loaded from USD file - torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) - torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) - - def test_setting_gains_from_cfg_dict(self): - """Test that gains are loaded from the configuration dictionary correctly. - - Note: We purposefully give one argument as int and other as float to check that it is handled correctly. - """ - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - # Play sim - sim.reset() - - # Expected gains - expected_stiffness = torch.full( - (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device - ) - expected_damping = torch.full_like(expected_stiffness, 2.0) - - # Check that gains are loaded from USD file - torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) - torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) - - def test_setting_velocity_limit_implicit(self): - """Test setting of velocity limit for implicit actuators. - - This test checks that the behavior of setting velocity limits are consistent for implicit actuators - with previously defined behaviors. - - We do not set velocity limit to simulation when `velocity_limit` is specified. This is mainly for backwards - compatibility. To set the velocity limit to simulation, users should set `velocity_limit_sim`. - """ - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - for vel_limit_sim in (1e5, None): - for vel_limit in (1e2, None): - with self.subTest( - num_articulations=num_articulations, - device=device, - vel_limit_sim=vel_limit_sim, - vel_limit=vel_limit, - ): - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True - ) as sim: - # create simulation - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - velocity_limit_sim=vel_limit_sim, - velocity_limit=vel_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - sim.reset() - - if vel_limit_sim is not None and vel_limit is not None: - # Case 1: during initialization, the actuator will raise a ValueError and fail to - # initialize when both these attributes are set. - # note: The Exception is not caught with self.assertRaises or try-except - self.assertTrue(len(articulation.actuators) == 0) - continue - - # read the values set into the simulation - physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device) - # check data buffer - torch.testing.assert_close(articulation.data.joint_velocity_limits, physx_vel_limit) - # check actuator has simulation velocity limit - torch.testing.assert_close( - articulation.actuators["joint"].velocity_limit_sim, physx_vel_limit - ) - # check that both values match for velocity limit - torch.testing.assert_close( - articulation.actuators["joint"].velocity_limit_sim, - articulation.actuators["joint"].velocity_limit, - ) - - if vel_limit_sim is None: - # Case 2: both velocity limit and velocity limit sim are not set - # This is the case where the velocity limit keeps its USD default value - # Case 3: velocity limit sim is not set but velocity limit is set - # For backwards compatibility, we do not set velocity limit to simulation - # Thus, both default to USD default value. - limit = articulation_cfg.spawn.joint_drive_props.max_velocity - else: - # Case 4: only velocity limit sim is set - # In this case, the velocity limit is set to the USD value - limit = vel_limit_sim - - # check max velocity is what we set - expected_velocity_limit = torch.full_like(physx_vel_limit, limit) - torch.testing.assert_close(physx_vel_limit, expected_velocity_limit) - - def test_setting_velocity_limit_explicit(self): - """Test setting of velocity limit for explicit actuators. - - This test checks that the behavior of setting velocity limits are consistent for explicit actuators - with previously defined behaviors. - - Velocity limits to simulation for explicit actuators are only configured through `velocity_limit_sim`. - """ - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - for vel_limit_sim in (1e5, None): - for vel_limit in (1e2, None): - with self.subTest( - num_articulations=num_articulations, - device=device, - vel_limit_sim=vel_limit_sim, - vel_limit=vel_limit, - ): - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True - ) as sim: - # create simulation - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_explicit", - velocity_limit_sim=vel_limit_sim, - velocity_limit=vel_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - sim.reset() - - # collect limit init values - physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device) - actuator_vel_limit = articulation.actuators["joint"].velocity_limit - actuator_vel_limit_sim = articulation.actuators["joint"].velocity_limit_sim - - # check data buffer for joint_velocity_limits_sim - torch.testing.assert_close(articulation.data.joint_velocity_limits, physx_vel_limit) - # check actuator velocity_limit_sim is set to physx - torch.testing.assert_close(actuator_vel_limit_sim, physx_vel_limit) - - if vel_limit is not None: - expected_actuator_vel_limit = torch.full_like(actuator_vel_limit, vel_limit) - # check actuator is set - torch.testing.assert_close(actuator_vel_limit, expected_actuator_vel_limit) - - # check physx is not velocity_limit - self.assertFalse(torch.allclose(actuator_vel_limit, physx_vel_limit)) - else: - # check actuator velocity_limit is the same as the PhysX default - torch.testing.assert_close(actuator_vel_limit, physx_vel_limit) - - # simulation velocity limit is set to USD value unless user overrides - if vel_limit_sim is not None: - limit = vel_limit_sim - else: - limit = articulation_cfg.spawn.joint_drive_props.max_velocity - # check physx is set to expected value - expected_vel_limit = torch.full_like(physx_vel_limit, limit) - torch.testing.assert_close(physx_vel_limit, expected_vel_limit) - - def test_setting_effort_limit_implicit(self): - """Test setting of the effort limit for implicit actuators. - - In this case, the `effort_limit` and `effort_limit_sim` are treated as equivalent parameters. - """ - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - for effort_limit_sim in (1e5, None): - for effort_limit in (1e2, None): - with self.subTest( - num_articulations=num_articulations, - device=device, - effort_limit_sim=effort_limit_sim, - effort_limit=effort_limit, - ): - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True - ) as sim: - # create simulation - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - effort_limit_sim=effort_limit_sim, - effort_limit=effort_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - sim.reset() - - if effort_limit_sim is not None and effort_limit is not None: - # during initialization, the actuator will raise a ValueError and fail to - # initialize. The Exception is not caught with self.assertRaises or try-except - self.assertTrue(len(articulation.actuators) == 0) - continue - - # obtain the physx effort limits - physx_effort_limit = articulation.root_physx_view.get_dof_max_forces() - physx_effort_limit = physx_effort_limit.to(device=device) - - # check that the two are equivalent - torch.testing.assert_close( - articulation.actuators["joint"].effort_limit_sim, - articulation.actuators["joint"].effort_limit, - ) - torch.testing.assert_close( - articulation.actuators["joint"].effort_limit_sim, physx_effort_limit - ) - - # decide the limit based on what is set - if effort_limit_sim is None and effort_limit is None: - limit = articulation_cfg.spawn.joint_drive_props.max_effort - elif effort_limit_sim is not None and effort_limit is None: - limit = effort_limit_sim - elif effort_limit_sim is None and effort_limit is not None: - limit = effort_limit - - # check that the max force is what we set - expected_effort_limit = torch.full_like(physx_effort_limit, limit) - torch.testing.assert_close(physx_effort_limit, expected_effort_limit) - - def test_setting_effort_limit_explicit(self): - """Test setting of effort limit for explicit actuators. - - This test checks that the behavior of setting effort limits are consistent for explicit actuators - with previously defined behaviors. - - Effort limits to simulation for explicit actuators are only configured through `effort_limit_sim`. - """ - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - for effort_limit_sim in (1e5, None): - for effort_limit in (1e2, None): - with self.subTest( - num_articulations=num_articulations, - device=device, - effort_limit_sim=effort_limit_sim, - effort_limit=effort_limit, - ): - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True - ) as sim: - # create simulation - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_explicit", - effort_limit_sim=effort_limit_sim, - effort_limit=effort_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - sim.reset() - - # collect limit init values - physx_effort_limit = articulation.root_physx_view.get_dof_max_forces().to(device) - actuator_effort_limit = articulation.actuators["joint"].effort_limit - actuator_effort_limit_sim = articulation.actuators["joint"].effort_limit_sim - - # check actuator effort_limit_sim is set to physx - torch.testing.assert_close(actuator_effort_limit_sim, physx_effort_limit) - - if effort_limit is not None: - expected_actuator_effort_limit = torch.full_like( - actuator_effort_limit, effort_limit - ) - # check actuator is set - torch.testing.assert_close(actuator_effort_limit, expected_actuator_effort_limit) - - # check physx effort limit does not match the one explicit actuator has - self.assertFalse(torch.allclose(actuator_effort_limit, physx_effort_limit)) - else: - # check actuator effort_limit is the same as the PhysX default - torch.testing.assert_close(actuator_effort_limit, physx_effort_limit) - - # when using explicit actuators, the limits are set to high unless user overrides - if effort_limit_sim is not None: - limit = effort_limit_sim - else: - limit = ActuatorBase._DEFAULT_MAX_EFFORT_SIM # type: ignore - # check physx internal value matches the expected sim value - expected_effort_limit = torch.full_like(physx_effort_limit, limit) - torch.testing.assert_close(physx_effort_limit, expected_effort_limit) - - def test_reset(self): - """Test that reset method works properly. - - Need to check that all actuators are reset and that forces, torques and last body velocities are reset to 0.0. - - NOTE: Currently no way to determine actuators have been reset, can leave this to actuator tests that - implement reset method. - - """ - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - # Now we are ready! - # reset articulation - articulation.reset() - - # Reset should zero external forces and torques - self.assertFalse(articulation.has_external_wrench) - self.assertEqual(torch.count_nonzero(articulation._external_force_b), 0) - self.assertEqual(torch.count_nonzero(articulation._external_torque_b), 0) - - def test_apply_joint_command(self): - """Test applying of joint position target functions correctly for a robotic arm.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_articulations=num_articulations, device=device): - with build_simulation_context( - gravity_enabled=True, device=device, add_ground_plane=True, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # reset joint state - joint_pos = articulation.data.default_joint_pos - joint_pos[:, 3] = 0.0 - - # apply action to the articulation - articulation.set_joint_position_target(joint_pos) - articulation.write_data_to_sim() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # Check that current joint position is not the same as default joint position, meaning - # the articulation moved. We can't check that it reached it's desired joint position as the gains - # are not properly tuned - assert not torch.allclose(articulation.data.joint_pos, joint_pos) - - def test_body_root_state(self): - """Test for reading the `body_state_w` property""" - for num_articulations in (1, 2): - # for num_articulations in ( 2,): - for device in ("cuda:0", "cpu"): - # for device in ("cuda:0",): - for with_offset in [True, False]: - # for with_offset in [True,]: - with self.subTest(num_articulations=num_articulations, device=device, with_offset=with_offset): - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") - articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) - env_idx = torch.tensor([x for x in range(num_articulations)]) - # Check that boundedness of articulation is correct - self.assertEqual(ctypes.c_long.from_address(id(articulation)).value, 1) - # Play sim - sim.reset() - # Check if articulation is initialized - self.assertTrue(articulation.is_initialized) - # Check that fixed base - self.assertTrue(articulation.is_fixed_base) - - # change center of mass offset from link frame - if with_offset: - offset = [0.5, 0.0, 0.0] - else: - offset = [0.0, 0.0, 0.0] - - # create com offsets - num_bodies = articulation.num_bodies - com = articulation.root_physx_view.get_coms() - link_offset = [1.0, 0.0, 0.0] # the offset from CenterPivot to Arm frames - new_com = torch.tensor(offset, device=device).repeat(num_articulations, 1, 1) - com[:, 1, :3] = new_com.squeeze(-2) - articulation.root_physx_view.set_coms(com, env_idx) - - # check they are set - torch.testing.assert_close(articulation.root_physx_view.get_coms(), com) - - for i in range(50): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # get state properties - root_state_w = articulation.data.root_state_w - root_link_state_w = articulation.data.root_link_state_w - root_com_state_w = articulation.data.root_com_state_w - body_state_w = articulation.data.body_state_w - body_link_state_w = articulation.data.body_link_state_w - body_com_state_w = articulation.data.body_com_state_w - - if with_offset: - # get joint state - joint_pos = articulation.data.joint_pos.unsqueeze(-1) - joint_vel = articulation.data.joint_vel.unsqueeze(-1) - - # LINK state - # pose - torch.testing.assert_close(root_state_w[..., :7], root_link_state_w[..., :7]) - torch.testing.assert_close(body_state_w[..., :7], body_link_state_w[..., :7]) - - # lin_vel arm - lin_vel_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) - vx = -(link_offset[0]) * joint_vel * torch.sin(joint_pos) - vy = torch.zeros(num_articulations, 1, 1, device=device) - vz = (link_offset[0]) * joint_vel * torch.cos(joint_pos) - lin_vel_gt[:, 1, :] = torch.cat([vx, vy, vz], dim=-1).squeeze(-2) - - # linear velocity of root link should be zero - torch.testing.assert_close( - lin_vel_gt[:, 0, :], root_link_state_w[..., 7:10], atol=1e-3, rtol=1e-1 - ) - # linear velocity of pendulum link should be - torch.testing.assert_close( - lin_vel_gt, body_link_state_w[..., 7:10], atol=1e-3, rtol=1e-1 - ) - - # ang_vel - torch.testing.assert_close(root_state_w[..., 10:], root_link_state_w[..., 10:]) - torch.testing.assert_close(body_state_w[..., 10:], body_link_state_w[..., 10:]) - - # COM state - # position and orientation shouldn't match for the _state_com_w but everything else will - pos_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) - px = (link_offset[0] + offset[0]) * torch.cos(joint_pos) - py = torch.zeros(num_articulations, 1, 1, device=device) - pz = (link_offset[0] + offset[0]) * torch.sin(joint_pos) - pos_gt[:, 1, :] = torch.cat([px, py, pz], dim=-1).squeeze(-2) - pos_gt += env_pos.unsqueeze(-2).repeat(1, num_bodies, 1) - torch.testing.assert_close( - pos_gt[:, 0, :], root_com_state_w[..., :3], atol=1e-3, rtol=1e-1 - ) - torch.testing.assert_close(pos_gt, body_com_state_w[..., :3], atol=1e-3, rtol=1e-1) - - # orientation - com_quat_b = articulation.data.com_quat_b - com_quat_w = math_utils.quat_mul(body_link_state_w[..., 3:7], com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_state_w[..., 3:7]) - torch.testing.assert_close(com_quat_w[:, 0, :], root_com_state_w[..., 3:7]) - - # linear vel, and angular vel - torch.testing.assert_close(root_state_w[..., 7:], root_com_state_w[..., 7:]) - torch.testing.assert_close(body_state_w[..., 7:], body_com_state_w[..., 7:]) - else: - # single joint center of masses are at link frames so they will be the same - torch.testing.assert_close(root_state_w, root_link_state_w) - torch.testing.assert_close(root_state_w, root_com_state_w) - torch.testing.assert_close(body_state_w, body_link_state_w) - torch.testing.assert_close(body_state_w, body_com_state_w) - - def test_write_root_state(self): - """Test the setters for root_state using both the link frame and center of mass as reference frame.""" - for num_articulations in (1, 2): - for device in ("cuda:0", "cpu"): - for with_offset in [True, False]: - for state_location in ("com", "link"): - with self.subTest( - num_articulations=num_articulations, - device=device, - with_offset=with_offset, - state_location=state_location, - ): - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True, gravity_enabled=False - ) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, env_pos = generate_articulation( - articulation_cfg, num_articulations, device - ) - env_idx = torch.tensor([x for x in range(num_articulations)]) - - # Play sim - sim.reset() - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([1.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - - # create com offsets - com = articulation.root_physx_view.get_coms() - new_com = offset - com[:, 0, :3] = new_com.squeeze(-2) - articulation.root_physx_view.set_coms(com, env_idx) - - # check they are set - torch.testing.assert_close(articulation.root_physx_view.get_coms(), com) - - rand_state = torch.zeros_like(articulation.data.root_state_w) - rand_state[..., :7] = articulation.data.default_root_state[..., :7] - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_idx = env_idx.to(device) - for i in range(10): - - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - if state_location == "com": - if i % 2 == 0: - articulation.write_root_com_state_to_sim(rand_state) - else: - articulation.write_root_com_state_to_sim(rand_state, env_ids=env_idx) - elif state_location == "link": - if i % 2 == 0: - articulation.write_root_link_state_to_sim(rand_state) - else: - articulation.write_root_link_state_to_sim(rand_state, env_ids=env_idx) - - if state_location == "com": - torch.testing.assert_close(rand_state, articulation.data.root_com_state_w) - elif state_location == "link": - torch.testing.assert_close(rand_state, articulation.data.root_link_state_w) + return + sim.reset() + + # read the values set into the simulation + physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device) + # check data buffer + torch.testing.assert_close(articulation.data.joint_velocity_limits, physx_vel_limit) + # check actuator has simulation velocity limit + torch.testing.assert_close(articulation.actuators["joint"].velocity_limit_sim, physx_vel_limit) + # check that both values match for velocity limit + torch.testing.assert_close( + articulation.actuators["joint"].velocity_limit_sim, + articulation.actuators["joint"].velocity_limit, + ) + + if vel_limit_sim is None: + # Case 2: both velocity limit and velocity limit sim are not set + # This is the case where the velocity limit keeps its USD default value + # Case 3: velocity limit sim is not set but velocity limit is set + # For backwards compatibility, we do not set velocity limit to simulation + # Thus, both default to USD default value. + limit = articulation_cfg.spawn.joint_drive_props.max_velocity + else: + # Case 4: only velocity limit sim is set + # In this case, the velocity limit is set to the USD value + limit = vel_limit_sim + + # check max velocity is what we set + expected_velocity_limit = torch.full_like(physx_vel_limit, limit) + torch.testing.assert_close(physx_vel_limit, expected_velocity_limit) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("vel_limit_sim", [1e5, None]) +@pytest.mark.parametrize("vel_limit", [1e2, None]) +@pytest.mark.isaacsim_ci +def test_setting_velocity_limit_explicit(sim, num_articulations, device, vel_limit_sim, vel_limit): + """Test setting of velocity limit for explicit actuators.""" + articulation_cfg = generate_articulation_cfg( + articulation_type="single_joint_explicit", + velocity_limit_sim=vel_limit_sim, + velocity_limit=vel_limit, + ) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, + num_articulations=num_articulations, + device=device, + ) + # Play sim + sim.reset() + + # collect limit init values + physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device) + actuator_vel_limit = articulation.actuators["joint"].velocity_limit + actuator_vel_limit_sim = articulation.actuators["joint"].velocity_limit_sim + + # check data buffer for joint_velocity_limits_sim + torch.testing.assert_close(articulation.data.joint_velocity_limits, physx_vel_limit) + # check actuator velocity_limit_sim is set to physx + torch.testing.assert_close(actuator_vel_limit_sim, physx_vel_limit) + + if vel_limit is not None: + expected_actuator_vel_limit = torch.full( + (articulation.num_instances, articulation.num_joints), + vel_limit, + device=articulation.device, + ) + # check actuator is set + torch.testing.assert_close(actuator_vel_limit, expected_actuator_vel_limit) + # check physx is not velocity_limit + assert not torch.allclose(actuator_vel_limit, physx_vel_limit) + else: + # check actuator velocity_limit is the same as the PhysX default + torch.testing.assert_close(actuator_vel_limit, physx_vel_limit) + + # simulation velocity limit is set to USD value unless user overrides + if vel_limit_sim is not None: + limit = vel_limit_sim + else: + limit = articulation_cfg.spawn.joint_drive_props.max_velocity + # check physx is set to expected value + expected_vel_limit = torch.full_like(physx_vel_limit, limit) + torch.testing.assert_close(physx_vel_limit, expected_vel_limit) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("effort_limit_sim", [1e5, None]) +@pytest.mark.parametrize("effort_limit", [1e2, 80.0, None]) +@pytest.mark.isaacsim_ci +def test_setting_effort_limit_implicit(sim, num_articulations, device, effort_limit_sim, effort_limit): + """Test setting of effort limit for implicit actuators. + + This test verifies the effort limit resolution logic for actuator models implemented in :class:`ActuatorBase`: + - Case 1: If USD value == actuator config value: values match correctly + - Case 2: If USD value != actuator config value: actuator config value is used + - Case 3: If actuator config value is None: USD value is used as default + """ + articulation_cfg = generate_articulation_cfg( + articulation_type="single_joint_implicit", + effort_limit_sim=effort_limit_sim, + effort_limit=effort_limit, + ) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, + num_articulations=num_articulations, + device=device, + ) + # Play sim + if effort_limit_sim is not None and effort_limit is not None: + with pytest.raises(ValueError): + sim.reset() + return + sim.reset() + + # obtain the physx effort limits + physx_effort_limit = articulation.root_physx_view.get_dof_max_forces().to(device=device) + + # check that the two are equivalent + torch.testing.assert_close( + articulation.actuators["joint"].effort_limit_sim, + articulation.actuators["joint"].effort_limit, + ) + torch.testing.assert_close(articulation.actuators["joint"].effort_limit_sim, physx_effort_limit) + + # decide the limit based on what is set + if effort_limit_sim is None and effort_limit is None: + limit = articulation_cfg.spawn.joint_drive_props.max_effort + elif effort_limit_sim is not None and effort_limit is None: + limit = effort_limit_sim + elif effort_limit_sim is None and effort_limit is not None: + limit = effort_limit + + # check that the max force is what we set + expected_effort_limit = torch.full_like(physx_effort_limit, limit) + torch.testing.assert_close(physx_effort_limit, expected_effort_limit) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("effort_limit_sim", [1e5, None]) +@pytest.mark.parametrize("effort_limit", [80.0, 1e2, None]) +@pytest.mark.isaacsim_ci +def test_setting_effort_limit_explicit(sim, num_articulations, device, effort_limit_sim, effort_limit): + """Test setting of effort limit for explicit actuators. + + This test verifies the effort limit resolution logic for actuator models implemented in :class:`ActuatorBase`: + - Case 1: If USD value == actuator config value: values match correctly + - Case 2: If USD value != actuator config value: actuator config value is used + - Case 3: If actuator config value is None: USD value is used as default + + """ + + articulation_cfg = generate_articulation_cfg( + articulation_type="single_joint_explicit", + effort_limit_sim=effort_limit_sim, + effort_limit=effort_limit, + ) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, + num_articulations=num_articulations, + device=device, + ) + # Play sim + sim.reset() + + # usd default effort limit is set to 80 + usd_default_effort_limit = 80.0 + + # collect limit init values + physx_effort_limit = articulation.root_physx_view.get_dof_max_forces().to(device) + actuator_effort_limit = articulation.actuators["joint"].effort_limit + actuator_effort_limit_sim = articulation.actuators["joint"].effort_limit_sim + + # check actuator effort_limit_sim is set to physx + torch.testing.assert_close(actuator_effort_limit_sim, physx_effort_limit) + + if effort_limit is not None: + expected_actuator_effort_limit = torch.full_like(actuator_effort_limit, effort_limit) + # check actuator is set + torch.testing.assert_close(actuator_effort_limit, expected_actuator_effort_limit) + + # check physx effort limit does not match the one explicit actuator has + assert not (torch.allclose(actuator_effort_limit, physx_effort_limit)) + else: + # When effort_limit is None, actuator should use USD default values + expected_actuator_effort_limit = torch.full_like(physx_effort_limit, usd_default_effort_limit) + torch.testing.assert_close(actuator_effort_limit, expected_actuator_effort_limit) + + # when using explicit actuators, the limits are set to high unless user overrides + if effort_limit_sim is not None: + limit = effort_limit_sim + else: + limit = ActuatorBase._DEFAULT_MAX_EFFORT_SIM # type: ignore + # check physx internal value matches the expected sim value + expected_effort_limit = torch.full_like(physx_effort_limit, limit) + torch.testing.assert_close(actuator_effort_limit_sim, expected_effort_limit) + torch.testing.assert_close(physx_effort_limit, expected_effort_limit) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_reset(sim, num_articulations, device): + """Test that reset method works properly.""" + articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device + ) + + # Play the simulator + sim.reset() + + # Now we are ready! + # reset articulation + articulation.reset() + + # Reset should zero external forces and torques + assert not articulation._instantaneous_wrench_composer.active + assert not articulation._permanent_wrench_composer.active + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force_as_torch) == 0 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque_as_torch) == 0 + assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force_as_torch) == 0 + assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque_as_torch) == 0 + + if num_articulations > 1: + num_bodies = articulation.num_bodies + # TODO: Replace with wrench composer once the deprecation is complete + articulation.set_external_force_and_torque( + forces=torch.ones((num_articulations, num_bodies, 3), device=device), + torques=torch.ones((num_articulations, num_bodies, 3), device=device), + ) + articulation.instantaneous_wrench_composer.add_forces_and_torques( + forces=torch.ones((num_articulations, num_bodies, 3), device=device), + torques=torch.ones((num_articulations, num_bodies, 3), device=device), + ) + articulation.reset(env_ids=torch.tensor([0], device=device)) + assert articulation._instantaneous_wrench_composer.active + assert articulation._permanent_wrench_composer.active + assert ( + torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force_as_torch) == num_bodies * 3 + ) + assert ( + torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque_as_torch) == num_bodies * 3 + ) + assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force_as_torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque_as_torch) == num_bodies * 3 + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_apply_joint_command(sim, num_articulations, device, add_ground_plane): + """Test applying of joint position target functions correctly for a robotic arm.""" + articulation_cfg = generate_articulation_cfg(articulation_type="panda") + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device + ) + + # Play the simulator + sim.reset() + + for _ in range(100): + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + + # reset dof state + joint_pos = articulation.data.default_joint_pos + joint_pos[:, 3] = 0.0 + + # apply action to the articulation + articulation.set_joint_position_target(joint_pos) + articulation.write_data_to_sim() + + for _ in range(100): + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + + # Check that current joint position is not the same as default joint position, meaning + # the articulation moved. We can't check that it reached its desired joint position as the gains + # are not properly tuned + assert not torch.allclose(articulation.data.joint_pos, joint_pos) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.isaacsim_ci +def test_body_root_state(sim, num_articulations, device, with_offset): + """Test for reading the `body_state_w` property. + + This test verifies that: + 1. Body states can be read correctly + 2. States are correct with and without offsets + 3. States are consistent across different devices + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + with_offset: Whether to test with offset + """ + sim._app_control_on_stop_handle = None + articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") + articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) + env_idx = torch.tensor([x for x in range(num_articulations)], device=device) + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1, "Boundedness of articulation is incorrect" + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized, "Articulation is not initialized" + # Check that fixed base + assert articulation.is_fixed_base, "Articulation is not a fixed base" + + # change center of mass offset from link frame + if with_offset: + offset = [0.5, 0.0, 0.0] + else: + offset = [0.0, 0.0, 0.0] + + # create com offsets + num_bodies = articulation.num_bodies + com = articulation.root_physx_view.get_coms() + link_offset = [1.0, 0.0, 0.0] # the offset from CenterPivot to Arm frames + new_com = torch.tensor(offset, device=device).repeat(num_articulations, 1, 1) + com[:, 1, :3] = new_com.squeeze(-2) + articulation.root_physx_view.set_coms(com.cpu(), env_idx.cpu()) + + # check they are set + torch.testing.assert_close(articulation.root_physx_view.get_coms(), com.cpu()) + + for i in range(50): + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + + # get state properties + root_state_w = articulation.data.root_state_w + root_link_state_w = articulation.data.root_link_state_w + root_com_state_w = articulation.data.root_com_state_w + body_state_w = articulation.data.body_state_w + body_link_state_w = articulation.data.body_link_state_w + body_com_state_w = articulation.data.body_com_state_w + + if with_offset: + # get joint state + joint_pos = articulation.data.joint_pos.unsqueeze(-1) + joint_vel = articulation.data.joint_vel.unsqueeze(-1) + + # LINK state + # pose + torch.testing.assert_close(root_state_w[..., :7], root_link_state_w[..., :7]) + torch.testing.assert_close(body_state_w[..., :7], body_link_state_w[..., :7]) + + # lin_vel arm + lin_vel_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) + vx = -(link_offset[0]) * joint_vel * torch.sin(joint_pos) + vy = torch.zeros(num_articulations, 1, 1, device=device) + vz = (link_offset[0]) * joint_vel * torch.cos(joint_pos) + lin_vel_gt[:, 1, :] = torch.cat([vx, vy, vz], dim=-1).squeeze(-2) + + # linear velocity of root link should be zero + torch.testing.assert_close(lin_vel_gt[:, 0, :], root_link_state_w[..., 7:10], atol=1e-3, rtol=1e-1) + # linear velocity of pendulum link should be + torch.testing.assert_close(lin_vel_gt, body_link_state_w[..., 7:10], atol=1e-3, rtol=1e-1) + + # ang_vel + torch.testing.assert_close(root_state_w[..., 10:], root_link_state_w[..., 10:]) + torch.testing.assert_close(body_state_w[..., 10:], body_link_state_w[..., 10:]) + + # COM state + # position and orientation shouldn't match for the _state_com_w but everything else will + pos_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) + px = (link_offset[0] + offset[0]) * torch.cos(joint_pos) + py = torch.zeros(num_articulations, 1, 1, device=device) + pz = (link_offset[0] + offset[0]) * torch.sin(joint_pos) + pos_gt[:, 1, :] = torch.cat([px, py, pz], dim=-1).squeeze(-2) + pos_gt += env_pos.unsqueeze(-2).repeat(1, num_bodies, 1) + torch.testing.assert_close(pos_gt[:, 0, :], root_com_state_w[..., :3], atol=1e-3, rtol=1e-1) + torch.testing.assert_close(pos_gt, body_com_state_w[..., :3], atol=1e-3, rtol=1e-1) + + # orientation + com_quat_b = articulation.data.body_com_quat_b + com_quat_w = math_utils.quat_mul(body_link_state_w[..., 3:7], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_state_w[..., 3:7]) + torch.testing.assert_close(com_quat_w[:, 0, :], root_com_state_w[..., 3:7]) + + # linear vel, and angular vel + torch.testing.assert_close(root_state_w[..., 7:], root_com_state_w[..., 7:]) + torch.testing.assert_close(body_state_w[..., 7:], body_com_state_w[..., 7:]) + else: + # single joint center of masses are at link frames so they will be the same + torch.testing.assert_close(root_state_w, root_link_state_w) + torch.testing.assert_close(root_state_w, root_com_state_w) + torch.testing.assert_close(body_state_w, body_link_state_w) + torch.testing.assert_close(body_state_w, body_com_state_w) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.parametrize("state_location", ["com", "link"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_write_root_state(sim, num_articulations, device, with_offset, state_location, gravity_enabled): + """Test the setters for root_state using both the link frame and center of mass as reference frame. + + This test verifies that: + 1. Root states can be written correctly + 2. States are correct with and without offsets + 3. States can be written for both COM and link frames + 4. States are consistent across different devices + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + with_offset: Whether to test with offset + state_location: Whether to test COM or link frame + """ + sim._app_control_on_stop_handle = None + articulation_cfg = generate_articulation_cfg(articulation_type="anymal") + articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) + env_idx = torch.tensor([x for x in range(num_articulations)]) + + # Play sim + sim.reset() + + # change center of mass offset from link frame + if with_offset: + offset = torch.tensor([1.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) + else: + offset = torch.tensor([0.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) + + # create com offsets + com = articulation.root_physx_view.get_coms() + new_com = offset + com[:, 0, :3] = new_com.squeeze(-2) + articulation.root_physx_view.set_coms(com, env_idx) + + # check they are set + torch.testing.assert_close(articulation.root_physx_view.get_coms(), com) + + rand_state = torch.zeros_like(articulation.data.root_state_w) + rand_state[..., :7] = articulation.data.default_root_state[..., :7] + rand_state[..., :3] += env_pos + # make quaternion a unit vector + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + + env_idx = env_idx.to(device) + for i in range(10): + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + + if state_location == "com": + if i % 2 == 0: + articulation.write_root_com_state_to_sim(rand_state) + else: + articulation.write_root_com_state_to_sim(rand_state, env_ids=env_idx) + elif state_location == "link": + if i % 2 == 0: + articulation.write_root_link_state_to_sim(rand_state) + else: + articulation.write_root_link_state_to_sim(rand_state, env_ids=env_idx) + + if state_location == "com": + torch.testing.assert_close(rand_state, articulation.data.root_com_state_w) + elif state_location == "link": + torch.testing.assert_close(rand_state, articulation.data.root_link_state_w) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_body_incoming_joint_wrench_b_single_joint(sim, num_articulations, device): + """Test the data.body_incoming_joint_wrench_b buffer is populated correctly and statically correct for single joint. + + This test verifies that: + 1. The body incoming joint wrench buffer has correct shape + 2. The wrench values are statically correct for a single joint + 3. The wrench values match expected values from gravity and external forces + + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device + ) + + # Play the simulator + sim.reset() + # apply external force + external_force_vector_b = torch.zeros((num_articulations, articulation.num_bodies, 3), device=device) + external_force_vector_b[:, 1, 1] = 10.0 # 10 N in Y direction + external_torque_vector_b = torch.zeros((num_articulations, articulation.num_bodies, 3), device=device) + external_torque_vector_b[:, 1, 2] = 10.0 # 10 Nm in z direction + + # apply action to the articulation + joint_pos = torch.ones_like(articulation.data.joint_pos) * 1.5708 / 2.0 + articulation.write_joint_state_to_sim( + torch.ones_like(articulation.data.joint_pos), torch.zeros_like(articulation.data.joint_vel) + ) + articulation.set_joint_position_target(joint_pos) + articulation.write_data_to_sim() + for _ in range(50): + # TODO: Replace with wrench composer once the deprecation is complete + articulation.set_external_force_and_torque(forces=external_force_vector_b, torques=external_torque_vector_b) + articulation.write_data_to_sim() + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + + # check shape + assert articulation.data.body_incoming_joint_wrench_b.shape == (num_articulations, articulation.num_bodies, 6) + + # calculate expected static + mass = articulation.data.default_mass + pos_w = articulation.data.body_pos_w + quat_w = articulation.data.body_quat_w + + mass_link2 = mass[:, 1].view(num_articulations, -1) + gravity = torch.tensor(sim.cfg.gravity, device="cpu").repeat(num_articulations, 1).view((num_articulations, 3)) + + # NOTE: the com and link pose for single joint are colocated + weight_vector_w = mass_link2 * gravity + # expected wrench from link mass and external wrench + expected_wrench = torch.zeros((num_articulations, 6), device=device) + expected_wrench[:, :3] = math_utils.quat_apply( + math_utils.quat_conjugate(quat_w[:, 0, :]), + weight_vector_w.to(device) + math_utils.quat_apply(quat_w[:, 1, :], external_force_vector_b[:, 1, :]), + ) + expected_wrench[:, 3:] = math_utils.quat_apply( + math_utils.quat_conjugate(quat_w[:, 0, :]), + torch.cross( + pos_w[:, 1, :].to(device) - pos_w[:, 0, :].to(device), + weight_vector_w.to(device) + math_utils.quat_apply(quat_w[:, 1, :], external_force_vector_b[:, 1, :]), + dim=-1, + ) + + math_utils.quat_apply(quat_w[:, 1, :], external_torque_vector_b[:, 1, :]), + ) + + # check value of last joint wrench + torch.testing.assert_close( + expected_wrench, + articulation.data.body_incoming_joint_wrench_b[:, 1, :].squeeze(1), + atol=1e-2, + rtol=1e-3, + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_setting_articulation_root_prim_path(sim, device): + """Test that the articulation root prim path can be set explicitly.""" + sim._app_control_on_stop_handle = None + # Create articulation + articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") + print(articulation_cfg.spawn.usd_path) + articulation_cfg.articulation_root_prim_path = "/torso" + articulation, _ = generate_articulation(articulation_cfg, 1, device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation._is_initialized + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_setting_invalid_articulation_root_prim_path(sim, device): + """Test that the articulation root prim path can be set explicitly.""" + sim._app_control_on_stop_handle = None + # Create articulation + articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") + print(articulation_cfg.spawn.usd_path) + articulation_cfg.articulation_root_prim_path = "/non_existing_prim_path" + articulation, _ = generate_articulation(articulation_cfg, 1, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + with pytest.raises(RuntimeError): + sim.reset() + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_write_joint_state_data_consistency(sim, num_articulations, device, gravity_enabled): + """Test the setters for root_state using both the link frame and center of mass as reference frame. + + This test verifies that after write_joint_state_to_sim operations: + 1. state, com_state, link_state value consistency + 2. body_pose, link + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + sim._app_control_on_stop_handle = None + articulation_cfg = generate_articulation_cfg(articulation_type="anymal") + articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) + env_idx = torch.tensor([x for x in range(num_articulations)]) + + # Play sim + sim.reset() + + limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) + limits[..., 0] = (torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0) * -1.0 + limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 + articulation.write_joint_position_limit_to_sim(limits) + + from torch.distributions import Uniform + + pos_dist = Uniform(articulation.data.joint_pos_limits[..., 0], articulation.data.joint_pos_limits[..., 1]) + vel_dist = Uniform(-articulation.data.joint_vel_limits, articulation.data.joint_vel_limits) + + original_body_states = articulation.data.body_state_w.clone() + + rand_joint_pos = pos_dist.sample() + rand_joint_vel = vel_dist.sample() + + articulation.write_joint_state_to_sim(rand_joint_pos, rand_joint_vel) + # make sure valued updated + assert torch.count_nonzero(original_body_states[:, 1:] != articulation.data.body_state_w[:, 1:]) > ( + len(original_body_states[:, 1:]) / 2 + ) + # validate body - link consistency + torch.testing.assert_close(articulation.data.body_state_w[..., :7], articulation.data.body_link_state_w[..., :7]) + # skip 7:10 because they differs from link frame, this should be fine because we are only checking + # if velocity update is triggered, which can be determined by comparing angular velocity + torch.testing.assert_close(articulation.data.body_state_w[..., 10:], articulation.data.body_link_state_w[..., 10:]) + + # validate link - com conistency + expected_com_pos, expected_com_quat = math_utils.combine_frame_transforms( + articulation.data.body_link_state_w[..., :3].view(-1, 3), + articulation.data.body_link_state_w[..., 3:7].view(-1, 4), + articulation.data.body_com_pos_b.view(-1, 3), + articulation.data.body_com_quat_b.view(-1, 4), + ) + torch.testing.assert_close(expected_com_pos.view(len(env_idx), -1, 3), articulation.data.body_com_pos_w) + torch.testing.assert_close(expected_com_quat.view(len(env_idx), -1, 4), articulation.data.body_com_quat_w) + + # validate body - com consistency + torch.testing.assert_close(articulation.data.body_state_w[..., 7:10], articulation.data.body_com_lin_vel_w) + torch.testing.assert_close(articulation.data.body_state_w[..., 10:], articulation.data.body_com_ang_vel_w) + + # validate pos_w, quat_w, pos_b, quat_b is consistent with pose_w and pose_b + expected_com_pose_w = torch.cat((articulation.data.body_com_pos_w, articulation.data.body_com_quat_w), dim=2) + expected_com_pose_b = torch.cat((articulation.data.body_com_pos_b, articulation.data.body_com_quat_b), dim=2) + expected_body_pose_w = torch.cat((articulation.data.body_pos_w, articulation.data.body_quat_w), dim=2) + expected_body_link_pose_w = torch.cat( + (articulation.data.body_link_pos_w, articulation.data.body_link_quat_w), dim=2 + ) + torch.testing.assert_close(articulation.data.body_com_pose_w, expected_com_pose_w) + torch.testing.assert_close(articulation.data.body_com_pose_b, expected_com_pose_b) + torch.testing.assert_close(articulation.data.body_pose_w, expected_body_pose_w) + torch.testing.assert_close(articulation.data.body_link_pose_w, expected_body_link_pose_w) + + # validate pose_w is consistent state[..., :7] + torch.testing.assert_close(articulation.data.body_pose_w, articulation.data.body_state_w[..., :7]) + torch.testing.assert_close(articulation.data.body_vel_w, articulation.data.body_state_w[..., 7:]) + torch.testing.assert_close(articulation.data.body_link_pose_w, articulation.data.body_link_state_w[..., :7]) + torch.testing.assert_close(articulation.data.body_com_pose_w, articulation.data.body_com_state_w[..., :7]) + torch.testing.assert_close(articulation.data.body_vel_w, articulation.data.body_state_w[..., 7:]) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_spatial_tendons(sim, num_articulations, device): + """Test spatial tendons apis. + This test verifies that: + 1. The articulation is properly initialized + 2. The articulation has spatial tendons + 3. All buffers have correct shapes + 4. The articulation can be simulated + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + device: The device to run the simulation on + """ + # skip test if Isaac Sim version is less than 5.0 + if get_isaac_sim_version().major < 5: + pytest.skip("Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later.") + return + articulation_cfg = generate_articulation_cfg(articulation_type="spatial_tendon_test_asset") + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + + # Check that boundedness of articulation is correct + assert ctypes.c_long.from_address(id(articulation)).value == 1 + + # Play sim + sim.reset() + # Check if articulation is initialized + assert articulation.is_initialized + # Check that fixed base + assert articulation.is_fixed_base + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.shape == (num_articulations, 4) + assert articulation.data.joint_pos.shape == (num_articulations, 3) + assert articulation.data.default_mass.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.default_inertia.shape == (num_articulations, articulation.num_bodies, 9) + assert articulation.num_spatial_tendons == 1 + + articulation.set_spatial_tendon_stiffness(torch.tensor([10.0])) + articulation.set_spatial_tendon_limit_stiffness(torch.tensor([10.0])) + articulation.set_spatial_tendon_damping(torch.tensor([10.0])) + articulation.set_spatial_tendon_offset(torch.tensor([10.0])) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + + +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground_plane): + """Test applying of joint position target functions correctly for a robotic arm.""" + articulation_cfg = generate_articulation_cfg(articulation_type="panda") + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device + ) + + # Play the simulator + sim.reset() + + for _ in range(100): + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + + # apply action to the articulation + 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 get_isaac_sim_version().major >= 5: + articulation.write_joint_dynamic_friction_coefficient_to_sim(dynamic_friction) + articulation.write_joint_viscous_friction_coefficient_to_sim(viscous_friction) + articulation.write_data_to_sim() + + for _ in range(100): + # perform step + sim.step() + # update buffers + articulation.update(sim.cfg.dt) + + if get_isaac_sim_version().major >= 5: + 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 get_isaac_sim_version().major >= 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__": - run_tests() + pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab/test/assets/test_deformable_object.py b/source/isaaclab/test/assets/test_deformable_object.py index adfdfdd371a8..4726a274462c 100644 --- a/source/isaaclab/test/assets/test_deformable_object.py +++ b/source/isaaclab/test/assets/test_deformable_object.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,24 +9,20 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests - -# Can set this to False to see the GUI for debugging -# This will also add lights to the scene -HEADLESS = True +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import ctypes + +import pytest import torch -import unittest +from flaky import flaky import carb -import isaacsim.core.utils.prims as prim_utils import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils @@ -62,7 +58,7 @@ def generate_cubes_scene( origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) # Create Top-level Xforms, one for each cube for i, origin in enumerate(origins): - prim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) + sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) # Resolve spawn configuration if has_api: @@ -93,354 +89,253 @@ def generate_cubes_scene( return cube_object -class TestDeformableObject(unittest.TestCase): - """Test for deformable object class.""" +@pytest.fixture +def sim(): + """Create simulation context.""" + with build_simulation_context(auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + yield sim - """ - Tests - """ - def test_initialization(self): - """Test initialization for prim with deformable body API at the provided prim path. - - This test checks that the deformable object is correctly initialized with deformable material at - different paths. - """ - for material_path in [None, "/World/SoftMaterial", "material"]: - for num_cubes in (1, 2): - with self.subTest(num_cubes=num_cubes, material_path=material_path): - with build_simulation_context(auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object = generate_cubes_scene(num_cubes=num_cubes, material_path=material_path) - - # Check that boundedness of deformable object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim - sim.reset() - - # Check that boundedness of deformable object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Check if object is initialized - self.assertTrue(cube_object.is_initialized) - - # Check correct number of cubes - self.assertEqual(cube_object.num_instances, num_cubes) - self.assertEqual(cube_object.root_physx_view.count, num_cubes) - - # Check correct number of materials in the view - if material_path: - if material_path.startswith("/"): - self.assertEqual(cube_object.material_physx_view.count, 1) - else: - self.assertEqual(cube_object.material_physx_view.count, num_cubes) - else: - self.assertIsNone(cube_object.material_physx_view) - - # Check buffers that exists and have correct shapes - self.assertEqual( - cube_object.data.nodal_state_w.shape, - (num_cubes, cube_object.max_sim_vertices_per_body, 6), - ) - self.assertEqual( - cube_object.data.nodal_kinematic_target.shape, - (num_cubes, cube_object.max_sim_vertices_per_body, 4), - ) - self.assertEqual(cube_object.data.root_pos_w.shape, (num_cubes, 3)) - self.assertEqual(cube_object.data.root_vel_w.shape, (num_cubes, 3)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # check we can get all the sim data from the object - self.assertEqual( - cube_object.data.sim_element_quat_w.shape, - (num_cubes, cube_object.max_sim_elements_per_body, 4), - ) - self.assertEqual( - cube_object.data.sim_element_deform_gradient_w.shape, - (num_cubes, cube_object.max_sim_elements_per_body, 3, 3), - ) - self.assertEqual( - cube_object.data.sim_element_stress_w.shape, - (num_cubes, cube_object.max_sim_elements_per_body, 3, 3), - ) - self.assertEqual( - cube_object.data.collision_element_quat_w.shape, - (num_cubes, cube_object.max_collision_elements_per_body, 4), - ) - self.assertEqual( - cube_object.data.collision_element_deform_gradient_w.shape, - (num_cubes, cube_object.max_collision_elements_per_body, 3, 3), - ) - self.assertEqual( - cube_object.data.collision_element_stress_w.shape, - (num_cubes, cube_object.max_collision_elements_per_body, 3, 3), - ) - - def test_initialization_on_device_cpu(self): - """Test that initialization fails with deformable body API on the CPU.""" - with build_simulation_context(device="cpu", auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object = generate_cubes_scene(num_cubes=5, device="cpu") - - # Check that boundedness of deformable object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("material_path", [None, "/World/SoftMaterial", "material"]) +def test_initialization(sim, num_cubes, material_path): + """Test initialization for prim with deformable body API at the provided prim path.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes, material_path=material_path) + + # Check that boundedness of deformable object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + sim.reset() + + # Check if object is initialized + assert cube_object.is_initialized + + # Check correct number of cubes + assert cube_object.num_instances == num_cubes + assert cube_object.root_physx_view.count == num_cubes + + # Check correct number of materials in the view + if material_path: + if material_path.startswith("/"): + assert cube_object.material_physx_view.count == 1 + else: + assert cube_object.material_physx_view.count == num_cubes + else: + assert cube_object.material_physx_view is None + + # Check buffers that exist and have correct shapes + assert cube_object.data.nodal_state_w.shape == (num_cubes, cube_object.max_sim_vertices_per_body, 6) + assert cube_object.data.nodal_kinematic_target.shape == (num_cubes, cube_object.max_sim_vertices_per_body, 4) + assert cube_object.data.root_pos_w.shape == (num_cubes, 3) + assert cube_object.data.root_vel_w.shape == (num_cubes, 3) + + # Simulate physics + for _ in range(2): + sim.step() + cube_object.update(sim.cfg.dt) + + # Check sim data + assert cube_object.data.sim_element_quat_w.shape == (num_cubes, cube_object.max_sim_elements_per_body, 4) + assert cube_object.data.sim_element_deform_gradient_w.shape == ( + num_cubes, + cube_object.max_sim_elements_per_body, + 3, + 3, + ) + assert cube_object.data.sim_element_stress_w.shape == (num_cubes, cube_object.max_sim_elements_per_body, 3, 3) + assert cube_object.data.collision_element_quat_w.shape == ( + num_cubes, + cube_object.max_collision_elements_per_body, + 4, + ) + assert cube_object.data.collision_element_deform_gradient_w.shape == ( + num_cubes, + cube_object.max_collision_elements_per_body, + 3, + 3, + ) + assert cube_object.data.collision_element_stress_w.shape == ( + num_cubes, + cube_object.max_collision_elements_per_body, + 3, + 3, + ) + + +@pytest.mark.isaacsim_ci +def test_initialization_on_device_cpu(): + """Test that initialization fails with deformable body API on the CPU.""" + with build_simulation_context(device="cpu", auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object = generate_cubes_scene(num_cubes=5, device="cpu") + + # Check that boundedness of deformable object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + with pytest.raises(RuntimeError): sim.reset() - # Check if object is initialized - self.assertFalse(cube_object.is_initialized) - - def test_initialization_with_kinematic_enabled(self): - """Test that initialization for prim with kinematic flag enabled.""" - for num_cubes in (1, 2): - with self.subTest(num_cubes=num_cubes): - with build_simulation_context(auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object = generate_cubes_scene(num_cubes=num_cubes, kinematic_enabled=True) - - # Check that boundedness of deformable object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertTrue(cube_object.is_initialized) - - # Check buffers that exists and have correct shapes - self.assertEqual(cube_object.data.root_pos_w.shape, (num_cubes, 3)) - self.assertEqual(cube_object.data.root_vel_w.shape, (num_cubes, 3)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - # check that the object is kinematic - default_nodal_state_w = cube_object.data.default_nodal_state_w.clone() - torch.testing.assert_close(cube_object.data.nodal_state_w, default_nodal_state_w) - - def test_initialization_with_no_deformable_body(self): - """Test that initialization fails when no deformable body is found at the provided prim path.""" - for num_cubes in (1, 2): - with self.subTest(num_cubes=num_cubes): - with build_simulation_context(auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object = generate_cubes_scene(num_cubes=num_cubes, has_api=False) - - # Check that boundedness of deformable object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertFalse(cube_object.is_initialized) - - def test_set_nodal_state(self): - """Test setting the state of the deformable object. - - In this test, we set the state of the deformable object to a random state and check - that the object is in that state after simulation. We set gravity to zero as - we don't want any external forces acting on the object to ensure state remains static. - """ - for num_cubes in (1, 2): - with self.subTest(num_cubes=num_cubes): - # Turn off gravity for this test as we don't want any external forces acting on the object - # to ensure state remains static - with build_simulation_context(gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object = generate_cubes_scene(num_cubes=num_cubes) - - # Play the simulator - sim.reset() - - # Set each state type individually as they are dependent on each other - for state_type_to_randomize in ["nodal_pos_w", "nodal_vel_w"]: - state_dict = { - "nodal_pos_w": torch.zeros_like(cube_object.data.nodal_pos_w), - "nodal_vel_w": torch.zeros_like(cube_object.data.nodal_vel_w), - } - - # Now we are ready! - for _ in range(5): - # reset object - cube_object.reset() - - # Set random state - state_dict[state_type_to_randomize] = torch.randn( - num_cubes, cube_object.max_sim_vertices_per_body, 3, device=sim.device - ) - - # perform simulation - for _ in range(5): - nodal_state = torch.cat( - [ - state_dict["nodal_pos_w"], - state_dict["nodal_vel_w"], - ], - dim=-1, - ) - # reset nodal state - cube_object.write_nodal_state_to_sim(nodal_state) - - # assert that set node quantities are equal to the ones set in the state_dict - torch.testing.assert_close( - cube_object.data.nodal_state_w, nodal_state, rtol=1e-5, atol=1e-5 - ) - - # perform step - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - def test_set_nodal_state_with_applied_transform(self): - """Test setting the state of the deformable object with applied transform. - - In this test, we apply a random pose to the object and check that the mean of the nodal positions - is equal to the applied pose after simulation. We set gravity to zero as we don't want any external - forces acting on the object to ensure state remains static. - """ - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - for num_cubes in (1, 2): - with self.subTest(num_cubes=num_cubes): - # Turn off gravity for this test as we don't want any external forces acting on the object - # to ensure state remains static - with build_simulation_context(gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object = generate_cubes_scene(num_cubes=num_cubes) - - # Play the simulator - sim.reset() - - for randomize_pos in [True, False]: - for randomize_rot in [True, False]: - # Now we are ready! - for _ in range(5): - # reset the nodal state of the object - nodal_state = cube_object.data.default_nodal_state_w.clone() - mean_nodal_pos_default = nodal_state[..., :3].mean(dim=1) - # sample randomize position and rotation - if randomize_pos: - pos_w = torch.rand(cube_object.num_instances, 3, device=sim.device) - pos_w[:, 2] += 0.5 - else: - pos_w = None - if randomize_rot: - quat_w = math_utils.random_orientation(cube_object.num_instances, device=sim.device) - else: - quat_w = None - # apply random pose to the object - nodal_state[..., :3] = cube_object.transform_nodal_pos( - nodal_state[..., :3], pos_w, quat_w - ) - # compute mean of initial nodal positions - mean_nodal_pos_init = nodal_state[..., :3].mean(dim=1) - - # check computation is correct - if pos_w is None: - torch.testing.assert_close( - mean_nodal_pos_init, mean_nodal_pos_default, rtol=1e-5, atol=1e-5 - ) - else: - torch.testing.assert_close( - mean_nodal_pos_init, mean_nodal_pos_default + pos_w, rtol=1e-5, atol=1e-5 - ) - - # write nodal state to simulation - cube_object.write_nodal_state_to_sim(nodal_state) - # reset object - cube_object.reset() - - # perform simulation - for _ in range(50): - # perform step - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # check that the mean of the nodal positions is equal to the applied pose - torch.testing.assert_close( - cube_object.data.root_pos_w, mean_nodal_pos_init, rtol=1e-5, atol=1e-5 - ) - - def test_set_kinematic_targets(self): - """Test setting kinematic targets for the deformable object. - - In this test, we set one of the cubes with only kinematic targets for its nodal positions and check - that the object is in that state after simulation. - """ - for num_cubes in (2, 4): - with self.subTest(num_cubes=num_cubes): - # Turn off gravity for this test as we don't want any external forces acting on the object - # to ensure state remains static - with build_simulation_context(auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object = generate_cubes_scene(num_cubes=num_cubes, height=1.0) - - # Play the simulator - sim.reset() - - # Get sim kinematic targets - nodal_kinematic_targets = cube_object.root_physx_view.get_sim_kinematic_targets().clone() - - # Now we are ready! - for _ in range(5): - # reset nodal state - cube_object.write_nodal_state_to_sim(cube_object.data.default_nodal_state_w) - - default_root_pos = cube_object.data.default_nodal_state_w.mean(dim=1) - - # reset object - cube_object.reset() - - # write kinematic targets - # -- enable kinematic targets for the first cube - nodal_kinematic_targets[1:, :, 3] = 1.0 - nodal_kinematic_targets[0, :, 3] = 0.0 - # -- set kinematic targets for the first cube - nodal_kinematic_targets[0, :, :3] = cube_object.data.default_nodal_state_w[0, :, :3] - # -- write kinematic targets to simulation - cube_object.write_nodal_kinematic_target_to_sim( - nodal_kinematic_targets[0], env_ids=torch.tensor([0], device=sim.device) - ) - - # perform simulation - for _ in range(20): - # perform step - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # assert that set node quantities are equal to the ones set in the state_dict - torch.testing.assert_close( - cube_object.data.nodal_pos_w[0], nodal_kinematic_targets[0, :, :3], rtol=1e-5, atol=1e-5 - ) - # see other cubes are dropping - root_pos_w = cube_object.data.root_pos_w - self.assertTrue(torch.all(root_pos_w[1:, 2] < default_root_pos[1:, 2])) - - -if __name__ == "__main__": - run_tests() + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.isaacsim_ci +def test_initialization_with_kinematic_enabled(sim, num_cubes): + """Test that initialization for prim with kinematic flag enabled.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes, kinematic_enabled=True) + + # Check that boundedness of deformable object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + sim.reset() + + # Check if object is initialized + assert cube_object.is_initialized + + # Check buffers that exist and have correct shapes + assert cube_object.data.root_pos_w.shape == (num_cubes, 3) + assert cube_object.data.root_vel_w.shape == (num_cubes, 3) + + # Simulate physics + for _ in range(2): + sim.step() + cube_object.update(sim.cfg.dt) + default_nodal_state_w = cube_object.data.default_nodal_state_w.clone() + torch.testing.assert_close(cube_object.data.nodal_state_w, default_nodal_state_w) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.isaacsim_ci +def test_initialization_with_no_deformable_body(sim, num_cubes): + """Test that initialization fails when no deformable body is found at the provided prim path.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes, has_api=False) + + # Check that boundedness of deformable object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + with pytest.raises(RuntimeError): + sim.reset() + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.isaacsim_ci +def test_set_nodal_state(sim, num_cubes): + """Test setting the state of the deformable object.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes) + + # Play the simulator + sim.reset() + + for state_type_to_randomize in ["nodal_pos_w", "nodal_vel_w"]: + state_dict = { + "nodal_pos_w": torch.zeros_like(cube_object.data.nodal_pos_w), + "nodal_vel_w": torch.zeros_like(cube_object.data.nodal_vel_w), + } + + for _ in range(5): + cube_object.reset() + + state_dict[state_type_to_randomize] = torch.randn( + num_cubes, cube_object.max_sim_vertices_per_body, 3, device=sim.device + ) + + for _ in range(5): + nodal_state = torch.cat( + [ + state_dict["nodal_pos_w"], + state_dict["nodal_vel_w"], + ], + dim=-1, + ) + cube_object.write_nodal_state_to_sim(nodal_state) + + torch.testing.assert_close(cube_object.data.nodal_state_w, nodal_state, rtol=1e-5, atol=1e-5) + + sim.step() + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("randomize_pos", [True, False]) +@pytest.mark.parametrize("randomize_rot", [True, False]) +@flaky(max_runs=3, min_passes=1) +@pytest.mark.isaacsim_ci +def test_set_nodal_state_with_applied_transform(sim, num_cubes, randomize_pos, randomize_rot): + """Test setting the state of the deformable object with applied transform.""" + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + # Create a new simulation context with gravity disabled + with build_simulation_context(auto_add_lighting=True, gravity_enabled=False) as sim: + sim._app_control_on_stop_handle = None + cube_object = generate_cubes_scene(num_cubes=num_cubes) + sim.reset() + + for _ in range(5): + nodal_state = cube_object.data.default_nodal_state_w.clone() + mean_nodal_pos_default = nodal_state[..., :3].mean(dim=1) + + if randomize_pos: + pos_w = torch.rand(cube_object.num_instances, 3, device=sim.device) + pos_w[:, 2] += 0.5 + else: + pos_w = None + if randomize_rot: + quat_w = math_utils.random_orientation(cube_object.num_instances, device=sim.device) + else: + quat_w = None + + nodal_state[..., :3] = cube_object.transform_nodal_pos(nodal_state[..., :3], pos_w, quat_w) + mean_nodal_pos_init = nodal_state[..., :3].mean(dim=1) + + if pos_w is None: + torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default, rtol=1e-5, atol=1e-5) + else: + torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default + pos_w, rtol=1e-5, atol=1e-5) + + cube_object.write_nodal_state_to_sim(nodal_state) + cube_object.reset() + + for _ in range(50): + sim.step() + cube_object.update(sim.cfg.dt) + + torch.testing.assert_close(cube_object.data.root_pos_w, mean_nodal_pos_init, rtol=1e-5, atol=1e-5) + + +@pytest.mark.parametrize("num_cubes", [2, 4]) +@pytest.mark.isaacsim_ci +def test_set_kinematic_targets(sim, num_cubes): + """Test setting kinematic targets for the deformable object.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes, height=1.0) + + sim.reset() + + nodal_kinematic_targets = cube_object.root_physx_view.get_sim_kinematic_targets().clone() + + for _ in range(5): + cube_object.write_nodal_state_to_sim(cube_object.data.default_nodal_state_w) + + default_root_pos = cube_object.data.default_nodal_state_w.mean(dim=1) + + cube_object.reset() + + nodal_kinematic_targets[1:, :, 3] = 1.0 + nodal_kinematic_targets[0, :, 3] = 0.0 + nodal_kinematic_targets[0, :, :3] = cube_object.data.default_nodal_state_w[0, :, :3] + cube_object.write_nodal_kinematic_target_to_sim( + nodal_kinematic_targets[0], env_ids=torch.tensor([0], device=sim.device) + ) + + for _ in range(20): + sim.step() + cube_object.update(sim.cfg.dt) + + torch.testing.assert_close( + cube_object.data.nodal_pos_w[0], nodal_kinematic_targets[0, :, :3], rtol=1e-5, atol=1e-5 + ) + root_pos_w = cube_object.data.root_pos_w + assert torch.all(root_pos_w[1:, 2] < default_root_pos[1:, 2]) diff --git a/source/isaaclab/test/assets/test_rigid_object.py b/source/isaaclab/test/assets/test_rigid_object.py index 0ab02d728073..8de5361e29ff 100644 --- a/source/isaaclab/test/assets/test_rigid_object.py +++ b/source/isaaclab/test/assets/test_rigid_object.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,31 +9,34 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests - -# Can set this to False to see the GUI for debugging -# This will also add lights to the scene -HEADLESS = True +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import ctypes -import torch -import unittest from typing import Literal -import isaacsim.core.utils.prims as prim_utils +import pytest +import torch +from flaky import flaky import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.sim import build_simulation_context from isaaclab.sim.spawners import materials from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR -from isaaclab.utils.math import default_orientation, quat_mul, quat_rotate_inverse, random_orientation +from isaaclab.utils.math import ( + combine_frame_transforms, + default_orientation, + quat_apply_inverse, + quat_inv, + quat_mul, + quat_rotate, + random_orientation, +) def generate_cubes_scene( @@ -59,7 +62,7 @@ def generate_cubes_scene( origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) # Create Top-level Xforms, one for each cube for i, origin in enumerate(origins): - prim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) + sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) # Resolve spawn configuration if api == "none": @@ -92,883 +95,1079 @@ def generate_cubes_scene( return cube_object, origins -class TestRigidObject(unittest.TestCase): - """Test for rigid object class.""" +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization(num_cubes, device): + """Test initialization for prim with rigid body API at the provided prim path.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Check that boundedness of rigid object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + sim.reset() + + # Check if object is initialized + assert cube_object.is_initialized + assert len(cube_object.body_names) == 1 + + # Check buffers that exists and have correct shapes + assert cube_object.data.root_pos_w.shape == (num_cubes, 3) + assert cube_object.data.root_quat_w.shape == (num_cubes, 4) + assert cube_object.data.default_mass.shape == (num_cubes, 1) + assert cube_object.data.default_inertia.shape == (num_cubes, 9) + + # Simulate physics + for _ in range(2): + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_with_kinematic_enabled(num_cubes, device): + """Test that initialization for prim with kinematic flag enabled.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, kinematic_enabled=True, device=device) + + # Check that boundedness of rigid object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + sim.reset() + + # Check if object is initialized + assert cube_object.is_initialized + assert len(cube_object.body_names) == 1 + + # Check buffers that exists and have correct shapes + assert cube_object.data.root_pos_w.shape == (num_cubes, 3) + assert cube_object.data.root_quat_w.shape == (num_cubes, 4) + + # Simulate physics + for _ in range(2): + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + # check that the object is kinematic + default_root_state = cube_object.data.default_root_state.clone() + default_root_state[:, :3] += origins + torch.testing.assert_close(cube_object.data.root_state_w, default_root_state) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_with_no_rigid_body(num_cubes, device): + """Test that initialization fails when no rigid body is found at the provided prim path.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, api="none", device=device) + + # Check that boundedness of rigid object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + with pytest.raises(RuntimeError): + sim.reset() + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_with_articulation_root(num_cubes, device): + """Test that initialization fails when an articulation root is found at the provided prim path.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, api="articulation_root", device=device) + + # Check that boundedness of rigid object is correct + assert ctypes.c_long.from_address(id(cube_object)).value == 1 + + # Play sim + with pytest.raises(RuntimeError): + sim.reset() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_buffer(device): + """Test if external force buffer correctly updates in the force value is zero case. + + In this test, we apply a non-zero force, then a zero force, then finally a non-zero force + to an object. We check if the force buffer is properly updated at each step. + """ + + # Generate cubes scene + with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object, origins = generate_cubes_scene(num_cubes=1, device=device) + + # play the simulator + sim.reset() + + # find bodies to apply the force + body_ids, body_names = cube_object.find_bodies(".*") + + # reset object + cube_object.reset() + + # perform simulation + for step in range(5): + # initiate force tensor + external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) + + if step == 0 or step == 3: + # set a non-zero force + force = 1 + else: + # set a zero force + force = 0 + + # set force value + external_wrench_b[:, :, 0] = force + external_wrench_b[:, :, 3] = force + + # apply force + cube_object.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + body_ids=body_ids, + ) + + # check if the cube's force and torque buffers are correctly updated + for i in range(cube_object.num_instances): + assert cube_object._permanent_wrench_composer.composed_force_as_torch[i, 0, 0].item() == force + assert cube_object._permanent_wrench_composer.composed_torque_as_torch[i, 0, 0].item() == force + + # Check if the instantaneous wrench is correctly added to the permanent wrench + cube_object.permanent_wrench_composer.add_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + body_ids=body_ids, + ) + + # apply action to the object + cube_object.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [2, 4]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_on_single_body(num_cubes, device): + """Test application of external force on the base of the object. + + In this test, we apply a force equal to the weight of an object on the base of + one of the objects. We check that the object does not move. For the other object, + we do not apply any force and check that it falls down. + We validate that this works when we apply the force in the global frame and in the local frame. """ - Tests + # Generate cubes scene + with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, body_names = cube_object.find_bodies(".*") + + # Sample a force equal to the weight of the object + external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[0::2, :, 2] = 9.81 * cube_object.root_physx_view.get_masses()[0] + + # Now we are ready! + for i in range(5): + # reset root state + root_state = cube_object.data.default_root_state.clone() + + # need to shift the position of the cubes otherwise they will be on top of each other + root_state[:, :3] = origins + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) + + # reset object + cube_object.reset() + + is_global = False + if i % 2 == 0: + is_global = True + positions = cube_object.data.body_com_pos_w[:, body_ids, :3] + else: + positions = None + + # apply force + cube_object.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=positions, + body_ids=body_ids, + is_global=is_global, + ) + # perform simulation + for _ in range(5): + # apply action to the object + cube_object.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + cube_object.update(sim.cfg.dt) + + # First object should still be at the same Z position (1.0) + torch.testing.assert_close( + cube_object.data.root_pos_w[0::2, 2], torch.ones(num_cubes // 2, device=sim.device) + ) + # Second object should have fallen, so it's Z height should be less than initial height of 1.0 + assert torch.all(cube_object.data.root_pos_w[1::2, 2] < 1.0) + + +@pytest.mark.parametrize("num_cubes", [2, 4]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_external_force_on_single_body_at_position(num_cubes, device): + """Test application of external force on the base of the object at a specific position. + + In this test, we apply a force equal to the weight of an object on the base of + one of the objects at 1m in the Y direction, we check that the object rotates around it's X axis. + For the other object, we do not apply any force and check that it falls down. + + We validate that this works when we apply the force in the global frame and in the local frame. """ + # Generate cubes scene + with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, body_names = cube_object.find_bodies(".*") + + # Sample a force equal to the weight of the object + external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_positions_b = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[0::2, :, 2] = 500.0 + external_wrench_positions_b[0::2, :, 1] = 1.0 + + # Desired force and torque + desired_force = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) + desired_force[0::2, :, 2] = 1000.0 + desired_torque = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) + desired_torque[0::2, :, 0] = 1000.0 + # Now we are ready! + for i in range(5): + # reset root state + root_state = cube_object.data.default_root_state.clone() + + # need to shift the position of the cubes otherwise they will be on top of each other + root_state[:, :3] = origins + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) + + # reset object + cube_object.reset() + + is_global = False + if i % 2 == 0: + is_global = True + body_com_pos_w = cube_object.data.body_com_pos_w[:, body_ids, :3] + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + external_wrench_positions_b += body_com_pos_w + else: + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + + # apply force + cube_object.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + cube_object.permanent_wrench_composer.add_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + torch.testing.assert_close( + cube_object._permanent_wrench_composer.composed_force_as_torch[:, 0, :], + desired_force[:, 0, :], + rtol=1e-6, + atol=1e-7, + ) + torch.testing.assert_close( + cube_object._permanent_wrench_composer.composed_torque_as_torch[:, 0, :], + desired_torque[:, 0, :], + rtol=1e-6, + atol=1e-7, + ) + # perform simulation + for _ in range(5): + # apply action to the object + cube_object.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + cube_object.update(sim.cfg.dt) + + # The first object should be rotating around it's X axis + assert torch.all(torch.abs(cube_object.data.root_ang_vel_b[0::2, 0]) > 0.1) + # Second object should have fallen, so it's Z height should be less than initial height of 1.0 + assert torch.all(cube_object.data.root_pos_w[1::2, 2] < 1.0) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_set_rigid_object_state(num_cubes, device): + """Test setting the state of the rigid object. + + In this test, we set the state of the rigid object to a random state and check + that the object is in that state after simulation. We set gravity to zero as + we don't want any external forces acting on the object to ensure state remains static. + """ + # Turn off gravity for this test as we don't want any external forces acting on the object + # to ensure state remains static + with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + state_types = ["root_pos_w", "root_quat_w", "root_lin_vel_w", "root_ang_vel_w"] + + # Set each state type individually as they are dependent on each other + for state_type_to_randomize in state_types: + state_dict = { + "root_pos_w": torch.zeros_like(cube_object.data.root_pos_w, device=sim.device), + "root_quat_w": default_orientation(num=num_cubes, device=sim.device), + "root_lin_vel_w": torch.zeros_like(cube_object.data.root_lin_vel_w, device=sim.device), + "root_ang_vel_w": torch.zeros_like(cube_object.data.root_ang_vel_w, device=sim.device), + } + + # Now we are ready! + for _ in range(5): + # reset object + cube_object.reset() + + # Set random state + if state_type_to_randomize == "root_quat_w": + state_dict[state_type_to_randomize] = random_orientation(num=num_cubes, device=sim.device) + else: + state_dict[state_type_to_randomize] = torch.randn(num_cubes, 3, device=sim.device) + + # perform simulation + for _ in range(5): + root_state = torch.cat( + [ + state_dict["root_pos_w"], + state_dict["root_quat_w"], + state_dict["root_lin_vel_w"], + state_dict["root_ang_vel_w"], + ], + dim=-1, + ) + # reset root state + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) + + sim.step() + + # assert that set root quantities are equal to the ones set in the state_dict + for key, expected_value in state_dict.items(): + value = getattr(cube_object.data, key) + torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) + + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_reset_rigid_object(num_cubes, device): + """Test resetting the state of the rigid object.""" + with build_simulation_context(device=device, gravity_enabled=True, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + for i in range(5): + # perform rendering + sim.step() + + # update object + cube_object.update(sim.cfg.dt) + + # Move the object to a random position + root_state = cube_object.data.default_root_state.clone() + root_state[:, :3] = torch.randn(num_cubes, 3, device=sim.device) + + # Random orientation + root_state[:, 3:7] = random_orientation(num=num_cubes, device=sim.device) + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) + + if i % 2 == 0: + # reset object + cube_object.reset() + + # Reset should zero external forces and torques + assert not cube_object._instantaneous_wrench_composer.active + assert not cube_object._permanent_wrench_composer.active + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_force_as_torch) == 0 + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_torque_as_torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_force_as_torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_torque_as_torch) == 0 + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_set_material_properties(num_cubes, device): + """Test getting and setting material properties of rigid object.""" + with build_simulation_context( + device=device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True + ) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play sim + sim.reset() + + # Set material properties + static_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) + dynamic_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) + restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) + + materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) + + indices = torch.tensor(range(num_cubes), dtype=torch.int) + # Add friction to cube + cube_object.root_physx_view.set_material_properties(materials, indices) + + # Simulate physics + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + # Get material properties + materials_to_check = cube_object.root_physx_view.get_material_properties() + + # Check if material properties are set correctly + torch.testing.assert_close(materials_to_check.reshape(num_cubes, 3), materials) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_no_friction(num_cubes, device): + """Test that a rigid object with no friction will maintain it's velocity when sliding across a plane.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + + # Create ground plane with no friction + cfg = sim_utils.GroundPlaneCfg( + physics_material=materials.RigidBodyMaterialCfg( + static_friction=0.0, + dynamic_friction=0.0, + restitution=0.0, + ) + ) + cfg.func("/World/GroundPlane", cfg) + + # Play sim + sim.reset() + + # Set material friction properties to be all zero + static_friction = torch.zeros(num_cubes, 1) + dynamic_friction = torch.zeros(num_cubes, 1) + restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) + + cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) + indices = torch.tensor(range(num_cubes), dtype=torch.int) + + cube_object.root_physx_view.set_material_properties(cube_object_materials, indices) + + # Set initial velocity + # Initial velocity in X to get the block moving + initial_velocity = torch.zeros((num_cubes, 6), device=sim.cfg.device) + initial_velocity[:, 0] = 0.1 + + cube_object.write_root_velocity_to_sim(initial_velocity) + + # Simulate physics + for _ in range(5): + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + # Non-deterministic when on GPU, so we use different tolerances + if device == "cuda:0": + tolerance = 1e-2 + else: + tolerance = 1e-5 + + torch.testing.assert_close( + cube_object.data.root_lin_vel_w, initial_velocity[:, :3], rtol=1e-5, atol=tolerance + ) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_with_static_friction(num_cubes, device): + """Test that static friction applied to rigid object works as expected. + + This test works by applying a force to the object and checking if the object moves or not based on the + mu (coefficient of static friction) value set for the object. We set the static friction to be non-zero and + apply a force to the object. When the force applied is below mu, the object should not move. When the force + applied is above mu, the object should move. + """ + with build_simulation_context(device=device, dt=0.01, add_ground_plane=False, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.03125, device=device) + + # Create ground plane + static_friction_coefficient = 0.5 + cfg = sim_utils.GroundPlaneCfg( + physics_material=materials.RigidBodyMaterialCfg( + static_friction=static_friction_coefficient, + dynamic_friction=static_friction_coefficient, # This shouldn't be required but is due to a bug in PhysX + ) + ) + cfg.func("/World/GroundPlane", cfg) + + # Play sim + sim.reset() + + # Set static friction to be non-zero + # Dynamic friction also needs to be zero due to a bug in PhysX + static_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) + dynamic_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) + restitution = torch.zeros(num_cubes, 1) + + cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) + + indices = torch.tensor(range(num_cubes), dtype=torch.int) + + # Add friction to cube + cube_object.root_physx_view.set_material_properties(cube_object_materials, indices) + + # let everything settle + for _ in range(100): + sim.step() + cube_object.update(sim.cfg.dt) + cube_object.write_root_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) + cube_mass = cube_object.root_physx_view.get_masses() + gravity_magnitude = abs(sim.cfg.gravity[2]) + # 2 cases: force applied is below and above mu + # below mu: block should not move as the force applied is <= mu + # above mu: block should move as the force applied is > mu + for force in "below_mu", "above_mu": + # set initial velocity to zero + cube_object.write_root_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) + + external_wrench_b = torch.zeros((num_cubes, 1, 6), device=sim.device) + if force == "below_mu": + external_wrench_b[..., 0] = static_friction_coefficient * cube_mass * gravity_magnitude * 0.99 + else: + external_wrench_b[..., 0] = static_friction_coefficient * cube_mass * gravity_magnitude * 1.01 + + # TODO: Replace with wrench composer once the deprecation is complete + cube_object.set_external_force_and_torque( + external_wrench_b[..., :3], + external_wrench_b[..., 3:], + ) + + # Get root state + initial_root_pos = cube_object.data.root_pos_w.clone() + # Simulate physics + for _ in range(200): + # apply the wrench + cube_object.write_data_to_sim() + sim.step() + # update object + cube_object.update(sim.cfg.dt) + if force == "below_mu": + # Assert that the block has not moved + torch.testing.assert_close(cube_object.data.root_pos_w, initial_root_pos, rtol=2e-3, atol=2e-3) + if force == "above_mu": + assert (cube_object.data.root_state_w[..., 0] - initial_root_pos[..., 0] > 0.02).all() + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_with_restitution(num_cubes, device): + """Test that restitution when applied to rigid object works as expected. + + This test works by dropping a block from a height and checking if the block bounces or not based on the + restitution value set for the object. We set the restitution to be non-zero and drop the block from a height. + When the restitution is 0, the block should not bounce. When the restitution is between 0 and 1, the block + should bounce with less energy. + """ + for expected_collision_type in "partially_elastic", "inelastic": + with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) + + # Set static friction to be non-zero + if expected_collision_type == "inelastic": + restitution_coefficient = 0.0 + elif expected_collision_type == "partially_elastic": + restitution_coefficient = 0.5 + + # Create ground plane such that has a restitution of 1.0 (perfectly elastic collision) + cfg = sim_utils.GroundPlaneCfg( + physics_material=materials.RigidBodyMaterialCfg( + restitution=restitution_coefficient, + ) + ) + cfg.func("/World/GroundPlane", cfg) + + indices = torch.tensor(range(num_cubes), dtype=torch.int) + + # Play sim + sim.reset() + + root_state = torch.zeros(num_cubes, 13, device=sim.device) + root_state[:, 3] = 1.0 # To make orientation a quaternion + for i in range(num_cubes): + root_state[i, 1] = 1.0 * i + root_state[:, 2] = 1.0 # Set an initial drop height + root_state[:, 9] = -1.0 # Set an initial downward velocity - def test_initialization(self): - """Test initialization for prim with rigid body API at the provided prim path.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Check that boundedness of rigid object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertTrue(cube_object.is_initialized) - self.assertEqual(len(cube_object.body_names), 1) - - # Check buffers that exists and have correct shapes - self.assertEqual(cube_object.data.root_pos_w.shape, (num_cubes, 3)) - self.assertEqual(cube_object.data.root_quat_w.shape, (num_cubes, 4)) - self.assertEqual(cube_object.data.default_mass.shape, (num_cubes, 1)) - self.assertEqual(cube_object.data.default_inertia.shape, (num_cubes, 9)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - def test_initialization_with_kinematic_enabled(self): - """Test that initialization for prim with kinematic flag enabled.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, origins = generate_cubes_scene( - num_cubes=num_cubes, kinematic_enabled=True, device=device - ) - - # Check that boundedness of rigid object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertTrue(cube_object.is_initialized) - self.assertEqual(len(cube_object.body_names), 1) - - # Check buffers that exists and have correct shapes - self.assertEqual(cube_object.data.root_pos_w.shape, (num_cubes, 3)) - self.assertEqual(cube_object.data.root_quat_w.shape, (num_cubes, 4)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - # check that the object is kinematic - default_root_state = cube_object.data.default_root_state.clone() - default_root_state[:, :3] += origins - torch.testing.assert_close(cube_object.data.root_state_w, default_root_state) - - def test_initialization_with_no_rigid_body(self): - """Test that initialization fails when no rigid body is found at the provided prim path.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, api="none", device=device) - - # Check that boundedness of rigid object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertFalse(cube_object.is_initialized) - - def test_initialization_with_articulation_root(self): - """Test that initialization fails when an articulation root is found at the provided prim path.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene( - num_cubes=num_cubes, api="articulation_root", device=device - ) - - # Check that boundedness of rigid object is correct - self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertFalse(cube_object.is_initialized) - - def test_external_force_buffer(self): - """Test if external force buffer correctly updates in the force value is zero case. - - In this test, we apply a non-zero force, then a zero force, then finally a non-zero force - to an object. We check if the force buffer is properly updated at each step. - """ - - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=1, device=device): - # Generate cubes scene - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, origins = generate_cubes_scene(num_cubes=1, device=device) - - # play the simulator - sim.reset() - - # find bodies to apply the force - body_ids, body_names = cube_object.find_bodies(".*") - - # reset object - cube_object.reset() - - # perform simulation - for step in range(5): - - # initiate force tensor - external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) - - if step == 0 or step == 3: - # set a non-zero force - force = 1 - else: - # set a zero force - force = 0 - - # set force value - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - # apply force - cube_object.set_external_force_and_torque( - external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids - ) - - # check if the cube's force and torque buffers are correctly updated - self.assertTrue(cube_object._external_force_b[0, 0, 0].item() == force) - self.assertTrue(cube_object._external_torque_b[0, 0, 0].item() == force) - - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) - - def test_external_force_on_single_body(self): - """Test application of external force on the base of the object. - - In this test, we apply a force equal to the weight of an object on the base of - one of the objects. We check that the object does not move. For the other object, - we do not apply any force and check that it falls down. - """ - for num_cubes in (2, 4): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - # Generate cubes scene - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - sim.reset() - - # Find bodies to apply the force - body_ids, body_names = cube_object.find_bodies(".*") - - # Sample a force equal to the weight of the object - external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) - # Every 2nd cube should have a force applied to it - external_wrench_b[0::2, :, 2] = 9.81 * cube_object.root_physx_view.get_masses()[0] - - # Now we are ready! - for _ in range(5): - # reset root state - root_state = cube_object.data.default_root_state.clone() - - # need to shift the position of the cubes otherwise they will be on top of each other - root_state[:, :3] = origins - cube_object.write_root_pose_to_sim(root_state[:, :7]) - cube_object.write_root_velocity_to_sim(root_state[:, 7:]) - - # reset object - cube_object.reset() - - # apply force - cube_object.set_external_force_and_torque( - external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids - ) - # perform simulation - for _ in range(5): - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) - - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - cube_object.data.root_pos_w[0::2, 2], torch.ones(num_cubes // 2, device=sim.device) - ) - # Second object should have fallen, so it's Z height should be less than initial height of 1.0 - self.assertTrue(torch.all(cube_object.data.root_pos_w[1::2, 2] < 1.0)) - - def test_set_rigid_object_state(self): - """Test setting the state of the rigid object. - - In this test, we set the state of the rigid object to a random state and check - that the object is in that state after simulation. We set gravity to zero as - we don't want any external forces acting on the object to ensure state remains static. - """ - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - # Turn off gravity for this test as we don't want any external forces acting on the object - # to ensure state remains static - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - sim.reset() - - state_types = ["root_pos_w", "root_quat_w", "root_lin_vel_w", "root_ang_vel_w"] - - # Set each state type individually as they are dependent on each other - for state_type_to_randomize in state_types: - state_dict = { - "root_pos_w": torch.zeros_like(cube_object.data.root_pos_w, device=sim.device), - "root_quat_w": default_orientation(num=num_cubes, device=sim.device), - "root_lin_vel_w": torch.zeros_like(cube_object.data.root_lin_vel_w, device=sim.device), - "root_ang_vel_w": torch.zeros_like(cube_object.data.root_ang_vel_w, device=sim.device), - } - - # Now we are ready! - for _ in range(5): - # reset object - cube_object.reset() - - # Set random state - if state_type_to_randomize == "root_quat_w": - state_dict[state_type_to_randomize] = random_orientation( - num=num_cubes, device=sim.device - ) - else: - state_dict[state_type_to_randomize] = torch.randn(num_cubes, 3, device=sim.device) - - # perform simulation - for _ in range(5): - root_state = torch.cat( - [ - state_dict["root_pos_w"], - state_dict["root_quat_w"], - state_dict["root_lin_vel_w"], - state_dict["root_ang_vel_w"], - ], - dim=-1, - ) - # reset root state - cube_object.write_root_pose_to_sim(root_state[:, :7]) - cube_object.write_root_velocity_to_sim(root_state[:, 7:]) - - sim.step() - - # assert that set root quantities are equal to the ones set in the state_dict - for key, expected_value in state_dict.items(): - value = getattr(cube_object.data, key) - torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) - - cube_object.update(sim.cfg.dt) - - def test_reset_rigid_object(self): - """Test resetting the state of the rigid object.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, gravity_enabled=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - sim.reset() - - for i in range(5): - # perform rendering - sim.step() - - # update object - cube_object.update(sim.cfg.dt) - - # Move the object to a random position - root_state = cube_object.data.default_root_state.clone() - root_state[:, :3] = torch.randn(num_cubes, 3, device=sim.device) - - # Random orientation - root_state[:, 3:7] = random_orientation(num=num_cubes, device=sim.device) - cube_object.write_root_pose_to_sim(root_state[:, :7]) - cube_object.write_root_velocity_to_sim(root_state[:, 7:]) - - if i % 2 == 0: - # reset object - cube_object.reset() - - # Reset should zero external forces and torques - self.assertFalse(cube_object.has_external_wrench) - self.assertEqual(torch.count_nonzero(cube_object._external_force_b), 0) - self.assertEqual(torch.count_nonzero(cube_object._external_torque_b), 0) - - def test_rigid_body_set_material_properties(self): - """Test getting and setting material properties of rigid object.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context( - device=device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play sim - sim.reset() - - # Set material properties - static_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) - dynamic_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) - restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) - - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - indices = torch.tensor(range(num_cubes), dtype=torch.int) - # Add friction to cube - cube_object.root_physx_view.set_material_properties(materials, indices) - - # Simulate physics - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Get material properties - materials_to_check = cube_object.root_physx_view.get_material_properties() - - # Check if material properties are set correctly - torch.testing.assert_close(materials_to_check.reshape(num_cubes, 3), materials) - - def test_rigid_body_no_friction(self): - """Test that a rigid object with no friction will maintain it's velocity when sliding across a plane.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - - # Create ground plane with no friction - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - static_friction=0.0, - dynamic_friction=0.0, - restitution=0.0, - ) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set material friction properties to be all zero - static_friction = torch.zeros(num_cubes, 1) - dynamic_friction = torch.zeros(num_cubes, 1) - restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - indices = torch.tensor(range(num_cubes), dtype=torch.int) - - cube_object.root_physx_view.set_material_properties(cube_object_materials, indices) - - # Set initial velocity - # Initial velocity in X to get the block moving - initial_velocity = torch.zeros((num_cubes, 6), device=sim.cfg.device) - initial_velocity[:, 0] = 0.1 - - cube_object.write_root_velocity_to_sim(initial_velocity) - - # Simulate physics - for _ in range(5): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Non-deterministic when on GPU, so we use different tolerances - if device == "cuda:0": - tolerance = 1e-2 - else: - tolerance = 1e-5 - - torch.testing.assert_close( - cube_object.data.root_lin_vel_w, initial_velocity[:, :3], rtol=1e-5, atol=tolerance - ) - - def test_rigid_body_with_static_friction(self): - """Test that static friction applied to rigid object works as expected. - - This test works by applying a force to the object and checking if the object moves or not based on the - mu (coefficient of static friction) value set for the object. We set the static friction to be non-zero and - apply a force to the object. When the force applied is below mu, the object should not move. When the force - applied is above mu, the object should move. - """ - for num_cubes in (1, 2): - for device in ("cuda", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context( - device=device, dt=0.01, add_ground_plane=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.03125, device=device) - - # Create ground plane - static_friction_coefficient = 0.5 - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - static_friction=static_friction_coefficient, - dynamic_friction=static_friction_coefficient, # This shouldn't be required but is due to a bug in PhysX - ) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set static friction to be non-zero - # Dynamic friction also needs to be zero due to a bug in PhysX - static_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) - dynamic_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) - restitution = torch.zeros(num_cubes, 1) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - indices = torch.tensor(range(num_cubes), dtype=torch.int) - - # Add friction to cube - cube_object.root_physx_view.set_material_properties(cube_object_materials, indices) - - # let everything settle - for _ in range(100): - sim.step() - cube_object.update(sim.cfg.dt) - cube_object.write_root_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) - cube_mass = cube_object.root_physx_view.get_masses() - gravity_magnitude = abs(sim.cfg.gravity[2]) - # 2 cases: force applied is below and above mu - # below mu: block should not move as the force applied is <= mu - # above mu: block should move as the force applied is > mu - for force in "below_mu", "above_mu": - with self.subTest(force=force): - # set initial velocity to zero - cube_object.write_root_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) - - external_wrench_b = torch.zeros((num_cubes, 1, 6), device=sim.device) - if force == "below_mu": - external_wrench_b[..., 0] = ( - static_friction_coefficient * cube_mass * gravity_magnitude * 0.99 - ) - else: - external_wrench_b[..., 0] = ( - static_friction_coefficient * cube_mass * gravity_magnitude * 1.01 - ) - - cube_object.set_external_force_and_torque( - external_wrench_b[..., :3], - external_wrench_b[..., 3:], - ) - - # Get root state - initial_root_pos = cube_object.data.root_pos_w.clone() - # Simulate physics - for _ in range(200): - # apply the wrench - cube_object.write_data_to_sim() - sim.step() - # update object - cube_object.update(sim.cfg.dt) - if force == "below_mu": - # Assert that the block has not moved - torch.testing.assert_close( - cube_object.data.root_pos_w, initial_root_pos, rtol=1e-3, atol=1e-3 - ) - if force == "above_mu": - self.assertTrue( - (cube_object.data.root_state_w[..., 0] - initial_root_pos[..., 0] > 0.02).all() - ) - - def test_rigid_body_with_restitution(self): - """Test that restitution when applied to rigid object works as expected. - - This test works by dropping a block from a height and checking if the block bounces or not based on the - restitution value set for the object. We set the restitution to be non-zero and drop the block from a height. - When the restitution is 0, the block should not bounce. When the restitution is between 0 and 1, the block - should bounce with less energy. - """ - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - for expected_collision_type in "partially_elastic", "inelastic": - with self.subTest( - expected_collision_type=expected_collision_type, num_cubes=num_cubes, device=device - ): - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) - - # Set static friction to be non-zero - if expected_collision_type == "inelastic": - restitution_coefficient = 0.0 - elif expected_collision_type == "partially_elastic": - restitution_coefficient = 0.5 - - # Create ground plane such that has a restitution of 1.0 (perfectly elastic collision) - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - restitution=restitution_coefficient, - ) - ) - cfg.func("/World/GroundPlane", cfg) - - indices = torch.tensor(range(num_cubes), dtype=torch.int) - - # Play sim - sim.reset() - - root_state = torch.zeros(num_cubes, 13, device=sim.device) - root_state[:, 3] = 1.0 # To make orientation a quaternion - for i in range(num_cubes): - root_state[i, 1] = 1.0 * i - root_state[:, 2] = 1.0 # Set an initial drop height - root_state[:, 9] = -1.0 # Set an initial downward velocity - - cube_object.write_root_pose_to_sim(root_state[:, :7]) - cube_object.write_root_velocity_to_sim(root_state[:, 7:]) - - static_friction = torch.zeros(num_cubes, 1) - dynamic_friction = torch.zeros(num_cubes, 1) - restitution = torch.Tensor([[restitution_coefficient]] * num_cubes) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - # Add restitution to cube - cube_object.root_physx_view.set_material_properties(cube_object_materials, indices) - - curr_z_velocity = cube_object.data.root_lin_vel_w[:, 2].clone() - - for _ in range(100): - sim.step() - - # update object - cube_object.update(sim.cfg.dt) - curr_z_velocity = cube_object.data.root_lin_vel_w[:, 2].clone() - - if expected_collision_type == "inelastic": - # assert that the block has not bounced by checking that the z velocity is less than or equal to 0 - self.assertTrue((curr_z_velocity <= 0.0).all()) - - if torch.all(curr_z_velocity <= 0.0): - # Still in the air - prev_z_velocity = curr_z_velocity - else: - # collision has happened, exit the for loop - break - - if expected_collision_type == "partially_elastic": - # Assert that the block has lost some energy by checking that the z velocity is less - self.assertTrue(torch.all(torch.le(abs(curr_z_velocity), abs(prev_z_velocity)))) - self.assertTrue((curr_z_velocity > 0.0).all()) - - def test_rigid_body_set_mass(self): - """Test getting and setting mass of rigid object.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context( - device=device, gravity_enabled=False, add_ground_plane=True, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) - - # Play sim - sim.reset() - - # Get masses before increasing - original_masses = cube_object.root_physx_view.get_masses() - - self.assertEqual(original_masses.shape, (num_cubes, 1)) - - # Randomize mass of the object - masses = original_masses + torch.FloatTensor(num_cubes, 1).uniform_(4, 8) - - indices = torch.tensor(range(num_cubes), dtype=torch.int) - - # Add friction to cube - cube_object.root_physx_view.set_masses(masses, indices) - - torch.testing.assert_close(cube_object.root_physx_view.get_masses(), masses) - - # Simulate physics - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - masses_to_check = cube_object.root_physx_view.get_masses() - - # Check if mass is set correctly - torch.testing.assert_close(masses, masses_to_check) - - def test_gravity_vec_w(self): - """Test that gravity vector direction is set correctly for the rigid object.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - for gravity_enabled in [True, False]: - with self.subTest(num_cubes=num_cubes, device=device, gravity_enabled=gravity_enabled): - with build_simulation_context(device=device, gravity_enabled=gravity_enabled) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Obtain gravity direction - if gravity_enabled: - gravity_dir = (0.0, 0.0, -1.0) - else: - gravity_dir = (0.0, 0.0, 0.0) - - # Play sim - sim.reset() - - # Check that gravity is set correctly - self.assertEqual(cube_object.data.GRAVITY_VEC_W[0, 0], gravity_dir[0]) - self.assertEqual(cube_object.data.GRAVITY_VEC_W[0, 1], gravity_dir[1]) - self.assertEqual(cube_object.data.GRAVITY_VEC_W[0, 2], gravity_dir[2]) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Expected gravity value is the acceleration of the body - gravity = torch.zeros(num_cubes, 1, 6, device=device) - if gravity_enabled: - gravity[:, :, 2] = -9.81 - # Check the body accelerations are correct - torch.testing.assert_close(cube_object.data.body_acc_w, gravity) - - def test_body_root_state_properties(self): - """Test the root_com_state_w, root_link_state_w, body_com_state_w, and body_link_state_w properties.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - for with_offset in [True, False]: - with self.subTest(num_cubes=num_cubes, device=device, with_offset=with_offset): - with build_simulation_context( - device=device, gravity_enabled=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - env_idx = torch.tensor([x for x in range(num_cubes)]) - - # Play sim - sim.reset() - - # Check if cube_object is initialized - self.assertTrue(cube_object.is_initialized) - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) - - com = cube_object.root_physx_view.get_coms() - com[..., :3] = offset.to("cpu") - cube_object.root_physx_view.set_coms(com, env_idx) - - # check ceter of mass has been set - torch.testing.assert_close(cube_object.root_physx_view.get_coms(), com) - - # random z spin velocity - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = torch.randn(1, device=device) - - # Simulate physics - for _ in range(100): - # spin the object around Z axis (com) - cube_object.write_root_velocity_to_sim(spin_twist.repeat(num_cubes, 1)) - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # get state properties - root_state_w = cube_object.data.root_state_w - root_link_state_w = cube_object.data.root_link_state_w - root_com_state_w = cube_object.data.root_com_state_w - body_state_w = cube_object.data.body_state_w - body_link_state_w = cube_object.data.body_link_state_w - body_com_state_w = cube_object.data.body_com_state_w - - # if offset is [0,0,0] all root_state_%_w will match and all body_%_w will match - if not with_offset: - torch.testing.assert_close(root_state_w, root_com_state_w) - torch.testing.assert_close(root_state_w, root_link_state_w) - torch.testing.assert_close(body_state_w, body_com_state_w) - torch.testing.assert_close(body_state_w, body_link_state_w) - else: - # cubes are spinning around center of mass - # position will not match - # center of mass position will be constant (i.e. spining around com) - torch.testing.assert_close(env_pos + offset, root_com_state_w[..., :3]) - torch.testing.assert_close(env_pos + offset, body_com_state_w[..., :3].squeeze(-2)) - # link position will be moving but should stay constant away from center of mass - root_link_state_pos_rel_com = quat_rotate_inverse( - root_link_state_w[..., 3:7], - root_link_state_w[..., :3] - root_com_state_w[..., :3], - ) - torch.testing.assert_close(-offset, root_link_state_pos_rel_com) - body_link_state_pos_rel_com = quat_rotate_inverse( - body_link_state_w[..., 3:7], - body_link_state_w[..., :3] - body_com_state_w[..., :3], - ) - torch.testing.assert_close(-offset, body_link_state_pos_rel_com.squeeze(-2)) - - # orientation of com will be a constant rotation from link orientation - com_quat_b = cube_object.data.com_quat_b - com_quat_w = quat_mul(body_link_state_w[..., 3:7], com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_state_w[..., 3:7]) - torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_state_w[..., 3:7]) - - # orientation of link will match root state will always match - torch.testing.assert_close(root_state_w[..., 3:7], root_link_state_w[..., 3:7]) - torch.testing.assert_close(body_state_w[..., 3:7], body_link_state_w[..., 3:7]) - - # lin_vel will not match - # center of mass vel will be constant (i.e. spining around com) - torch.testing.assert_close( - torch.zeros_like(root_com_state_w[..., 7:10]), root_com_state_w[..., 7:10] - ) - torch.testing.assert_close( - torch.zeros_like(body_com_state_w[..., 7:10]), body_com_state_w[..., 7:10] - ) - # link frame will be moving, and should be equal to input angular velocity cross offset - lin_vel_rel_root_gt = quat_rotate_inverse( - root_link_state_w[..., 3:7], root_link_state_w[..., 7:10] - ) - lin_vel_rel_body_gt = quat_rotate_inverse( - body_link_state_w[..., 3:7], body_link_state_w[..., 7:10] - ) - lin_vel_rel_gt = torch.linalg.cross( - spin_twist.repeat(num_cubes, 1)[..., 3:], -offset - ) - torch.testing.assert_close( - lin_vel_rel_gt, lin_vel_rel_root_gt, atol=1e-4, rtol=1e-4 - ) - torch.testing.assert_close( - lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), atol=1e-4, rtol=1e-4 - ) - - # ang_vel will always match - torch.testing.assert_close(root_state_w[..., 10:], root_com_state_w[..., 10:]) - torch.testing.assert_close(root_state_w[..., 10:], root_link_state_w[..., 10:]) - torch.testing.assert_close(body_state_w[..., 10:], body_com_state_w[..., 10:]) - torch.testing.assert_close(body_state_w[..., 10:], body_link_state_w[..., 10:]) - - def test_write_root_state(self): - """Test the setters for root_state using both the link frame and center of mass as reference frame.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - for with_offset in [True, False]: - for state_location in ("com", "link"): - with self.subTest(num_cubes=num_cubes, device=device, with_offset=with_offset): - with build_simulation_context( - device=device, gravity_enabled=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene( - num_cubes=num_cubes, height=0.0, device=device - ) - env_idx = torch.tensor([x for x in range(num_cubes)]) - - # Play sim - sim.reset() - - # Check if cube_object is initialized - self.assertTrue(cube_object.is_initialized) - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) - - com = cube_object.root_physx_view.get_coms() - com[..., :3] = offset.to("cpu") - cube_object.root_physx_view.set_coms(com, env_idx) - - # check ceter of mass has been set - torch.testing.assert_close(cube_object.root_physx_view.get_coms(), com) - - rand_state = torch.zeros_like(cube_object.data.root_state_w) - rand_state[..., :7] = cube_object.data.default_root_state[..., :7] - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_idx = env_idx.to(device) - for i in range(10): - - # perform step - sim.step() - # update buffers - cube_object.update(sim.cfg.dt) - - if state_location == "com": - if i % 2 == 0: - cube_object.write_root_com_state_to_sim(rand_state) - else: - cube_object.write_root_com_state_to_sim(rand_state, env_ids=env_idx) - elif state_location == "link": - if i % 2 == 0: - cube_object.write_root_link_state_to_sim(rand_state) - else: - cube_object.write_root_link_state_to_sim(rand_state, env_ids=env_idx) - - if state_location == "com": - torch.testing.assert_close(rand_state, cube_object.data.root_com_state_w) - elif state_location == "link": - torch.testing.assert_close(rand_state, cube_object.data.root_link_state_w) - - -if __name__ == "__main__": - run_tests() + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) + + static_friction = torch.zeros(num_cubes, 1) + dynamic_friction = torch.zeros(num_cubes, 1) + restitution = torch.Tensor([[restitution_coefficient]] * num_cubes) + + cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) + + # Add restitution to cube + cube_object.root_physx_view.set_material_properties(cube_object_materials, indices) + + curr_z_velocity = cube_object.data.root_lin_vel_w[:, 2].clone() + + for _ in range(100): + sim.step() + + # update object + cube_object.update(sim.cfg.dt) + curr_z_velocity = cube_object.data.root_lin_vel_w[:, 2].clone() + + if expected_collision_type == "inelastic": + # assert that the block has not bounced by checking that the z velocity is less than or equal to 0 + assert (curr_z_velocity <= 0.0).all() + + if torch.all(curr_z_velocity <= 0.0): + # Still in the air + prev_z_velocity = curr_z_velocity + else: + # collision has happened, exit the for loop + break + + if expected_collision_type == "partially_elastic": + # Assert that the block has lost some energy by checking that the z velocity is less + assert torch.all(torch.le(abs(curr_z_velocity), abs(prev_z_velocity))) + assert (curr_z_velocity > 0.0).all() + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_set_mass(num_cubes, device): + """Test getting and setting mass of rigid object.""" + with build_simulation_context( + device=device, gravity_enabled=False, add_ground_plane=True, auto_add_lighting=True + ) as sim: + sim._app_control_on_stop_handle = None + # Create a scene with random cubes + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) + + # Play sim + sim.reset() + + # Get masses before increasing + original_masses = cube_object.root_physx_view.get_masses() + + assert original_masses.shape == (num_cubes, 1) + + # Randomize mass of the object + masses = original_masses + torch.FloatTensor(num_cubes, 1).uniform_(4, 8) + + indices = torch.tensor(range(num_cubes), dtype=torch.int) + + # Add friction to cube + cube_object.root_physx_view.set_masses(masses, indices) + + torch.testing.assert_close(cube_object.root_physx_view.get_masses(), masses) + + # Simulate physics + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + masses_to_check = cube_object.root_physx_view.get_masses() + + # Check if mass is set correctly + torch.testing.assert_close(masses, masses_to_check) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("gravity_enabled", [True, False]) +@pytest.mark.isaacsim_ci +def test_gravity_vec_w(num_cubes, device, gravity_enabled): + """Test that gravity vector direction is set correctly for the rigid object.""" + with build_simulation_context(device=device, gravity_enabled=gravity_enabled) as sim: + sim._app_control_on_stop_handle = None + # Create a scene with random cubes + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Obtain gravity direction + if gravity_enabled: + gravity_dir = (0.0, 0.0, -1.0) + else: + gravity_dir = (0.0, 0.0, 0.0) + + # Play sim + sim.reset() + + # Check that gravity is set correctly + assert cube_object.data.GRAVITY_VEC_W[0, 0] == gravity_dir[0] + assert cube_object.data.GRAVITY_VEC_W[0, 1] == gravity_dir[1] + assert cube_object.data.GRAVITY_VEC_W[0, 2] == gravity_dir[2] + + # Simulate physics + for _ in range(2): + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + # Expected gravity value is the acceleration of the body + gravity = torch.zeros(num_cubes, 1, 6, device=device) + if gravity_enabled: + gravity[:, :, 2] = -9.81 + # Check the body accelerations are correct + torch.testing.assert_close(cube_object.data.body_acc_w, gravity) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@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: + sim._app_control_on_stop_handle = None + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + env_idx = torch.tensor([x for x in range(num_cubes)]) + + # Play sim + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + if with_offset: + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) + else: + offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + + com = cube_object.root_physx_view.get_coms() + com[..., :3] = offset.to("cpu") + cube_object.root_physx_view.set_coms(com, env_idx) + + # check ceter of mass has been set + torch.testing.assert_close(cube_object.root_physx_view.get_coms(), com) + + # random z spin velocity + spin_twist = torch.zeros(6, device=device) + spin_twist[5] = torch.randn(1, device=device) + + # Simulate physics + for _ in range(100): + # spin the object around Z axis (com) + cube_object.write_root_velocity_to_sim(spin_twist.repeat(num_cubes, 1)) + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + # get state properties + root_state_w = cube_object.data.root_state_w + root_link_state_w = cube_object.data.root_link_state_w + root_com_state_w = cube_object.data.root_com_state_w + body_state_w = cube_object.data.body_state_w + body_link_state_w = cube_object.data.body_link_state_w + body_com_state_w = cube_object.data.body_com_state_w + + # if offset is [0,0,0] all root_state_%_w will match and all body_%_w will match + if not with_offset: + torch.testing.assert_close(root_state_w, root_com_state_w) + torch.testing.assert_close(root_state_w, root_link_state_w) + torch.testing.assert_close(body_state_w, body_com_state_w) + torch.testing.assert_close(body_state_w, body_link_state_w) + else: + # cubes are spinning around center of mass + # position will not match + # center of mass position will be constant (i.e. spinning around com) + torch.testing.assert_close(env_pos + offset, root_com_state_w[..., :3]) + torch.testing.assert_close(env_pos + offset, body_com_state_w[..., :3].squeeze(-2)) + # link position will be moving but should stay constant away from center of mass + root_link_state_pos_rel_com = quat_apply_inverse( + root_link_state_w[..., 3:7], + root_link_state_w[..., :3] - root_com_state_w[..., :3], + ) + torch.testing.assert_close(-offset, root_link_state_pos_rel_com) + body_link_state_pos_rel_com = quat_apply_inverse( + body_link_state_w[..., 3:7], + body_link_state_w[..., :3] - body_com_state_w[..., :3], + ) + torch.testing.assert_close(-offset, body_link_state_pos_rel_com.squeeze(-2)) + + # orientation of com will be a constant rotation from link orientation + com_quat_b = cube_object.data.body_com_quat_b + com_quat_w = quat_mul(body_link_state_w[..., 3:7], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_state_w[..., 3:7]) + torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_state_w[..., 3:7]) + + # orientation of link will match root state will always match + torch.testing.assert_close(root_state_w[..., 3:7], root_link_state_w[..., 3:7]) + torch.testing.assert_close(body_state_w[..., 3:7], body_link_state_w[..., 3:7]) + + # lin_vel will not match + # center of mass vel will be constant (i.e. spinning around com) + torch.testing.assert_close(torch.zeros_like(root_com_state_w[..., 7:10]), root_com_state_w[..., 7:10]) + torch.testing.assert_close(torch.zeros_like(body_com_state_w[..., 7:10]), body_com_state_w[..., 7:10]) + # link frame will be moving, and should be equal to input angular velocity cross offset + lin_vel_rel_root_gt = quat_apply_inverse(root_link_state_w[..., 3:7], root_link_state_w[..., 7:10]) + lin_vel_rel_body_gt = quat_apply_inverse(body_link_state_w[..., 3:7], body_link_state_w[..., 7:10]) + lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_cubes, 1)[..., 3:], -offset) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_root_gt, atol=1e-4, rtol=1e-4) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), atol=1e-4, rtol=1e-4) + + # ang_vel will always match + torch.testing.assert_close(root_state_w[..., 10:], root_com_state_w[..., 10:]) + torch.testing.assert_close(root_state_w[..., 10:], root_link_state_w[..., 10:]) + torch.testing.assert_close(body_state_w[..., 10:], body_com_state_w[..., 10:]) + torch.testing.assert_close(body_state_w[..., 10:], body_link_state_w[..., 10:]) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.parametrize("state_location", ["com", "link"]) +@pytest.mark.isaacsim_ci +def test_write_root_state(num_cubes, device, with_offset, state_location): + """Test the setters for root_state using both the link frame and center of mass as reference frame.""" + with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + env_idx = torch.tensor([x for x in range(num_cubes)]) + + # Play sim + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + if with_offset: + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) + else: + offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + + com = cube_object.root_physx_view.get_coms() + com[..., :3] = offset.to("cpu") + cube_object.root_physx_view.set_coms(com, env_idx) + + # check center of mass has been set + torch.testing.assert_close(cube_object.root_physx_view.get_coms(), com) + + rand_state = torch.zeros_like(cube_object.data.root_state_w) + rand_state[..., :7] = cube_object.data.default_root_state[..., :7] + rand_state[..., :3] += env_pos + # make quaternion a unit vector + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + + env_idx = env_idx.to(device) + for i in range(10): + # perform step + sim.step() + # update buffers + cube_object.update(sim.cfg.dt) + + if state_location == "com": + if i % 2 == 0: + cube_object.write_root_com_state_to_sim(rand_state) + else: + cube_object.write_root_com_state_to_sim(rand_state, env_ids=env_idx) + elif state_location == "link": + if i % 2 == 0: + cube_object.write_root_link_state_to_sim(rand_state) + else: + cube_object.write_root_link_state_to_sim(rand_state, env_ids=env_idx) + + if state_location == "com": + torch.testing.assert_close(rand_state, cube_object.data.root_com_state_w) + elif state_location == "link": + torch.testing.assert_close(rand_state, cube_object.data.root_link_state_w) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True]) +@pytest.mark.parametrize("state_location", ["com", "link", "root"]) +@pytest.mark.isaacsim_ci +def test_write_state_functions_data_consistency(num_cubes, device, with_offset, state_location): + """Test the setters for root_state using both the link frame and center of mass as reference frame.""" + with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + env_idx = torch.tensor([x for x in range(num_cubes)]) + + # Play sim + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + if with_offset: + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) + else: + offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + + com = cube_object.root_physx_view.get_coms() + com[..., :3] = offset.to("cpu") + cube_object.root_physx_view.set_coms(com, env_idx) + + # check ceter of mass has been set + torch.testing.assert_close(cube_object.root_physx_view.get_coms(), com) + + rand_state = torch.rand_like(cube_object.data.root_state_w) + # rand_state[..., :7] = cube_object.data.default_root_state[..., :7] + rand_state[..., :3] += env_pos + # make quaternion a unit vector + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + + env_idx = env_idx.to(device) + + # perform step + sim.step() + # update buffers + cube_object.update(sim.cfg.dt) + + if state_location == "com": + cube_object.write_root_com_state_to_sim(rand_state) + elif state_location == "link": + cube_object.write_root_link_state_to_sim(rand_state) + elif state_location == "root": + cube_object.write_root_state_to_sim(rand_state) + + if state_location == "com": + expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( + cube_object.data.root_com_state_w[:, :3], + cube_object.data.root_com_state_w[:, 3:7], + quat_rotate( + quat_inv(cube_object.data.body_com_pose_b[:, 0, 3:7]), -cube_object.data.body_com_pose_b[:, 0, :3] + ), + quat_inv(cube_object.data.body_com_pose_b[:, 0, 3:7]), + ) + expected_root_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1) + # test both root_pose and root_link_state_w successfully updated when root_com_state_w updates + torch.testing.assert_close(expected_root_link_pose, cube_object.data.root_link_state_w[:, :7]) + # skip 7:10 because they differs from link frame, this should be fine because we are only checking + # if velocity update is triggered, which can be determined by comparing angular velocity + torch.testing.assert_close( + cube_object.data.root_com_state_w[:, 10:], cube_object.data.root_link_state_w[:, 10:] + ) + torch.testing.assert_close(expected_root_link_pose, cube_object.data.root_state_w[:, :7]) + torch.testing.assert_close(cube_object.data.root_com_state_w[:, 10:], cube_object.data.root_state_w[:, 10:]) + elif state_location == "link": + expected_com_pos, expected_com_quat = combine_frame_transforms( + cube_object.data.root_link_state_w[:, :3], + cube_object.data.root_link_state_w[:, 3:7], + cube_object.data.body_com_pose_b[:, 0, :3], + cube_object.data.body_com_pose_b[:, 0, 3:7], + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) + # test both root_pose and root_com_state_w successfully updated when root_link_state_w updates + torch.testing.assert_close(expected_com_pose, cube_object.data.root_com_state_w[:, :7]) + # skip 7:10 because they differs from link frame, this should be fine because we are only checking + # if velocity update is triggered, which can be determined by comparing angular velocity + torch.testing.assert_close( + cube_object.data.root_link_state_w[:, 10:], cube_object.data.root_com_state_w[:, 10:] + ) + torch.testing.assert_close(cube_object.data.root_link_state_w[:, :7], cube_object.data.root_state_w[:, :7]) + torch.testing.assert_close( + cube_object.data.root_link_state_w[:, 10:], cube_object.data.root_state_w[:, 10:] + ) + elif state_location == "root": + expected_com_pos, expected_com_quat = combine_frame_transforms( + cube_object.data.root_state_w[:, :3], + cube_object.data.root_state_w[:, 3:7], + cube_object.data.body_com_pose_b[:, 0, :3], + cube_object.data.body_com_pose_b[:, 0, 3:7], + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) + # test both root_com_state_w and root_link_state_w successfully updated when root_pose updates + torch.testing.assert_close(expected_com_pose, cube_object.data.root_com_state_w[:, :7]) + torch.testing.assert_close(cube_object.data.root_state_w[:, 7:], cube_object.data.root_com_state_w[:, 7:]) + torch.testing.assert_close(cube_object.data.root_state_w[:, :7], cube_object.data.root_link_state_w[:, :7]) + torch.testing.assert_close( + cube_object.data.root_state_w[:, 10:], cube_object.data.root_link_state_w[:, 10:] + ) diff --git a/source/isaaclab/test/assets/test_rigid_object_collection.py b/source/isaaclab/test/assets/test_rigid_object_collection.py index ffbfc1a9152b..8d919bf4d7ad 100644 --- a/source/isaaclab/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab/test/assets/test_rigid_object_collection.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,29 +9,32 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests - -# Can set this to False to see the GUI for debugging -# This will also add lights to the scene -HEADLESS = True +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import ctypes -import torch -import unittest -import isaacsim.core.utils.prims as prim_utils +import pytest +import torch import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg, RigidObjectCollection, RigidObjectCollectionCfg from isaaclab.sim import build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR -from isaaclab.utils.math import default_orientation, quat_mul, quat_rotate_inverse, random_orientation +from isaaclab.utils.math import ( + combine_frame_transforms, + default_orientation, + quat_apply_inverse, + quat_inv, + quat_mul, + quat_rotate, + random_orientation, + subtract_frame_transforms, +) def generate_cubes_scene( @@ -59,7 +62,7 @@ def generate_cubes_scene( origins = torch.tensor([(i * 3.0, 0, height) for i in range(num_envs)]).to(device) # Create Top-level Xforms, one for each cube for i, origin in enumerate(origins): - prim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) + sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) # Resolve spawn configuration if has_api: @@ -90,695 +93,747 @@ def generate_cubes_scene( return cube_object_colection, origins -class TestRigidObjectCollection(unittest.TestCase): - """Test for rigid object collection class.""" +@pytest.fixture +def sim(request): + """Create simulation context with the specified device.""" + device = request.getfixturevalue("device") + if "gravity_enabled" in request.fixturenames: + gravity_enabled = request.getfixturevalue("gravity_enabled") + else: + gravity_enabled = True # default to gravity enabled + with build_simulation_context(device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled) as sim: + sim._app_control_on_stop_handle = None + yield sim + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_cubes", [1, 3]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_initialization(sim, num_envs, num_cubes, device): + """Test initialization for prim with rigid body API at the provided prim path.""" + object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + + # Check that boundedness of rigid object is correct + assert ctypes.c_long.from_address(id(object_collection)).value == 1 + + # Play sim + sim.reset() + + # Check if object is initialized + assert object_collection.is_initialized + assert len(object_collection.object_names) == num_cubes + + # Check buffers that exist and have correct shapes + assert object_collection.data.object_link_pos_w.shape == (num_envs, num_cubes, 3) + assert object_collection.data.object_link_quat_w.shape == (num_envs, num_cubes, 4) + assert object_collection.data.default_mass.shape == (num_envs, num_cubes, 1) + assert object_collection.data.default_inertia.shape == (num_envs, num_cubes, 9) + + # Simulate physics + for _ in range(2): + sim.step() + object_collection.update(sim.cfg.dt) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_id_conversion(sim, device): + """Test environment and object index conversion to physics view indices.""" + object_collection, _ = generate_cubes_scene(num_envs=2, num_cubes=3, device=device) + + # Play sim + sim.reset() + + expected = [ + torch.tensor([4, 5], device=device, dtype=torch.long), + torch.tensor([4], device=device, dtype=torch.long), + torch.tensor([0, 2, 4], device=device, dtype=torch.long), + torch.tensor([1, 3, 5], device=device, dtype=torch.long), + ] + + view_ids = object_collection._env_obj_ids_to_view_ids( + object_collection._ALL_ENV_INDICES, object_collection._ALL_OBJ_INDICES[None, 2] + ) + assert (view_ids == expected[0]).all() + view_ids = object_collection._env_obj_ids_to_view_ids( + object_collection._ALL_ENV_INDICES[None, 0], object_collection._ALL_OBJ_INDICES[None, 2] + ) + assert (view_ids == expected[1]).all() + view_ids = object_collection._env_obj_ids_to_view_ids( + object_collection._ALL_ENV_INDICES[None, 0], object_collection._ALL_OBJ_INDICES + ) + assert (view_ids == expected[2]).all() + view_ids = object_collection._env_obj_ids_to_view_ids( + object_collection._ALL_ENV_INDICES[None, 1], object_collection._ALL_OBJ_INDICES + ) + assert (view_ids == expected[3]).all() + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_cubes", [1, 3]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_initialization_with_kinematic_enabled(sim, num_envs, num_cubes, device): + """Test that initialization for prim with kinematic flag enabled.""" + object_collection, origins = generate_cubes_scene( + num_envs=num_envs, num_cubes=num_cubes, kinematic_enabled=True, device=device + ) + + # Check that boundedness of rigid object is correct + assert ctypes.c_long.from_address(id(object_collection)).value == 1 + + # Play sim + sim.reset() + + # Check if object is initialized + assert object_collection.is_initialized + assert len(object_collection.object_names) == num_cubes + + # Check buffers that exist and have correct shapes + assert object_collection.data.object_link_pos_w.shape == (num_envs, num_cubes, 3) + assert object_collection.data.object_link_quat_w.shape == (num_envs, num_cubes, 4) + + # Simulate physics + for _ in range(2): + sim.step() + object_collection.update(sim.cfg.dt) + # check that the object is kinematic + default_object_state = object_collection.data.default_object_state.clone() + default_object_state[..., :3] += origins.unsqueeze(1) + torch.testing.assert_close(object_collection.data.object_link_state_w, default_object_state) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_initialization_with_no_rigid_body(sim, num_cubes, device): + """Test that initialization fails when no rigid body is found at the provided prim path.""" + object_collection, _ = generate_cubes_scene(num_cubes=num_cubes, has_api=False, device=device) + + # Check that boundedness of rigid object is correct + assert ctypes.c_long.from_address(id(object_collection)).value == 1 + + # Play sim + with pytest.raises(RuntimeError): + sim.reset() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_external_force_buffer(sim, device): + """Test if external force buffer correctly updates in the force value is zero case.""" + num_envs = 2 + num_cubes = 1 + object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + sim.reset() + + # find objects to apply the force + object_ids, object_names = object_collection.find_objects(".*") + # reset object + object_collection.reset() + + # perform simulation + for step in range(5): + # initiate force tensor + external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) + + # decide if zero or non-zero force + if step == 0 or step == 3: + force = 1.0 + else: + force = 0.0 + + # apply force to the object + external_wrench_b[:, :, 0] = force + external_wrench_b[:, :, 3] = force + + object_collection.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + body_ids=object_ids, + env_ids=None, + ) + + # check if the object collection's force and torque buffers are correctly updated + for i in range(num_envs): + assert object_collection._permanent_wrench_composer.composed_force_as_torch[i, 0, 0].item() == force + assert object_collection._permanent_wrench_composer.composed_torque_as_torch[i, 0, 0].item() == force + object_collection.instantaneous_wrench_composer.add_forces_and_torques( + body_ids=object_ids, + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + ) + + # apply action to the object collection + object_collection.write_data_to_sim() + sim.step() + object_collection.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_cubes", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_external_force_on_single_body(sim, num_envs, num_cubes, device): + """Test application of external force on the base of the object.""" + object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + sim.reset() + + # find objects to apply the force + object_ids, object_names = object_collection.find_objects(".*") + + # Sample a force equal to the weight of the object + external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[:, 0::2, 2] = 9.81 * object_collection.data.default_mass[:, 0::2, 0] + + for i in range(5): + # reset object state + object_state = object_collection.data.default_object_state.clone() + # need to shift the position of the cubes otherwise they will be on top of each other + object_state[..., :2] += origins.unsqueeze(1)[..., :2] + object_collection.write_object_state_to_sim(object_state) + # reset object + object_collection.reset() + + is_global = False + if i % 2 == 0: + positions = object_collection.data.object_link_pos_w[:, object_ids, :3] + is_global = True + else: + positions = None + + # apply force + object_collection.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=positions, + body_ids=object_ids, + env_ids=None, + is_global=is_global, + ) + for _ in range(10): + # write data to sim + object_collection.write_data_to_sim() + # step sim + sim.step() + # update object collection + object_collection.update(sim.cfg.dt) + + # First object should still be at the same Z position (1.0) + torch.testing.assert_close( + object_collection.data.object_link_pos_w[:, 0::2, 2], + torch.ones_like(object_collection.data.object_pos_w[:, 0::2, 2]), + ) + # Second object should have fallen, so it's Z height should be less than initial height of 1.0 + assert torch.all(object_collection.data.object_link_pos_w[:, 1::2, 2] < 1.0) + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_cubes", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_external_force_on_single_body_at_position(sim, num_envs, num_cubes, device): + """Test application of external force on the base of the object at a specific position. + + In this test, we apply a force equal to the weight of an object on the base of + one of the objects at 1m in the Y direction, we check that the object rotates around it's X axis. + For the other object, we do not apply any force and check that it falls down. """ - Tests - """ + object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + sim.reset() + + # find objects to apply the force + object_ids, object_names = object_collection.find_objects(".*") + + # Sample a force equal to the weight of the object + external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) + external_wrench_positions_b = torch.zeros(object_collection.num_instances, len(object_ids), 3, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[:, 0::2, 2] = 500.0 + external_wrench_positions_b[:, 0::2, 1] = 1.0 + + # Desired force and torque + for i in range(5): + # reset object state + object_state = object_collection.data.default_object_state.clone() + # need to shift the position of the cubes otherwise they will be on top of each other + object_state[..., :2] += origins.unsqueeze(1)[..., :2] + object_collection.write_object_state_to_sim(object_state) + # reset object + object_collection.reset() + + is_global = False + if i % 2 == 0: + body_com_pos_w = object_collection.data.object_link_pos_w[:, object_ids, :3] + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + external_wrench_positions_b += body_com_pos_w + is_global = True + else: + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + + # apply force + object_collection.permanent_wrench_composer.set_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=object_ids, + env_ids=None, + is_global=is_global, + ) + object_collection.permanent_wrench_composer.add_forces_and_torques( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=object_ids, + is_global=is_global, + ) - def test_initialization(self): - """Test initialization for prim with rigid body API at the provided prim path.""" - for num_envs in (1, 2): - for num_cubes in (1, 3): - for device in ("cuda:0", "cpu"): - with self.subTest(num_envs=num_envs, num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - object_collection, _ = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, device=device - ) - - # Check that boundedness of rigid object is correct - self.assertEqual(ctypes.c_long.from_address(id(object_collection)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertTrue(object_collection.is_initialized) - self.assertEqual(len(object_collection.object_names), num_cubes) - - # Check buffers that exists and have correct shapes - self.assertEqual(object_collection.data.object_link_pos_w.shape, (num_envs, num_cubes, 3)) - self.assertEqual(object_collection.data.object_link_quat_w.shape, (num_envs, num_cubes, 4)) - self.assertEqual(object_collection.data.default_mass.shape, (num_envs, num_cubes, 1)) - self.assertEqual(object_collection.data.default_inertia.shape, (num_envs, num_cubes, 9)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - object_collection.update(sim.cfg.dt) - - def test_id_conversion(self): - """Test environment and object index conversion to physics view indices.""" - for device in ("cuda:0", "cpu"): - with self.subTest(num_envs=2, num_cubes=3, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - object_collection, _ = generate_cubes_scene(num_envs=2, num_cubes=3, device=device) - - # Play sim - sim.reset() - - expected = [ - torch.tensor([4, 5], device=device, dtype=torch.long), - torch.tensor([4], device=device, dtype=torch.long), - torch.tensor([0, 2, 4], device=device, dtype=torch.long), - torch.tensor([1, 3, 5], device=device, dtype=torch.long), - ] - - view_ids = object_collection._env_obj_ids_to_view_ids( - object_collection._ALL_ENV_INDICES, object_collection._ALL_OBJ_INDICES[None, 2] - ) - self.assertTrue((view_ids == expected[0]).all()) - view_ids = object_collection._env_obj_ids_to_view_ids( - object_collection._ALL_ENV_INDICES[None, 0], object_collection._ALL_OBJ_INDICES[None, 2] - ) - self.assertTrue((view_ids == expected[1]).all()) - view_ids = object_collection._env_obj_ids_to_view_ids( - object_collection._ALL_ENV_INDICES[None, 0], object_collection._ALL_OBJ_INDICES - ) - self.assertTrue((view_ids == expected[2]).all()) - view_ids = object_collection._env_obj_ids_to_view_ids( - object_collection._ALL_ENV_INDICES[None, 1], object_collection._ALL_OBJ_INDICES - ) - self.assertTrue((view_ids == expected[3]).all()) - - def test_initialization_with_kinematic_enabled(self): - """Test that initialization for prim with kinematic flag enabled.""" - for num_envs in (1, 2): - for num_cubes in (1, 3): - for device in ("cuda:0", "cpu"): - with self.subTest(num_envs=num_envs, num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - object_collection, origins = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, kinematic_enabled=True, device=device - ) - - # Check that boundedness of rigid object is correct - self.assertEqual(ctypes.c_long.from_address(id(object_collection)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertTrue(object_collection.is_initialized) - self.assertEqual(len(object_collection.object_names), num_cubes) - - # Check buffers that exists and have correct shapes - self.assertEqual(object_collection.data.object_link_pos_w.shape, (num_envs, num_cubes, 3)) - self.assertEqual(object_collection.data.object_link_quat_w.shape, (num_envs, num_cubes, 4)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - object_collection.update(sim.cfg.dt) - # check that the object is kinematic - default_object_state = object_collection.data.default_object_state.clone() - default_object_state[..., :3] += origins.unsqueeze(1) - torch.testing.assert_close( - object_collection.data.object_link_state_w, default_object_state - ) - - def test_initialization_with_no_rigid_body(self): - """Test that initialization fails when no rigid body is found at the provided prim path.""" - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=num_cubes, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - object_collection, _ = generate_cubes_scene(num_cubes=num_cubes, has_api=False, device=device) - - # Check that boundedness of rigid object is correct - self.assertEqual(ctypes.c_long.from_address(id(object_collection)).value, 1) - - # Play sim - sim.reset() - - # Check if object is initialized - self.assertFalse(object_collection.is_initialized) - - def test_external_force_buffer(self): - """Test if external force buffer correctly updates in the force value is zero case. - - In this test, we apply a non-zero force, then a zero force, then finally a non-zero force - to an object collection. We check if the force buffer is properly updated at each step. - """ - - num_envs = 2 - num_cubes = 1 - for device in ("cuda:0", "cpu"): - with self.subTest(num_cubes=1, device=device): - # Generate cubes scene - with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, origins = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, device=device - ) - # play the simulator - sim.reset() - - # find objects to apply the force - object_ids, object_names = object_collection.find_objects(".*") - - # reset object - object_collection.reset() - - # perform simulation - for step in range(5): - - # initiate force tensor - external_wrench_b = torch.zeros( - object_collection.num_instances, len(object_ids), 6, device=sim.device - ) - - if step == 0 or step == 3: - # set a non-zero force - force = 1 - else: - # set a zero force - force = 0 - - # set force value - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - # apply force - object_collection.set_external_force_and_torque( - external_wrench_b[..., :3], external_wrench_b[..., 3:], object_ids=object_ids - ) - - # check if the object collection's force and torque buffers are correctly updated - for i in range(num_envs): - self.assertTrue(object_collection._external_force_b[i, 0, 0].item() == force) - self.assertTrue(object_collection._external_torque_b[i, 0, 0].item() == force) - - # apply action to the object collection - object_collection.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - object_collection.update(sim.cfg.dt) - - def test_external_force_on_single_body(self): - """Test application of external force on the base of the object. - - In this test, we apply a force equal to the weight of an object on the base of - one of the objects. We check that the object does not move. For the other object, - we do not apply any force and check that it falls down. - """ - for num_envs in (1, 2): - for num_cubes in (1, 4): - for device in ("cuda:0", "cpu"): - with self.subTest(num_envs=num_envs, num_cubes=num_cubes, device=device): - # Generate cubes scene - with build_simulation_context( - device=device, add_ground_plane=True, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - object_collection, origins = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, device=device - ) - - # Play the simulator - sim.reset() - - # Find objects to apply the force - object_ids, object_names = object_collection.find_objects(".*") - - # Sample a force equal to the weight of the object - external_wrench_b = torch.zeros( - object_collection.num_instances, len(object_ids), 6, device=sim.device - ) - # Every 2nd cube should have a force applied to it - external_wrench_b[:, 0::2, 2] = 9.81 * object_collection.data.default_mass[:, 0::2, 0] - - # Now we are ready! - for _ in range(5): - # reset object state - object_state = object_collection.data.default_object_state.clone() - - # need to shift the position of the cubes otherwise they will be on top of each other - object_state[..., :2] += origins.unsqueeze(1)[..., :2] - object_collection.write_object_state_to_sim(object_state) - - # reset object - object_collection.reset() - - # apply force - object_collection.set_external_force_and_torque( - external_wrench_b[..., :3], external_wrench_b[..., 3:], object_ids=object_ids - ) - # perform simulation - for _ in range(10): - # apply action to the object - object_collection.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - object_collection.update(sim.cfg.dt) - - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - object_collection.data.object_link_pos_w[:, 0::2, 2], - torch.ones_like(object_collection.data.object_pos_w[:, 0::2, 2]), - ) - # Second object should have fallen, so it's Z height should be less than initial height of 1.0 - self.assertTrue(torch.all(object_collection.data.object_link_pos_w[:, 1::2, 2] < 1.0)) - - def test_set_object_state(self): - """Test setting the state of the object. - - In this test, we set the state of the object to a random state and check - that the object is in that state after simulation. We set gravity to zero as - we don't want any external forces acting on the object to ensure state remains static. - """ - for num_envs in (1, 3): - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_envs=num_envs, num_cubes=num_cubes, device=device): - # Turn off gravity for this test as we don't want any external forces acting on the object - # to ensure state remains static - with build_simulation_context( - device=device, gravity_enabled=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - object_collection, origins = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, device=device - ) - - # Play the simulator - sim.reset() - - state_types = ["object_pos_w", "object_quat_w", "object_lin_vel_w", "object_ang_vel_w"] - - # Set each state type individually as they are dependent on each other - for state_type_to_randomize in state_types: - state_dict = { - "object_pos_w": torch.zeros_like( - object_collection.data.object_pos_w, device=sim.device - ), - "object_quat_w": default_orientation( - num=num_cubes * num_envs, device=sim.device - ).view(num_envs, num_cubes, 4), - "object_lin_vel_w": torch.zeros_like( - object_collection.data.object_lin_vel_w, device=sim.device - ), - "object_ang_vel_w": torch.zeros_like( - object_collection.data.object_ang_vel_w, device=sim.device - ), - } - - # Now we are ready! - for _ in range(5): - # reset object - object_collection.reset() - - # Set random state - if state_type_to_randomize == "object_quat_w": - state_dict[state_type_to_randomize] = random_orientation( - num=num_cubes * num_envs, device=sim.device - ).view(num_envs, num_cubes, 4) - else: - state_dict[state_type_to_randomize] = torch.randn( - num_envs, num_cubes, 3, device=sim.device - ) - # make sure objects do not overlap - if state_type_to_randomize == "object_pos_w": - state_dict[state_type_to_randomize][..., :2] += origins.unsqueeze(1)[ - ..., :2 - ] - - # perform simulation - for _ in range(5): - object_state = torch.cat( - [ - state_dict["object_pos_w"], - state_dict["object_quat_w"], - state_dict["object_lin_vel_w"], - state_dict["object_ang_vel_w"], - ], - dim=-1, - ) - # reset object state - object_collection.write_object_state_to_sim(object_state=object_state) - - sim.step() - - # assert that set object quantities are equal to the ones set in the state_dict - for key, expected_value in state_dict.items(): - value = getattr(object_collection.data, key) - torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) - - object_collection.update(sim.cfg.dt) - - def test_object_state_properties(self): - """Test the object_com_state_w and object_link_state_w properties.""" - for num_envs in (1, 4): - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - for with_offset in [True, False]: - with self.subTest( - num_envs=num_envs, num_cubes=num_cubes, device=device, with_offset=with_offset - ): - with build_simulation_context( - device=device, gravity_enabled=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device - ) - view_ids = torch.tensor([x for x in range(num_cubes * num_envs)]) - - # Play sim - sim.reset() - - # Check if cube_object is initialized - self.assertTrue(cube_object.is_initialized) - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - - com = cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()) - com[..., :3] = offset.to("cpu") - cube_object.root_physx_view.set_coms( - cube_object.reshape_data_to_view(com.clone()), view_ids - ) - - # check center of mass has been set - torch.testing.assert_close( - cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()), com - ) - - # random z spin velocity - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = torch.randn(1, device=device) - - # initial spawn point - init_com = cube_object.data.object_com_state_w[..., :3] - - # Simulate physics - for i in range(10): - # spin the object around Z axis (com) - cube_object.write_object_com_velocity_to_sim( - spin_twist.repeat(num_envs, num_cubes, 1) - ) - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # get state properties - object_state_w = cube_object.data.object_state_w - object_link_state_w = cube_object.data.object_link_state_w - object_com_state_w = cube_object.data.object_com_state_w - - # if offset is [0,0,0] all object_state_%_w will match and all body_%_w will match - if not with_offset: - torch.testing.assert_close(object_state_w, object_com_state_w) - torch.testing.assert_close(object_state_w, object_link_state_w) - else: - # cubes are spinning around center of mass - # position will not match - # center of mass position will be constant (i.e. spinning around com) - torch.testing.assert_close(init_com, object_com_state_w[..., :3]) - - # link position will be moving but should stay constant away from center of mass - object_link_state_pos_rel_com = quat_rotate_inverse( - object_link_state_w[..., 3:7], - object_link_state_w[..., :3] - object_com_state_w[..., :3], - ) - - torch.testing.assert_close(-offset, object_link_state_pos_rel_com) - - # orientation of com will be a constant rotation from link orientation - com_quat_b = cube_object.data.com_quat_b - com_quat_w = quat_mul(object_link_state_w[..., 3:7], com_quat_b) - torch.testing.assert_close(com_quat_w, object_com_state_w[..., 3:7]) - - # orientation of link will match object state will always match - torch.testing.assert_close( - object_state_w[..., 3:7], object_link_state_w[..., 3:7] - ) - - # lin_vel will not match - # center of mass vel will be constant (i.e. spining around com) - torch.testing.assert_close( - torch.zeros_like(object_com_state_w[..., 7:10]), - object_com_state_w[..., 7:10], - ) - - # link frame will be moving, and should be equal to input angular velocity cross offset - lin_vel_rel_object_gt = quat_rotate_inverse( - object_link_state_w[..., 3:7], object_link_state_w[..., 7:10] - ) - lin_vel_rel_gt = torch.linalg.cross( - spin_twist.repeat(num_envs, num_cubes, 1)[..., 3:], -offset - ) - torch.testing.assert_close( - lin_vel_rel_gt, lin_vel_rel_object_gt, atol=1e-4, rtol=1e-3 - ) - - # ang_vel will always match - torch.testing.assert_close( - object_state_w[..., 10:], object_com_state_w[..., 10:] - ) - torch.testing.assert_close( - object_state_w[..., 10:], object_link_state_w[..., 10:] - ) - - def test_write_object_state(self): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" - for num_envs in (1, 3): - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - for with_offset in [True, False]: - for state_location in ("com", "link"): - with self.subTest( - num_envs=num_envs, num_cubes=num_cubes, device=device, with_offset=with_offset - ): - with build_simulation_context( - device=device, gravity_enabled=False, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device - ) - view_ids = torch.tensor([x for x in range(num_cubes * num_cubes)]) - env_ids = torch.tensor([x for x in range(num_envs)]) - object_ids = torch.tensor([x for x in range(num_cubes)]) - - # Play sim - sim.reset() - - # Check if cube_object is initialized - self.assertTrue(cube_object.is_initialized) - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat( - num_envs, num_cubes, 1 - ) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat( - num_envs, num_cubes, 1 - ) - - com = cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()) - com[..., :3] = offset.to("cpu") - cube_object.root_physx_view.set_coms( - cube_object.reshape_data_to_view(com.clone()), view_ids - ) - - # check center of mass has been set - torch.testing.assert_close( - cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()), com - ) - - rand_state = torch.zeros_like(cube_object.data.object_link_state_w) - rand_state[..., :7] = cube_object.data.default_object_state[..., :7] - rand_state[..., :3] += cube_object.data.object_link_pos_w - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_ids = env_ids.to(device) - object_ids = object_ids.to(device) - for i in range(10): - - # perform step - sim.step() - # update buffers - cube_object.update(sim.cfg.dt) - - if state_location == "com": - if i % 2 == 0: - cube_object.write_object_com_state_to_sim(rand_state) - else: - cube_object.write_object_com_state_to_sim( - rand_state, env_ids=env_ids, object_ids=object_ids - ) - elif state_location == "link": - if i % 2 == 0: - cube_object.write_object_link_state_to_sim(rand_state) - else: - cube_object.write_object_link_state_to_sim( - rand_state, env_ids=env_ids, object_ids=object_ids - ) - - if state_location == "com": - torch.testing.assert_close(rand_state, cube_object.data.object_com_state_w) - elif state_location == "link": - torch.testing.assert_close(rand_state, cube_object.data.object_link_state_w) - - def test_reset_object_collection(self): - """Test resetting the state of the rigid object.""" - for num_envs in (1, 3): - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_envs=num_envs, num_cubes=num_cubes, device=device): - with build_simulation_context( - device=device, gravity_enabled=True, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - object_collection, _ = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, device=device - ) - - # Play the simulator - sim.reset() - - for i in range(5): - # perform rendering - sim.step() - - # update object - object_collection.update(sim.cfg.dt) - - # Move the object to a random position - object_state = object_collection.data.default_object_state.clone() - object_state[..., :3] = torch.randn(num_envs, num_cubes, 3, device=sim.device) - - # Random orientation - object_state[..., 3:7] = random_orientation(num=num_cubes, device=sim.device) - object_collection.write_object_state_to_sim(object_state) - - if i % 2 == 0: - # reset object - object_collection.reset() - - # Reset should zero external forces and torques - self.assertFalse(object_collection.has_external_wrench) - self.assertEqual(torch.count_nonzero(object_collection._external_force_b), 0) - self.assertEqual(torch.count_nonzero(object_collection._external_torque_b), 0) - - def test_set_material_properties(self): - """Test getting and setting material properties of rigid object.""" - for num_envs in (1, 3): - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - with self.subTest(num_envs=num_envs, num_cubes=num_cubes, device=device): - with build_simulation_context( - device=device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - object_collection, _ = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, device=device - ) - - # Play sim - sim.reset() - - # Set material properties - static_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) - dynamic_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) - restitution = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.0, 0.2) - - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - indices = torch.tensor(range(num_cubes * num_envs), dtype=torch.int) - # Add friction to cube - object_collection.root_physx_view.set_material_properties( - object_collection.reshape_data_to_view(materials), indices - ) - - # Simulate physics - sim.step() - # update object - object_collection.update(sim.cfg.dt) - - # Get material properties - materials_to_check = object_collection.root_physx_view.get_material_properties() - - # Check if material properties are set correctly - torch.testing.assert_close( - object_collection.reshape_view_to_data(materials_to_check), materials - ) - - def test_gravity_vec_w(self): - """Test that gravity vector direction is set correctly for the rigid object.""" - for num_envs in (1, 3): - for num_cubes in (1, 2): - for device in ("cuda:0", "cpu"): - for gravity_enabled in [True, False]: - with self.subTest( - num_envs=num_envs, num_cubes=num_cubes, device=device, gravity_enabled=gravity_enabled - ): - with build_simulation_context(device=device, gravity_enabled=gravity_enabled) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - object_collection, _ = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, device=device - ) - - # Obtain gravity direction - if gravity_enabled: - gravity_dir = (0.0, 0.0, -1.0) - else: - gravity_dir = (0.0, 0.0, 0.0) - - # Play sim - sim.reset() - - # Check that gravity is set correctly - self.assertEqual(object_collection.data.GRAVITY_VEC_W[0, 0, 0], gravity_dir[0]) - self.assertEqual(object_collection.data.GRAVITY_VEC_W[0, 0, 1], gravity_dir[1]) - self.assertEqual(object_collection.data.GRAVITY_VEC_W[0, 0, 2], gravity_dir[2]) - - # Simulate physics - for _ in range(2): - sim.step() - # update object - object_collection.update(sim.cfg.dt) - - # Expected gravity value is the acceleration of the body - gravity = torch.zeros(num_envs, num_cubes, 6, device=device) - if gravity_enabled: - gravity[..., 2] = -9.81 - # Check the body accelerations are correct - torch.testing.assert_close(object_collection.data.object_acc_w, gravity) - - -if __name__ == "__main__": - run_tests() + for _ in range(10): + # write data to sim + object_collection.write_data_to_sim() + # step sim + sim.step() + # update object collection + object_collection.update(sim.cfg.dt) + + # First object should be rotating around it's X axis + assert torch.all(object_collection.data.object_ang_vel_b[:, 0::2, 0] > 0.1) + # Second object should have fallen, so it's Z height should be less than initial height of 1.0 + assert torch.all(object_collection.data.object_link_pos_w[:, 1::2, 2] < 1.0) + + +@pytest.mark.parametrize("num_envs", [1, 3]) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +def test_set_object_state(sim, num_envs, num_cubes, device, gravity_enabled): + """Test setting the state of the object. + + .. note:: + Turn off gravity for this test as we don't want any external forces acting on the object + to ensure state remains static + """ + object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + sim.reset() + + state_types = ["object_pos_w", "object_quat_w", "object_lin_vel_w", "object_ang_vel_w"] + + # Set each state type individually as they are dependent on each other + for state_type_to_randomize in state_types: + state_dict = { + "object_pos_w": torch.zeros_like(object_collection.data.object_pos_w, device=sim.device), + "object_quat_w": default_orientation(num=num_cubes * num_envs, device=sim.device).view( + num_envs, num_cubes, 4 + ), + "object_lin_vel_w": torch.zeros_like(object_collection.data.object_lin_vel_w, device=sim.device), + "object_ang_vel_w": torch.zeros_like(object_collection.data.object_ang_vel_w, device=sim.device), + } + + for _ in range(5): + # reset object + object_collection.reset() + + # Set random state + if state_type_to_randomize == "object_quat_w": + state_dict[state_type_to_randomize] = random_orientation( + num=num_cubes * num_envs, device=sim.device + ).view(num_envs, num_cubes, 4) + else: + state_dict[state_type_to_randomize] = torch.randn(num_envs, num_cubes, 3, device=sim.device) + # make sure objects do not overlap + if state_type_to_randomize == "object_pos_w": + state_dict[state_type_to_randomize][..., :2] += origins.unsqueeze(1)[..., :2] + + # perform simulation + for _ in range(5): + object_state = torch.cat( + [ + state_dict["object_pos_w"], + state_dict["object_quat_w"], + state_dict["object_lin_vel_w"], + state_dict["object_ang_vel_w"], + ], + dim=-1, + ) + # reset object state + object_collection.write_object_state_to_sim(object_state=object_state) + sim.step() + + # assert that set object quantities are equal to the ones set in the state_dict + for key, expected_value in state_dict.items(): + value = getattr(object_collection.data, key) + torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) + + object_collection.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_envs", [1, 4]) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.parametrize("gravity_enabled", [False]) +def test_object_state_properties(sim, num_envs, num_cubes, device, with_offset, gravity_enabled): + """Test the object_com_state_w and object_link_state_w properties.""" + cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) + view_ids = torch.tensor([x for x in range(num_cubes * num_envs)]) + + sim.reset() + + # check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + offset = ( + torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) + if with_offset + else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) + ) + + com = cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()) + com[..., :3] = offset.to("cpu") + cube_object.root_physx_view.set_coms(cube_object.reshape_data_to_view(com.clone()), view_ids) + + # check center of mass has been set + torch.testing.assert_close(cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()), com) + + # random z spin velocity + spin_twist = torch.zeros(6, device=device) + spin_twist[5] = torch.randn(1, device=device) + + # initial spawn point + init_com = cube_object.data.object_com_state_w[..., :3] + + for i in range(10): + # spin the object around Z axis (com) + cube_object.write_object_velocity_to_sim(spin_twist.repeat(num_envs, num_cubes, 1)) + sim.step() + cube_object.update(sim.cfg.dt) + + # get state properties + object_state_w = cube_object.data.object_state_w + object_link_state_w = cube_object.data.object_link_state_w + object_com_state_w = cube_object.data.object_com_state_w + + # if offset is [0,0,0] all object_state_%_w will match and all body_%_w will match + if not with_offset: + torch.testing.assert_close(object_state_w, object_com_state_w) + torch.testing.assert_close(object_state_w, object_link_state_w) + else: + # cubes are spinning around center of mass + # position will not match + # center of mass position will be constant (i.e. spinning around com) + torch.testing.assert_close(init_com, object_com_state_w[..., :3]) + + # link position will be moving but should stay constant away from center of mass + object_link_state_pos_rel_com = quat_apply_inverse( + object_link_state_w[..., 3:7], + object_link_state_w[..., :3] - object_com_state_w[..., :3], + ) + + torch.testing.assert_close(-offset, object_link_state_pos_rel_com) + + # orientation of com will be a constant rotation from link orientation + com_quat_b = cube_object.data.object_com_quat_b + com_quat_w = quat_mul(object_link_state_w[..., 3:7], com_quat_b) + torch.testing.assert_close(com_quat_w, object_com_state_w[..., 3:7]) + + # orientation of link will match object state will always match + torch.testing.assert_close(object_state_w[..., 3:7], object_link_state_w[..., 3:7]) + + # lin_vel will not match + # center of mass vel will be constant (i.e. spinning around com) + torch.testing.assert_close( + torch.zeros_like(object_com_state_w[..., 7:10]), + object_com_state_w[..., 7:10], + ) + + # link frame will be moving, and should be equal to input angular velocity cross offset + lin_vel_rel_object_gt = quat_apply_inverse(object_link_state_w[..., 3:7], object_link_state_w[..., 7:10]) + lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_envs, num_cubes, 1)[..., 3:], -offset) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_object_gt, atol=1e-4, rtol=1e-3) + + # ang_vel will always match + torch.testing.assert_close(object_state_w[..., 10:], object_com_state_w[..., 10:]) + torch.testing.assert_close(object_state_w[..., 10:], object_link_state_w[..., 10:]) + + +@pytest.mark.parametrize("num_envs", [1, 3]) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.parametrize("state_location", ["com", "link"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +def test_write_object_state(sim, num_envs, num_cubes, device, with_offset, state_location, gravity_enabled): + """Test the setters for object_state using both the link frame and center of mass as reference frame.""" + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) + view_ids = torch.tensor([x for x in range(num_cubes * num_cubes)]) + env_ids = torch.tensor([x for x in range(num_envs)]) + object_ids = torch.tensor([x for x in range(num_cubes)]) + + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + offset = ( + torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) + if with_offset + else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) + ) + + com = cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()) + com[..., :3] = offset.to("cpu") + cube_object.root_physx_view.set_coms(cube_object.reshape_data_to_view(com.clone()), view_ids) + + # check center of mass has been set + torch.testing.assert_close(cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()), com) + + rand_state = torch.zeros_like(cube_object.data.object_link_state_w) + rand_state[..., :7] = cube_object.data.default_object_state[..., :7] + rand_state[..., :3] += cube_object.data.object_link_pos_w + # make quaternion a unit vector + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + + env_ids = env_ids.to(device) + object_ids = object_ids.to(device) + for i in range(10): + sim.step() + cube_object.update(sim.cfg.dt) + + if state_location == "com": + if i % 2 == 0: + cube_object.write_object_com_state_to_sim(rand_state) + else: + cube_object.write_object_com_state_to_sim(rand_state, env_ids=env_ids, object_ids=object_ids) + elif state_location == "link": + if i % 2 == 0: + cube_object.write_object_link_state_to_sim(rand_state) + else: + cube_object.write_object_link_state_to_sim(rand_state, env_ids=env_ids, object_ids=object_ids) + + if state_location == "com": + torch.testing.assert_close(rand_state, cube_object.data.object_com_state_w) + elif state_location == "link": + torch.testing.assert_close(rand_state, cube_object.data.object_link_state_w) + + +@pytest.mark.parametrize("num_envs", [1, 3]) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_reset_object_collection(sim, num_envs, num_cubes, device): + """Test resetting the state of the rigid object.""" + object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + sim.reset() + + for i in range(5): + sim.step() + object_collection.update(sim.cfg.dt) + + # Move the object to a random position + object_state = object_collection.data.default_object_state.clone() + object_state[..., :3] = torch.randn(num_envs, num_cubes, 3, device=sim.device) + # Random orientation + object_state[..., 3:7] = random_orientation(num=num_cubes, device=sim.device) + object_collection.write_object_state_to_sim(object_state) + + if i % 2 == 0: + object_collection.reset() + + # Reset should zero external forces and torques + assert not object_collection._instantaneous_wrench_composer.active + assert not object_collection._permanent_wrench_composer.active + assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_force_as_torch) == 0 + assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_torque_as_torch) == 0 + assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_force_as_torch) == 0 + assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_torque_as_torch) == 0 + + +@pytest.mark.parametrize("num_envs", [1, 3]) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_set_material_properties(sim, num_envs, num_cubes, device): + """Test getting and setting material properties of rigid object.""" + object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + sim.reset() + + # Set material properties + static_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) + dynamic_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) + restitution = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.0, 0.2) + + materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) + + # Add friction to cube + indices = torch.tensor(range(num_cubes * num_envs), dtype=torch.int) + object_collection.root_physx_view.set_material_properties( + object_collection.reshape_data_to_view(materials), indices + ) + + # Perform simulation + sim.step() + object_collection.update(sim.cfg.dt) + + # Get material properties + materials_to_check = object_collection.root_physx_view.get_material_properties() + + # Check if material properties are set correctly + torch.testing.assert_close(object_collection.reshape_view_to_data(materials_to_check), materials) + + +@pytest.mark.parametrize("num_envs", [1, 3]) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("gravity_enabled", [True, False]) +def test_gravity_vec_w(sim, num_envs, num_cubes, device, gravity_enabled): + """Test that gravity vector direction is set correctly for the rigid object.""" + object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) + + # Obtain gravity direction + gravity_dir = (0.0, 0.0, -1.0) if gravity_enabled else (0.0, 0.0, 0.0) + + sim.reset() + + # Check if gravity vector is set correctly + assert object_collection.data.GRAVITY_VEC_W[0, 0, 0] == gravity_dir[0] + assert object_collection.data.GRAVITY_VEC_W[0, 0, 1] == gravity_dir[1] + assert object_collection.data.GRAVITY_VEC_W[0, 0, 2] == gravity_dir[2] + + # Perform simulation + for _ in range(2): + sim.step() + object_collection.update(sim.cfg.dt) + + # Expected gravity value is the acceleration of the body + gravity = torch.zeros(num_envs, num_cubes, 6, device=device) + if gravity_enabled: + gravity[..., 2] = -9.81 + + # Check the body accelerations are correct + torch.testing.assert_close(object_collection.data.object_acc_w, gravity) + + +@pytest.mark.parametrize("num_envs", [1, 3]) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True]) +@pytest.mark.parametrize("state_location", ["com", "link", "root"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +def test_write_object_state_functions_data_consistency( + sim, num_envs, num_cubes, device, with_offset, state_location, gravity_enabled +): + """Test the setters for object_state using both the link frame and center of mass as reference frame.""" + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) + view_ids = torch.tensor([x for x in range(num_cubes * num_cubes)]) + env_ids = torch.tensor([x for x in range(num_envs)]) + object_ids = torch.tensor([x for x in range(num_cubes)]) + + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + offset = ( + torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) + if with_offset + else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) + ) + + com = cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()) + com[..., :3] = offset.to("cpu") + cube_object.root_physx_view.set_coms(cube_object.reshape_data_to_view(com.clone()), view_ids) + + # check center of mass has been set + torch.testing.assert_close(cube_object.reshape_view_to_data(cube_object.root_physx_view.get_coms()), com) + + rand_state = torch.rand_like(cube_object.data.object_link_state_w) + rand_state[..., :3] += cube_object.data.object_link_pos_w + # make quaternion a unit vector + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + + env_ids = env_ids.to(device) + object_ids = object_ids.to(device) + sim.step() + cube_object.update(sim.cfg.dt) + + object_link_to_com_pos, object_link_to_com_quat = subtract_frame_transforms( + cube_object.data.object_link_state_w[..., :3].view(-1, 3), + cube_object.data.object_link_state_w[..., 3:7].view(-1, 4), + cube_object.data.object_com_state_w[..., :3].view(-1, 3), + cube_object.data.object_com_state_w[..., 3:7].view(-1, 4), + ) + + if state_location == "com": + cube_object.write_object_com_state_to_sim(rand_state, env_ids=env_ids, object_ids=object_ids) + elif state_location == "link": + cube_object.write_object_link_state_to_sim(rand_state, env_ids=env_ids, object_ids=object_ids) + elif state_location == "root": + cube_object.write_object_state_to_sim(rand_state, env_ids=env_ids, object_ids=object_ids) + + if state_location == "com": + expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( + cube_object.data.object_com_state_w[..., :3].view(-1, 3), + cube_object.data.object_com_state_w[..., 3:7].view(-1, 4), + quat_rotate(quat_inv(object_link_to_com_quat), -object_link_to_com_pos), + quat_inv(object_link_to_com_quat), + ) + # torch.testing.assert_close(rand_state, cube_object.data.object_com_state_w) + expected_object_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1).view( + num_envs, -1, 7 + ) + # test both root_pose and root_link_state_w successfully updated when root_com_state_w updates + torch.testing.assert_close(expected_object_link_pose, cube_object.data.object_link_state_w[..., :7]) + # skip 7:10 because they differs from link frame, this should be fine because we are only checking + # if velocity update is triggered, which can be determined by comparing angular velocity + torch.testing.assert_close( + cube_object.data.object_com_state_w[..., 10:], cube_object.data.object_link_state_w[..., 10:] + ) + torch.testing.assert_close(expected_object_link_pose, cube_object.data.object_state_w[..., :7]) + torch.testing.assert_close( + cube_object.data.object_com_state_w[..., 10:], cube_object.data.object_state_w[..., 10:] + ) + elif state_location == "link": + expected_com_pos, expected_com_quat = combine_frame_transforms( + cube_object.data.object_link_state_w[..., :3].view(-1, 3), + cube_object.data.object_link_state_w[..., 3:7].view(-1, 4), + object_link_to_com_pos, + object_link_to_com_quat, + ) + expected_object_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1).view(num_envs, -1, 7) + # test both root_pose and root_com_state_w successfully updated when root_link_state_w updates + torch.testing.assert_close(expected_object_com_pose, cube_object.data.object_com_state_w[..., :7]) + # skip 7:10 because they differs from link frame, this should be fine because we are only checking + # if velocity update is triggered, which can be determined by comparing angular velocity + torch.testing.assert_close( + cube_object.data.object_link_state_w[..., 10:], cube_object.data.object_com_state_w[..., 10:] + ) + torch.testing.assert_close( + cube_object.data.object_link_state_w[..., :7], cube_object.data.object_state_w[..., :7] + ) + torch.testing.assert_close( + cube_object.data.object_link_state_w[..., 10:], cube_object.data.object_state_w[..., 10:] + ) + elif state_location == "root": + expected_object_com_pos, expected_object_com_quat = combine_frame_transforms( + cube_object.data.object_state_w[..., :3].view(-1, 3), + cube_object.data.object_state_w[..., 3:7].view(-1, 4), + object_link_to_com_pos, + object_link_to_com_quat, + ) + expected_object_com_pose = torch.cat((expected_object_com_pos, expected_object_com_quat), dim=1).view( + num_envs, -1, 7 + ) + # test both root_com_state_w and root_link_state_w successfully updated when root_pose updates + torch.testing.assert_close(expected_object_com_pose, cube_object.data.object_com_state_w[..., :7]) + torch.testing.assert_close( + cube_object.data.object_state_w[..., 7:], cube_object.data.object_com_state_w[..., 7:] + ) + torch.testing.assert_close( + cube_object.data.object_state_w[..., :7], cube_object.data.object_link_state_w[..., :7] + ) + torch.testing.assert_close( + cube_object.data.object_state_w[..., 10:], cube_object.data.object_link_state_w[..., 10:] + ) diff --git a/source/isaaclab/test/assets/test_surface_gripper.py b/source/isaaclab/test/assets/test_surface_gripper.py new file mode 100644 index 000000000000..86f112fdf984 --- /dev/null +++ b/source/isaaclab/test/assets/test_surface_gripper.py @@ -0,0 +1,221 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest +import torch + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ( + Articulation, + ArticulationCfg, + RigidObject, + RigidObjectCfg, + SurfaceGripper, + SurfaceGripperCfg, +) +from isaaclab.sim import build_simulation_context +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.version import get_isaac_sim_version + +# from isaacsim.robot.surface_gripper import GripperView + + +def generate_surface_gripper_cfgs( + kinematic_enabled: bool = False, + max_grip_distance: float = 0.1, + coaxial_force_limit: float = 100.0, + shear_force_limit: float = 100.0, + retry_interval: float = 0.1, + reset_xform_op_properties: bool = False, +) -> tuple[SurfaceGripperCfg, ArticulationCfg]: + """Generate a surface gripper cfg and an articulation cfg. + + Args: + max_grip_distance: The maximum grip distance of the surface gripper. + coaxial_force_limit: The coaxial force limit of the surface gripper. + shear_force_limit: The shear force limit of the surface gripper. + retry_interval: The retry interval of the surface gripper. + reset_xform_op_properties: Whether to reset the xform op properties of the surface gripper. + + Returns: + A tuple containing the surface gripper cfg and the articulation cfg. + """ + articulation_cfg = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/SurfaceGripper/test_gripper.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(1.0, 0.0, 0.0, 0.0), + joint_pos={ + ".*": 0.0, + }, + ), + actuators={ + "dummy": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=0.0, + damping=0.0, + ), + }, + ) + + surface_gripper_cfg = SurfaceGripperCfg( + max_grip_distance=max_grip_distance, + coaxial_force_limit=coaxial_force_limit, + shear_force_limit=shear_force_limit, + retry_interval=retry_interval, + ) + + return surface_gripper_cfg, articulation_cfg + + +def generate_surface_gripper( + surface_gripper_cfg: SurfaceGripperCfg, + articulation_cfg: ArticulationCfg, + num_surface_grippers: int, + device: str, +) -> tuple[SurfaceGripper, Articulation, torch.Tensor]: + """Generate a surface gripper and an articulation. + + Args: + surface_gripper_cfg: The surface gripper cfg. + articulation_cfg: The articulation cfg. + num_surface_grippers: The number of surface grippers to generate. + device: The device to run the test on. + + Returns: + A tuple containing the surface gripper, the articulation, and the translations of the surface grippers. + """ + # Generate translations of 2.5 m in x for each articulation + translations = torch.zeros(num_surface_grippers, 3, device=device) + translations[:, 0] = torch.arange(num_surface_grippers) * 2.5 + + # Create Top-level Xforms, one for each articulation + for i in range(num_surface_grippers): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) + articulation = Articulation(articulation_cfg.replace(prim_path="/World/Env_.*/Robot")) + surface_gripper_cfg = surface_gripper_cfg.replace(prim_path="/World/Env_.*/Robot/Gripper/SurfaceGripper") + surface_gripper = SurfaceGripper(surface_gripper_cfg) + + return surface_gripper, articulation, translations + + +def generate_grippable_object(sim, num_grippable_objects: int): + object_cfg = RigidObjectCfg( + prim_path="/World/Env_.*/Object", + spawn=sim_utils.CuboidCfg( + size=(1.0, 1.0, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.5)), + ) + grippable_object = RigidObject(object_cfg) + + return grippable_object + + +@pytest.fixture +def sim(request): + """Create simulation context with the specified device.""" + device = request.getfixturevalue("device") + if "gravity_enabled" in request.fixturenames: + gravity_enabled = request.getfixturevalue("gravity_enabled") + else: + gravity_enabled = True # default to gravity enabled + if "add_ground_plane" in request.fixturenames: + add_ground_plane = request.getfixturevalue("add_ground_plane") + else: + add_ground_plane = False # default to no ground plane + with build_simulation_context( + device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane + ) as sim: + sim._app_control_on_stop_handle = None + yield sim + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cpu"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_initialization(sim, num_articulations, device, add_ground_plane) -> None: + """Test initialization for articulation with a surface gripper. + + This test verifies that: + 1. The surface gripper is initialized correctly. + 2. The command and state buffers have the correct shapes. + 3. The command and state are initialized to the correct values. + + Args: + num_articulations: The number of articulations to initialize. + device: The device to run the test on. + add_ground_plane: Whether to add a ground plane to the simulation. + """ + if get_isaac_sim_version().major < 5: + return + surface_gripper_cfg, articulation_cfg = generate_surface_gripper_cfgs(kinematic_enabled=False) + surface_gripper, articulation, _ = generate_surface_gripper( + surface_gripper_cfg, articulation_cfg, num_articulations, device + ) + + sim.reset() + + assert articulation.is_initialized + assert surface_gripper.is_initialized + + # Check that the command and state buffers have the correct shapes + assert surface_gripper.command.shape == (num_articulations,) + assert surface_gripper.state.shape == (num_articulations,) + + # Check that the command and state are initialized to the correct values + assert surface_gripper.command == 0.0 # Idle command after a reset + assert surface_gripper.state == -1.0 # Open state after a reset + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update articulation + articulation.update(sim.cfg.dt) + surface_gripper.update(sim.cfg.dt) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.isaacsim_ci +def test_raise_error_if_not_cpu(sim, device, add_ground_plane) -> None: + """Test that the SurfaceGripper raises an error if the device is not CPU.""" + if get_isaac_sim_version().major < 5: + return + num_articulations = 1 + surface_gripper_cfg, articulation_cfg = generate_surface_gripper_cfgs(kinematic_enabled=False) + surface_gripper, articulation, translations = generate_surface_gripper( + surface_gripper_cfg, articulation_cfg, num_articulations, device + ) + + with pytest.raises(Exception): + sim.reset() + + +if __name__ == "__main__": + pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab/test/controllers/simplified_test_robot.urdf b/source/isaaclab/test/controllers/simplified_test_robot.urdf new file mode 100644 index 000000000000..b66ce68324bb --- /dev/null +++ b/source/isaaclab/test/controllers/simplified_test_robot.urdf @@ -0,0 +1,191 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 000000000000..80a839a847f4 --- /dev/null +++ b/source/isaaclab/test/controllers/test_controller_utils.py @@ -0,0 +1,662 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test 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 pytest +import torch + +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_differential_ik.py b/source/isaaclab/test/controllers/test_differential_ik.py index 400af80dce33..65ce828129f3 100644 --- a/source/isaaclab/test/controllers/test_differential_ik.py +++ b/source/isaaclab/test/controllers/test_differential_ik.py @@ -1,22 +1,20 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" +import pytest import torch -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils from isaacsim.core.cloner import GridCloner import isaaclab.sim as sim_utils @@ -37,186 +35,194 @@ from isaaclab_assets import FRANKA_PANDA_HIGH_PD_CFG, UR10_CFG # isort:skip -class TestDifferentialIKController(unittest.TestCase): - """Test fixture for checking that differential IK controller tracks commands properly.""" - - def setUp(self): - """Create a blank new stage for each test.""" - # Wait for spawning - stage_utils.create_new_stage() - # Constants - self.num_envs = 128 - # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=0.01) - self.sim = sim_utils.SimulationContext(sim_cfg) - # TODO: Remove this once we have a better way to handle this. - self.sim._app_control_on_stop_handle = None - - # Create a ground plane - cfg = sim_utils.GroundPlaneCfg() - cfg.func("/World/GroundPlane", cfg) - - # Create interface to clone the scene - cloner = GridCloner(spacing=2.0) - cloner.define_base_env("/World/envs") - self.env_prim_paths = cloner.generate_paths("/World/envs/env", self.num_envs) - # create source prim - prim_utils.define_prim(self.env_prim_paths[0], "Xform") - # clone the env xform - self.env_origins = cloner.clone( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - replicate_physics=True, - ) - - # Define goals for the arm - ee_goals_set = [ - [0.5, 0.5, 0.7, 0.707, 0, 0.707, 0], - [0.5, -0.4, 0.6, 0.707, 0.707, 0.0, 0.0], - [0.5, 0, 0.5, 0.0, 1.0, 0.0, 0.0], - ] - self.ee_pose_b_des_set = torch.tensor(ee_goals_set, device=self.sim.device) - - def tearDown(self): - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Test fixtures. +@pytest.fixture +def sim(): + """Create a simulation context for testing.""" + # Wait for spawning + stage = sim_utils.create_new_stage() + # Constants + num_envs = 128 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=0.01) + sim = sim_utils.SimulationContext(sim_cfg) + # TODO: Remove this once we have a better way to handle this. + sim._app_control_on_stop_handle = None + + # Create a ground plane + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/GroundPlane", cfg) + + # Create interface to clone the scene + cloner = GridCloner(spacing=2.0) + cloner.define_base_env("/World/envs") + env_prim_paths = cloner.generate_paths("/World/envs/env", num_envs) + # create source prim + stage.DefinePrim(env_prim_paths[0], "Xform") + # clone the env xform + cloner.clone( + source_prim_path=env_prim_paths[0], + prim_paths=env_prim_paths, + replicate_physics=True, + ) + + # Define goals for the arm + ee_goals_set = [ + [0.5, 0.5, 0.7, 0.707, 0, 0.707, 0], + [0.5, -0.4, 0.6, 0.707, 0.707, 0.0, 0.0], + [0.5, 0, 0.5, 0.0, 1.0, 0.0, 0.0], + ] + ee_pose_b_des_set = torch.tensor(ee_goals_set, device=sim.device) + + yield sim, num_envs, ee_pose_b_des_set + + # Cleanup + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +def test_franka_ik_pose_abs(sim): + """Test IK controller for Franka arm with Franka hand.""" + sim_context, num_envs, ee_pose_b_des_set = sim + + # Create robot instance + robot_cfg = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="/World/envs/env_.*/Robot") + robot = Articulation(cfg=robot_cfg) + + # Create IK controller + diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls") + diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=num_envs, device=sim_context.device) + + # Run the controller and check that it converges to the goal + _run_ik_controller( + robot, diff_ik_controller, "panda_hand", ["panda_joint.*"], sim_context, num_envs, ee_pose_b_des_set + ) + + +def test_ur10_ik_pose_abs(sim): + """Test IK controller for UR10 arm.""" + sim_context, num_envs, ee_pose_b_des_set = sim + + # Create robot instance + robot_cfg = UR10_CFG.replace(prim_path="/World/envs/env_.*/Robot") + robot_cfg.spawn.rigid_props.disable_gravity = True + robot = Articulation(cfg=robot_cfg) + + # Create IK controller + diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls") + diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=num_envs, device=sim_context.device) + + # Run the controller and check that it converges to the goal + _run_ik_controller(robot, diff_ik_controller, "ee_link", [".*"], sim_context, num_envs, ee_pose_b_des_set) + + +def _run_ik_controller( + robot: Articulation, + diff_ik_controller: DifferentialIKController, + ee_frame_name: str, + arm_joint_names: list[str], + sim: sim_utils.SimulationContext, + num_envs: int, + ee_pose_b_des_set: torch.Tensor, +): + """Run the IK controller with the given parameters. + + Args: + robot (Articulation): The robot to control. + diff_ik_controller (DifferentialIKController): The differential IK controller. + ee_frame_name (str): The name of the end-effector frame. + arm_joint_names (list[str]): The names of the arm joints. + sim (sim_utils.SimulationContext): The simulation context. + num_envs (int): The number of environments. + ee_pose_b_des_set (torch.Tensor): The set of desired end-effector poses. """ - - def test_franka_ik_pose_abs(self): - """Test IK controller for Franka arm with Franka hand.""" - # Create robot instance - robot_cfg = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="/World/envs/env_.*/Robot") - robot = Articulation(cfg=robot_cfg) - - # Create IK controller - diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls") - diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=self.num_envs, device=self.sim.device) - - # Run the controller and check that it converges to the goal - self._run_ik_controller(robot, diff_ik_controller, "panda_hand", ["panda_joint.*"]) - - def test_ur10_ik_pose_abs(self): - """Test IK controller for UR10 arm.""" - # Create robot instance - robot_cfg = UR10_CFG.replace(prim_path="/World/envs/env_.*/Robot") - robot_cfg.spawn.rigid_props.disable_gravity = True - robot = Articulation(cfg=robot_cfg) - - # Create IK controller - diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls") - diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=self.num_envs, device=self.sim.device) - - # Run the controller and check that it converges to the goal - self._run_ik_controller(robot, diff_ik_controller, "ee_link", [".*"]) - - """ - Helper functions. - """ - - def _run_ik_controller( - self, - robot: Articulation, - diff_ik_controller: DifferentialIKController, - ee_frame_name: str, - arm_joint_names: list[str], - ): - # Define simulation stepping - sim_dt = self.sim.get_physics_dt() - # Play the simulator - self.sim.reset() - - # Obtain the frame index of the end-effector - ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0] - ee_jacobi_idx = ee_frame_idx - 1 - # Obtain joint indices - arm_joint_ids = robot.find_joints(arm_joint_names)[0] - # Update existing buffers - # Note: We need to update buffers before the first step for the controller. - robot.update(dt=sim_dt) - - # Track the given command - current_goal_idx = 0 - # Current goal for the arm - ee_pose_b_des = torch.zeros(self.num_envs, diff_ik_controller.action_dim, device=self.sim.device) - ee_pose_b_des[:] = self.ee_pose_b_des_set[current_goal_idx] - # Compute current pose of the end-effector - ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] - root_pose_w = robot.data.root_state_w[:, 0:7] - ee_pos_b, ee_quat_b = subtract_frame_transforms( - root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] - ) - - # Now we are ready! - for count in range(1500): - # reset every 150 steps - if count % 250 == 0: - # check that we converged to the goal - if count > 0: - pos_error, rot_error = compute_pose_error( - ee_pos_b, ee_quat_b, ee_pose_b_des[:, 0:3], ee_pose_b_des[:, 3:7] - ) - pos_error_norm = torch.norm(pos_error, dim=-1) - rot_error_norm = torch.norm(rot_error, dim=-1) - # desired error (zer) - des_error = torch.zeros_like(pos_error_norm) - # check convergence - torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=1e-3) - torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=1e-3) - # reset joint state - joint_pos = robot.data.default_joint_pos.clone() - joint_vel = robot.data.default_joint_vel.clone() - # joint_pos *= sample_uniform(0.9, 1.1, joint_pos.shape, joint_pos.device) - robot.write_joint_state_to_sim(joint_pos, joint_vel) - robot.set_joint_position_target(joint_pos) - robot.write_data_to_sim() - # randomize root state yaw, ik should work regardless base rotation - root_state = robot.data.root_state_w.clone() - root_state[:, 3:7] = random_yaw_orientation(self.num_envs, self.sim.device) - robot.write_root_pose_to_sim(root_state[:, :7]) - robot.write_root_velocity_to_sim(root_state[:, 7:]) - robot.reset() - # reset actions - ee_pose_b_des[:] = self.ee_pose_b_des_set[current_goal_idx] - joint_pos_des = joint_pos[:, arm_joint_ids].clone() - # update goal for next iteration - current_goal_idx = (current_goal_idx + 1) % len(self.ee_pose_b_des_set) - # set the controller commands - diff_ik_controller.reset() - diff_ik_controller.set_command(ee_pose_b_des) - else: - # at reset, the jacobians are not updated to the latest state - # so we MUST skip the first step - # obtain quantities from simulation - jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] - ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] - root_pose_w = robot.data.root_state_w[:, 0:7] - base_rot = root_pose_w[:, 3:7] - base_rot_matrix = matrix_from_quat(quat_inv(base_rot)) - jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) - jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) - joint_pos = robot.data.joint_pos[:, arm_joint_ids] - # compute frame in root frame - ee_pos_b, ee_quat_b = subtract_frame_transforms( - root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # Play the simulator + sim.reset() + + # Obtain the frame index of the end-effector + ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0] + ee_jacobi_idx = ee_frame_idx - 1 + # Obtain joint indices + arm_joint_ids = robot.find_joints(arm_joint_names)[0] + # Update existing buffers + # Note: We need to update buffers before the first step for the controller. + robot.update(dt=sim_dt) + + # Track the given command + current_goal_idx = 0 + # Current goal for the arm + ee_pose_b_des = torch.zeros(num_envs, diff_ik_controller.action_dim, device=sim.device) + ee_pose_b_des[:] = ee_pose_b_des_set[current_goal_idx] + # Compute current pose of the end-effector + ee_pose_w = robot.data.body_pose_w[:, ee_frame_idx] + root_pose_w = robot.data.root_pose_w + ee_pos_b, ee_quat_b = subtract_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] + ) + + # Now we are ready! + for count in range(1500): + # reset every 150 steps + if count % 250 == 0: + # check that we converged to the goal + if count > 0: + pos_error, rot_error = compute_pose_error( + ee_pos_b, ee_quat_b, ee_pose_b_des[:, 0:3], ee_pose_b_des[:, 3:7] ) - # compute the joint commands - joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos) - - # apply actions - robot.set_joint_position_target(joint_pos_des, arm_joint_ids) + pos_error_norm = torch.norm(pos_error, dim=-1) + rot_error_norm = torch.norm(rot_error, dim=-1) + # desired error (zer) + des_error = torch.zeros_like(pos_error_norm) + # check convergence + torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=1e-3) + torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=1e-3) + # reset joint state + joint_pos = robot.data.default_joint_pos.clone() + joint_vel = robot.data.default_joint_vel.clone() + # joint_pos *= sample_uniform(0.9, 1.1, joint_pos.shape, joint_pos.device) + robot.write_joint_state_to_sim(joint_pos, joint_vel) + robot.set_joint_position_target(joint_pos) robot.write_data_to_sim() - # perform step - self.sim.step(render=False) - # update buffers - robot.update(sim_dt) - - -if __name__ == "__main__": - run_tests() + # randomize root state yaw, ik should work regardless base rotation + root_state = robot.data.root_state_w.clone() + root_state[:, 3:7] = random_yaw_orientation(num_envs, sim.device) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) + robot.reset() + # reset actions + ee_pose_b_des[:] = ee_pose_b_des_set[current_goal_idx] + joint_pos_des = joint_pos[:, arm_joint_ids].clone() + # update goal for next iteration + current_goal_idx = (current_goal_idx + 1) % len(ee_pose_b_des_set) + # set the controller commands + diff_ik_controller.reset() + diff_ik_controller.set_command(ee_pose_b_des) + else: + # at reset, the jacobians are not updated to the latest state + # so we MUST skip the first step + # obtain quantities from simulation + jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] + ee_pose_w = robot.data.body_pose_w[:, ee_frame_idx] + root_pose_w = robot.data.root_pose_w + base_rot = root_pose_w[:, 3:7] + base_rot_matrix = matrix_from_quat(quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + joint_pos = robot.data.joint_pos[:, arm_joint_ids] + # compute frame in root frame + ee_pos_b, ee_quat_b = subtract_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] + ) + # compute the joint commands + joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos) + + # apply actions + robot.set_joint_position_target(joint_pos_des, arm_joint_ids) + robot.write_data_to_sim() + # perform step + sim.step(render=False) + # update buffers + robot.update(sim_dt) 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 000000000000..ccbdae06b52e --- /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 000000000000..f5d0d60717da --- /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_ik_configs/pink_ik_gr1_test_configs.json b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json new file mode 100644 index 000000000000..be40d7cf7abc --- /dev/null +++ b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json @@ -0,0 +1,93 @@ +{ + "tolerances": { + "position": 0.001, + "pd_position": 0.001, + "rotation": 0.02, + "check_errors": true + }, + "allowed_steps_to_settle": 5, + "tests": { + "vertical_movement": { + "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] + ], + "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] + ], + "allowed_steps_per_motion": 8, + "repeat": 2, + "requires_waist_bending": false + }, + "stay_still": { + "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] + ], + "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] + ], + "allowed_steps_per_motion": 8, + "repeat": 4, + "requires_waist_bending": false + }, + "horizontal_movement": { + "left_hand_pose": [ + [-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] + ], + "right_hand_pose": [ + [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": 8, + "repeat": 2, + "requires_waist_bending": false + }, + "horizontal_small_movement": { + "left_hand_pose": [ + [-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] + ], + "right_hand_pose": [ + [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": 8, + "repeat": 2, + "requires_waist_bending": false + }, + "forward_waist_bending_movement": { + "left_hand_pose": [ + [-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] + ], + "right_hand_pose": [ + [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": 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.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.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": 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 000000000000..69790724248a --- /dev/null +++ b/source/isaaclab/test/controllers/test_local_frame_task.py @@ -0,0 +1,483 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test 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 + +from pathlib import Path + +import numpy as np +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_null_space_posture_task.py b/source/isaaclab/test/controllers/test_null_space_posture_task.py new file mode 100644 index 000000000000..a1d0e1ac50d5 --- /dev/null +++ b/source/isaaclab/test/controllers/test_null_space_posture_task.py @@ -0,0 +1,342 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +"""Launch Isaac Sim Simulator first.""" + +# 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 + import pinocchio as pin # noqa: F401 +else: + import pinocchio # noqa: F401 + import pinocchio as pin # noqa: F401 + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Unit tests for NullSpacePostureTask with simplified robot configuration using Pink library directly.""" + +import numpy as np +import pytest +from pink.configuration import Configuration +from pink.tasks import FrameTask +from pinocchio.robot_wrapper import RobotWrapper + +from isaaclab.controllers.pink_ik.null_space_posture_task import NullSpacePostureTask + + +class TestNullSpacePostureTaskSimplifiedRobot: + """Test cases for NullSpacePostureTask with simplified robot configuration.""" + + @pytest.fixture + def num_joints(self): + """Number of joints in the simplified robot.""" + return 20 + + @pytest.fixture + def joint_configurations(self): + """Pre-generated joint configurations for testing.""" + # Set random seed for reproducible tests + np.random.seed(42) + + return { + "random": np.random.uniform(-0.5, 0.5, 20), + "controlled_only": np.array([0.5] * 5 + [0.0] * 15), # Non-zero for controlled joints only + "sequential": np.linspace(0.1, 2.0, 20), + } + + @pytest.fixture + def robot_urdf(self): + """Load the simplified test robot URDF file.""" + import os + + current_dir = os.path.dirname(os.path.abspath(__file__)) + urdf_path = os.path.join(current_dir, "simplified_test_robot.urdf") + return urdf_path + + @pytest.fixture + def robot_configuration(self, robot_urdf): + """Simplified robot wrapper.""" + wrapper = RobotWrapper.BuildFromURDF(robot_urdf, None, root_joint=None) + return Configuration(wrapper.model, wrapper.data, wrapper.q0) + + @pytest.fixture + def tasks(self): + """pink tasks.""" + return [ + FrameTask("left_hand_pitch_link", position_cost=1.0, orientation_cost=1.0), + NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=[ + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + ], + ), + ] + + def test_null_space_jacobian_zero_end_effector_velocity( + self, robot_configuration, tasks, joint_configurations, num_joints + ): + """Test that velocities projected through null space Jacobian result in zero end-effector velocity.""" + # Set specific joint configuration + robot_configuration.q = joint_configurations["random"] + + # Set frame task target to a specific position in workspace + frame_task = tasks[0] + # Create pin.SE3 from position and quaternion + position = np.array([0.5, 0.3, 0.8]) # x, y, z + quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0) # w, x, y, z (identity quaternion) + target_pose = pin.SE3(quaternion, position) + frame_task.set_target(target_pose) + + # Set null space posture task target + null_space_task = tasks[1] + target_posture = np.zeros(num_joints) + null_space_task.set_target(target_posture) + + # Get the null space Jacobian + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + + # Get the end-effector Jacobian + frame_task_jacobian = frame_task.compute_jacobian(robot_configuration) + + # Test multiple random velocities in null space + for _ in range(10): + # Generate random joint velocity + random_velocity = np.random.randn(num_joints) * 0.1 + + # Project through null space Jacobian + null_space_velocity = null_space_jacobian @ random_velocity + + # Compute resulting end-effector velocity + ee_velocity = frame_task_jacobian @ null_space_velocity + + # The end-effector velocity should be approximately zero + assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), f"End-effector velocity not zero: {ee_velocity}" + + def test_null_space_jacobian_properties(self, robot_configuration, tasks, joint_configurations, num_joints): + """Test mathematical properties of the null space Jacobian.""" + # Set specific joint configuration + robot_configuration.q = joint_configurations["random"] + + # Set frame task target + frame_task = tasks[0] + # Create pin.SE3 from position and quaternion + position = np.array([0.3, 0.4, 0.6]) + quaternion = pin.Quaternion(0.707, 0.0, 0.0, 0.707) # w, x, y, z (90-degree rotation around X) + target_pose = pin.SE3(quaternion, position) + frame_task.set_target(target_pose) + + # Set null space posture task target + null_space_task = tasks[1] + target_posture = np.zeros(num_joints) + target_posture[0:5] = [0.1, -0.1, 0.2, -0.2, 0.0] # Set first 5 joints (controlled joints) + null_space_task.set_target(target_posture) + + # Get Jacobians + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link") + + # Test: N * J^T should be approximately zero (null space property) + # where N is the null space projector and J is the end-effector Jacobian + null_space_projection = null_space_jacobian @ ee_jacobian.T + assert np.allclose(null_space_projection, np.zeros_like(null_space_projection), atol=1e-7), ( + f"Null space projection of end-effector Jacobian not zero: {null_space_projection}" + ) + + def test_null_space_jacobian_identity_when_no_frame_tasks( + self, robot_configuration, joint_configurations, num_joints + ): + """Test that null space Jacobian is identity when no frame tasks are defined.""" + # Create null space task without frame task controlled joints + null_space_task = NullSpacePostureTask(cost=1.0, controlled_frames=[], controlled_joints=[]) + + # Set specific joint configuration + robot_configuration.q = joint_configurations["sequential"] + + # Set target + target_posture = np.zeros(num_joints) + null_space_task.set_target(target_posture) + + # Get the null space Jacobian + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + + # Should be identity matrix + expected_identity = np.eye(num_joints) + assert np.allclose(null_space_jacobian, expected_identity), ( + f"Null space Jacobian should be identity when no frame tasks defined: {null_space_jacobian}" + ) + + def test_null_space_jacobian_consistency_across_configurations( + self, robot_configuration, tasks, joint_configurations, num_joints + ): + """Test that null space Jacobian is consistent across different joint configurations.""" + # Test multiple joint configurations + test_configs = [ + np.zeros(num_joints), # Zero configuration + joint_configurations["controlled_only"], # Non-zero for controlled joints + joint_configurations["random"], # Random configuration + ] + + # Set frame task target + frame_task = tasks[0] + # Create pin.SE3 from position and quaternion + position = np.array([0.3, 0.3, 0.5]) + quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0) # w, x, y, z (identity quaternion) + target_pose = pin.SE3(quaternion, position) + frame_task.set_target(target_pose) + + # Set null space posture task target + null_space_task = tasks[1] + target_posture = np.zeros(num_joints) + null_space_task.set_target(target_posture) + + jacobians = [] + for config in test_configs: + robot_configuration.q = config + jacobian = null_space_task.compute_jacobian(robot_configuration) + jacobians.append(jacobian) + + # Verify that velocities through this Jacobian result in zero end-effector velocity + ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link") + + # Test with random velocity + random_velocity = np.random.randn(num_joints) * 0.1 + null_space_velocity = jacobian @ random_velocity + ee_velocity = ee_jacobian @ null_space_velocity + + assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), ( + f"End-effector velocity not zero for configuration {config}: {ee_velocity}" + ) + + def test_compute_error_without_target(self, robot_configuration, joint_configurations): + """Test that compute_error raises ValueError when no target is set.""" + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=["waist_yaw_joint", "waist_pitch_joint"], + ) + + robot_configuration.q = joint_configurations["sequential"] + + # Should raise ValueError when no target is set + with pytest.raises(ValueError, match="No posture target has been set"): + null_space_task.compute_error(robot_configuration) + + def test_joint_masking(self, robot_configuration, joint_configurations, num_joints): + """Test that joint mask correctly filters only controlled joints.""" + + controlled_joint_names = ["waist_pitch_joint", "left_shoulder_pitch_joint", "left_elbow_pitch_joint"] + + # Create task with specific controlled joints + null_space_task = NullSpacePostureTask( + cost=1.0, controlled_frames=["left_hand_pitch_link"], controlled_joints=controlled_joint_names + ) + + # Find the joint indexes in robot_configuration.model.names.tolist()[1:] + joint_names = robot_configuration.model.names.tolist()[1:] + joint_indexes = [joint_names.index(jn) for jn in controlled_joint_names] + + # Set configurations + current_config = joint_configurations["sequential"] + target_config = np.zeros(num_joints) + + robot_configuration.q = current_config + null_space_task.set_target(target_config) + + # Compute error + error = null_space_task.compute_error(robot_configuration) + + # Only controlled joints should have non-zero error + # Joint indices: + # waist_yaw_joint=0, waist_pitch_joint=1, waist_roll_joint=2, left_shoulder_pitch_joint=3, + # left_shoulder_roll_joint=4, etc. + expected_error = np.zeros(num_joints) + for i in joint_indexes: + expected_error[i] = current_config[i] + + assert np.allclose(error, expected_error, atol=1e-7), ( + f"Joint mask not working correctly: expected {expected_error}, got {error}" + ) + + def test_empty_controlled_joints(self, robot_configuration, joint_configurations, num_joints): + """Test behavior when controlled_joints is empty.""" + null_space_task = NullSpacePostureTask( + cost=1.0, controlled_frames=["left_hand_pitch_link"], controlled_joints=[] + ) + + current_config = joint_configurations["sequential"] + target_config = np.zeros(num_joints) + + robot_configuration.q = current_config + null_space_task.set_target(target_config) + + # Error should be all zeros + error = null_space_task.compute_error(robot_configuration) + expected_error = np.zeros(num_joints) + assert np.allclose(error, expected_error), f"Error should be zero when no joints controlled: {error}" + + def test_set_target_from_configuration(self, robot_configuration, joint_configurations): + """Test set_target_from_configuration method.""" + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=["waist_yaw_joint", "waist_pitch_joint"], + ) + + # Set a specific configuration + test_config = joint_configurations["sequential"] + robot_configuration.q = test_config + + # Set target from configuration + null_space_task.set_target_from_configuration(robot_configuration) + + # Verify target was set correctly + assert null_space_task.target_q is not None + assert np.allclose(null_space_task.target_q, test_config) + + def test_multiple_frame_tasks(self, robot_configuration, joint_configurations, num_joints): + """Test null space projection with multiple frame tasks.""" + # Create task with multiple controlled frames + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link", "right_hand_pitch_link"], + controlled_joints=["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"], + ) + + current_config = joint_configurations["sequential"] + robot_configuration.q = current_config + + # Get null space Jacobian + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + + # Get Jacobians for both frames + jacobian_left_hand = robot_configuration.get_frame_jacobian("left_hand_pitch_link") + jacobian_right_hand = robot_configuration.get_frame_jacobian("right_hand_pitch_link") + + # Test that null space velocities result in zero velocity for both frames + for _ in range(5): + random_velocity = np.random.randn(num_joints) * 0.1 + null_space_velocity = null_space_jacobian @ random_velocity + + # Check both frames + ee_velocity_left = jacobian_left_hand @ null_space_velocity + ee_velocity_right = jacobian_right_hand @ null_space_velocity + + assert np.allclose(ee_velocity_left, np.zeros(6), atol=1e-7), ( + f"Left hand velocity not zero: {ee_velocity_left}" + ) + assert np.allclose(ee_velocity_right, np.zeros(6), atol=1e-7), ( + f"Right hand velocity not zero: {ee_velocity_right}" + ) diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 24451bdf024e..eeec14d877dc 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -1,22 +1,20 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" +import pytest import torch -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils from isaacsim.core.cloner import GridCloner import isaaclab.sim as sim_utils @@ -30,8 +28,8 @@ combine_frame_transforms, compute_pose_error, matrix_from_quat, + quat_apply_inverse, quat_inv, - quat_rotate_inverse, subtract_frame_transforms, ) @@ -41,1030 +39,1634 @@ from isaaclab_assets import FRANKA_PANDA_CFG # isort:skip -class TestOperationalSpaceController(unittest.TestCase): - """Test fixture for checking that Operational Space controller tracks commands properly.""" - - def setUp(self): - """Create a blank new stage for each test.""" - # Wait for spawning - stage_utils.create_new_stage() - # Constants - self.num_envs = 16 - # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=0.01) - self.sim = sim_utils.SimulationContext(sim_cfg) - # TODO: Remove this once we have a better way to handle this. - self.sim._app_control_on_stop_handle = None - - # Create a ground plane - cfg = sim_utils.GroundPlaneCfg() - cfg.func("/World/GroundPlane", cfg) - - # Markers - frame_marker_cfg = FRAME_MARKER_CFG.copy() - frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) - self.ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) - self.goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) - - light_cfg = sim_utils.DistantLightCfg(intensity=5.0, exposure=10.0) - light_cfg.func( - "/Light", - light_cfg, - translation=[0, 0, 1], - ) - - # Create interface to clone the scene - cloner = GridCloner(spacing=2.0) - cloner.define_base_env("/World/envs") - self.env_prim_paths = cloner.generate_paths("/World/envs/env", self.num_envs) - # create source prim - prim_utils.define_prim(self.env_prim_paths[0], "Xform") - # clone the env xform - self.env_origins = cloner.clone( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - replicate_physics=True, - ) - - self.robot_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/envs/env_.*/Robot") - self.robot_cfg.actuators["panda_shoulder"].stiffness = 0.0 - self.robot_cfg.actuators["panda_shoulder"].damping = 0.0 - self.robot_cfg.actuators["panda_forearm"].stiffness = 0.0 - self.robot_cfg.actuators["panda_forearm"].damping = 0.0 - self.robot_cfg.spawn.rigid_props.disable_gravity = True - - # Define the ContactSensor - self.contact_forces = None - - # Define the target sets - ee_goal_abs_pos_set_b = torch.tensor( - [ - [0.5, 0.5, 0.7], - [0.5, -0.4, 0.6], - [0.5, 0, 0.5], - ], - device=self.sim.device, - ) - ee_goal_abs_quad_set_b = torch.tensor( - [ - [0.707, 0.0, 0.707, 0.0], - [0.707, 0.707, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - ], - device=self.sim.device, - ) - ee_goal_rel_pos_set = torch.tensor( - [ - [0.2, 0.0, 0.0], - [0.2, 0.2, 0.0], - [0.2, 0.2, -0.2], - ], - device=self.sim.device, - ) - ee_goal_rel_axisangle_set = torch.tensor( - [ - [0.0, torch.pi / 2, 0.0], # for [0.707, 0, 0.707, 0] - [torch.pi / 2, 0.0, 0.0], # for [0.707, 0.707, 0, 0] - [torch.pi, 0.0, 0.0], # for [0.0, 1.0, 0, 0] - ], - device=self.sim.device, - ) - ee_goal_abs_wrench_set_b = torch.tensor( - [ - [0.0, 0.0, 10.0, 0.0, -1.0, 0.0], - [0.0, 10.0, 0.0, 0.0, 0.0, 0.0], - [10.0, 0.0, 0.0, 0.0, 0.0, 0.0], - ], - device=self.sim.device, - ) - kp_set = torch.tensor( - [ - [200.0, 200.0, 200.0, 200.0, 200.0, 200.0], - [240.0, 240.0, 240.0, 240.0, 240.0, 240.0], - [160.0, 160.0, 160.0, 160.0, 160.0, 160.0], - ], - device=self.sim.device, - ) - d_ratio_set = torch.tensor( - [ - [1.0, 1.0, 1.0, 1.0, 1.0, 1.0], - [1.1, 1.1, 1.1, 1.1, 1.1, 1.1], - [0.9, 0.9, 0.9, 0.9, 0.9, 0.9], - ], - device=self.sim.device, - ) - ee_goal_hybrid_set_b = torch.tensor( - [ - [0.6, 0.2, 0.5, 0.0, 0.707, 0.0, 0.707, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.6, -0.29, 0.6, 0.0, 0.707, 0.0, 0.707, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.6, 0.1, 0.8, 0.0, 0.5774, 0.0, 0.8165, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0], - ], - device=self.sim.device, - ) - ee_goal_pose_set_tilted_b = torch.tensor( - [ - [0.6, 0.15, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], - [0.6, -0.3, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], - [0.8, 0.0, 0.5, 0.0, 0.92387953, 0.0, 0.38268343], - ], - device=self.sim.device, - ) - ee_goal_wrench_set_tilted_task = torch.tensor( - [ - [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], - ], - device=self.sim.device, - ) - - # Define goals for the arm [xyz] - self.target_abs_pos_set_b = ee_goal_abs_pos_set_b.clone() - # Define goals for the arm [xyz + quat_wxyz] - self.target_abs_pose_set_b = torch.cat([ee_goal_abs_pos_set_b, ee_goal_abs_quad_set_b], dim=-1) - # Define goals for the arm [xyz] - self.target_rel_pos_set = ee_goal_rel_pos_set.clone() - # Define goals for the arm [xyz + axis-angle] - self.target_rel_pose_set_b = torch.cat([ee_goal_rel_pos_set, ee_goal_rel_axisangle_set], dim=-1) - # Define goals for the arm [force_xyz + torque_xyz] - self.target_abs_wrench_set = ee_goal_abs_wrench_set_b.clone() - # Define goals for the arm [xyz + quat_wxyz] and variable kp [kp_xyz + kp_rot_xyz] - self.target_abs_pose_variable_kp_set = torch.cat([self.target_abs_pose_set_b, kp_set], dim=-1) - # Define goals for the arm [xyz + quat_wxyz] and the variable imp. [kp_xyz + kp_rot_xyz + d_xyz + d_rot_xyz] - self.target_abs_pose_variable_set = torch.cat([self.target_abs_pose_set_b, kp_set, d_ratio_set], dim=-1) - # Define goals for the arm pose [xyz + quat_wxyz] and wrench [force_xyz + torque_xyz] - self.target_hybrid_set_b = ee_goal_hybrid_set_b.clone() - # Define goals for the arm pose, and wrench, and kp - self.target_hybrid_variable_kp_set = torch.cat([self.target_hybrid_set_b, kp_set], dim=-1) - # Define goals for the arm pose [xyz + quat_wxyz] in root and and wrench [force_xyz + torque_xyz] in task frame - self.target_hybrid_set_tilted = torch.cat([ee_goal_pose_set_tilted_b, ee_goal_wrench_set_tilted_task], dim=-1) - - # Reference frame for targets - self.frame = "root" - - def tearDown(self): - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - # self.sim.clear() # FIXME: This hangs the test for some reason when LIVESTREAM is not enabled. - self.sim.clear_all_callbacks() - self.sim.clear_instance() - # Make contact_forces None after relevant tests otherwise other tests give warning - self.contact_forces = None - - """ - Test fixtures. - """ - - def test_franka_pose_abs_without_inertial_decoupling(self): - """Test absolute pose control with fixed impedance and without inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=[400.0, 400.0, 400.0, 100.0, 100.0, 100.0], - motion_damping_ratio_task=[5.0, 5.0, 5.0, 0.001, 0.001, 0.001], - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_pose_abs_with_partial_inertial_decoupling(self): - """Test absolute pose control with fixed impedance and partial inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=True, - gravity_compensation=False, - motion_stiffness_task=1000.0, - motion_damping_ratio_task=1.0, - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_pose_abs_fixed_impedance_with_gravity_compensation(self): - """Test absolute pose control with fixed impedance, gravity compensation, and inertial dynamics decoupling.""" - self.robot_cfg.spawn.rigid_props.disable_gravity = False - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=True, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_pose_abs(self): - """Test absolute pose control with fixed impedance and inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_pose_rel(self): - """Test relative pose control with fixed impedance and inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_rel"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_rel_pose_set_b) - - def test_franka_pose_abs_variable_impedance(self): - """Test absolute pose control with variable impedance and inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="variable", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_variable_set) - - def test_franka_wrench_abs_open_loop(self): - """Test open loop absolute force control.""" - robot = Articulation(cfg=self.robot_cfg) - - obstacle_spawn_cfg = sim_utils.CuboidCfg( - size=(0.7, 0.7, 0.01), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), - activate_contact_sensors=True, - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle1", - obstacle_spawn_cfg, - translation=(0.2, 0.0, 0.93), - orientation=(0.9848, 0.0, -0.1736, 0.0), - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle2", - obstacle_spawn_cfg, - translation=(0.2, 0.35, 0.7), - orientation=(0.707, 0.707, 0.0, 0.0), - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle3", - obstacle_spawn_cfg, - translation=(0.55, 0.0, 0.7), - orientation=(0.707, 0.0, 0.707, 0.0), - ) - contact_forces_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/obstacle.*", - update_period=0.0, - history_length=50, - debug_vis=False, - force_threshold=0.1, - ) - self.contact_forces = ContactSensor(contact_forces_cfg) - - osc_cfg = OperationalSpaceControllerCfg( - target_types=["wrench_abs"], - motion_control_axes_task=[0, 0, 0, 0, 0, 0], - contact_wrench_control_axes_task=[1, 1, 1, 1, 1, 1], - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_wrench_set) - - def test_franka_wrench_abs_closed_loop(self): - """Test closed loop absolute force control.""" - robot = Articulation(cfg=self.robot_cfg) - - obstacle_spawn_cfg = sim_utils.CuboidCfg( - size=(0.7, 0.7, 0.01), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), - activate_contact_sensors=True, - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle1", - obstacle_spawn_cfg, - translation=(0.2, 0.0, 0.93), - orientation=(0.9848, 0.0, -0.1736, 0.0), - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle2", - obstacle_spawn_cfg, - translation=(0.2, 0.35, 0.7), - orientation=(0.707, 0.707, 0.0, 0.0), - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle3", - obstacle_spawn_cfg, - translation=(0.55, 0.0, 0.7), - orientation=(0.707, 0.0, 0.707, 0.0), - ) - contact_forces_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/obstacle.*", - update_period=0.0, - history_length=2, - debug_vis=False, - force_threshold=0.1, - ) - self.contact_forces = ContactSensor(contact_forces_cfg) - - osc_cfg = OperationalSpaceControllerCfg( - target_types=["wrench_abs"], - contact_wrench_stiffness_task=[ - 0.2, - 0.2, - 0.2, - 0.0, - 0.0, - 0.0, - ], # Zero torque feedback as we cannot contact torque - motion_control_axes_task=[0, 0, 0, 0, 0, 0], - contact_wrench_control_axes_task=[1, 1, 1, 1, 1, 1], - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_wrench_set) - - def test_franka_hybrid_decoupled_motion(self): - """Test hybrid control with fixed impedance and partial inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - - obstacle_spawn_cfg = sim_utils.CuboidCfg( - size=(1.0, 1.0, 0.01), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), - activate_contact_sensors=True, - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle1", - obstacle_spawn_cfg, - translation=(self.target_hybrid_set_b[0, 0] + 0.05, 0.0, 0.7), - orientation=(0.707, 0.0, 0.707, 0.0), - ) - contact_forces_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/obstacle.*", - update_period=0.0, - history_length=2, - debug_vis=False, - force_threshold=0.1, - ) - self.contact_forces = ContactSensor(contact_forces_cfg) - - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs", "wrench_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=True, - gravity_compensation=False, - motion_stiffness_task=300.0, - motion_damping_ratio_task=1.0, - contact_wrench_stiffness_task=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], - motion_control_axes_task=[0, 1, 1, 1, 1, 1], - contact_wrench_control_axes_task=[1, 0, 0, 0, 0, 0], - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_set_b) - - def test_franka_hybrid_variable_kp_impedance(self): - """Test hybrid control with variable kp impedance and inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - - obstacle_spawn_cfg = sim_utils.CuboidCfg( - size=(1.0, 1.0, 0.01), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), - activate_contact_sensors=True, - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle1", - obstacle_spawn_cfg, - translation=(self.target_hybrid_set_b[0, 0] + 0.05, 0.0, 0.7), - orientation=(0.707, 0.0, 0.707, 0.0), - ) - contact_forces_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/obstacle.*", - update_period=0.0, - history_length=2, - debug_vis=False, - force_threshold=0.1, - ) - self.contact_forces = ContactSensor(contact_forces_cfg) - - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs", "wrench_abs"], - impedance_mode="variable_kp", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_damping_ratio_task=0.8, - contact_wrench_stiffness_task=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], - motion_control_axes_task=[0, 1, 1, 1, 1, 1], - contact_wrench_control_axes_task=[1, 0, 0, 0, 0, 0], - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller( - robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_variable_kp_set - ) - - def test_franka_taskframe_pose_abs(self): - """Test absolute pose control in task frame with fixed impedance and inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - self.frame = "task" - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_taskframe_pose_rel(self): - """Test relative pose control in task frame with fixed impedance and inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - self.frame = "task" - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_rel"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_rel_pose_set_b) - - def test_franka_taskframe_hybrid(self): - """Test hybrid control in task frame with fixed impedance and inertial dynamics decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - self.frame = "task" - - obstacle_spawn_cfg = sim_utils.CuboidCfg( - size=(2.0, 1.5, 0.01), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), - activate_contact_sensors=True, - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle1", - obstacle_spawn_cfg, - translation=(self.target_hybrid_set_tilted[0, 0] + 0.085, 0.0, 0.3), - orientation=(0.9238795325, 0.0, -0.3826834324, 0.0), - ) - contact_forces_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/obstacle.*", - update_period=0.0, - history_length=2, - debug_vis=False, - force_threshold=0.1, - ) - self.contact_forces = ContactSensor(contact_forces_cfg) - - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs", "wrench_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=400.0, - motion_damping_ratio_task=1.0, - contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], - motion_control_axes_task=[1, 1, 0, 1, 1, 1], - contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_set_tilted) - - def test_franka_pose_abs_without_inertial_decoupling_with_nullspace_centering(self): - """Test absolute pose control with fixed impedance and nullspace centerin but without inertial decoupling.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=[400.0, 400.0, 400.0, 100.0, 100.0, 100.0], - motion_damping_ratio_task=[5.0, 5.0, 5.0, 0.001, 0.001, 0.001], - nullspace_control="position", - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_pose_abs_with_partial_inertial_decoupling_nullspace_centering(self): - """Test absolute pose control with fixed impedance, partial inertial decoupling and nullspace centering.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=True, - gravity_compensation=False, - motion_stiffness_task=1000.0, - motion_damping_ratio_task=1.0, - nullspace_control="position", - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_pose_abs_with_nullspace_centering(self): - """Test absolute pose control with fixed impedance, inertial decoupling and nullspace centering.""" - robot = Articulation(cfg=self.robot_cfg) - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - nullspace_control="position", - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) - - def test_franka_taskframe_hybrid_with_nullspace_centering(self): - """Test hybrid control in task frame with fixed impedance, inertial decoupling and nullspace centering.""" - robot = Articulation(cfg=self.robot_cfg) - self.frame = "task" - - obstacle_spawn_cfg = sim_utils.CuboidCfg( - size=(2.0, 1.5, 0.01), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), - activate_contact_sensors=True, - ) - obstacle_spawn_cfg.func( - "/World/envs/env_.*/obstacle1", - obstacle_spawn_cfg, - translation=(self.target_hybrid_set_tilted[0, 0] + 0.085, 0.0, 0.3), - orientation=(0.9238795325, 0.0, -0.3826834324, 0.0), - ) - contact_forces_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/obstacle.*", - update_period=0.0, - history_length=2, - debug_vis=False, - force_threshold=0.1, - ) - self.contact_forces = ContactSensor(contact_forces_cfg) - - osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs", "wrench_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=400.0, - motion_damping_ratio_task=1.0, - contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], - motion_control_axes_task=[1, 1, 0, 1, 1, 1], - contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], - nullspace_control="position", - ) - osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) - - self._run_op_space_controller(robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_set_tilted) - - """ - Helper functions +@pytest.fixture +def sim(): + """Create a simulation context for testing.""" + # Wait for spawning + stage = sim_utils.create_new_stage() + # Constants + num_envs = 16 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=0.01) + sim = sim_utils.SimulationContext(sim_cfg) + # TODO: Remove this once we have a better way to handle this. + sim._app_control_on_stop_handle = None + + # Create a ground plane + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/GroundPlane", cfg) + + # Markers + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) + goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) + + light_cfg = sim_utils.DistantLightCfg(intensity=5.0, exposure=10.0) + light_cfg.func( + "/Light", + light_cfg, + translation=[0, 0, 1], + ) + + # Create interface to clone the scene + cloner = GridCloner(spacing=2.0) + cloner.define_base_env("/World/envs") + env_prim_paths = cloner.generate_paths("/World/envs/env", num_envs) + # create source prim + stage.DefinePrim(env_prim_paths[0], "Xform") + # clone the env xform + cloner.clone( + source_prim_path=env_prim_paths[0], + prim_paths=env_prim_paths, + replicate_physics=True, + ) + + robot_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/envs/env_.*/Robot") + robot_cfg.actuators["panda_shoulder"].stiffness = 0.0 + robot_cfg.actuators["panda_shoulder"].damping = 0.0 + robot_cfg.actuators["panda_forearm"].stiffness = 0.0 + robot_cfg.actuators["panda_forearm"].damping = 0.0 + robot_cfg.spawn.rigid_props.disable_gravity = True + + # Define the ContactSensor + contact_forces = None + + # Define the target sets + ee_goal_abs_pos_set_b = torch.tensor( + [ + [0.5, 0.5, 0.7], + [0.5, -0.4, 0.6], + [0.5, 0, 0.5], + ], + device=sim.device, + ) + ee_goal_abs_quad_set_b = torch.tensor( + [ + [0.707, 0.0, 0.707, 0.0], + [0.707, 0.707, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + ], + device=sim.device, + ) + ee_goal_rel_pos_set = torch.tensor( + [ + [0.2, 0.0, 0.0], + [0.2, 0.2, 0.0], + [0.2, 0.2, -0.2], + ], + device=sim.device, + ) + ee_goal_rel_axisangle_set = torch.tensor( + [ + [0.0, torch.pi / 2, 0.0], # for [0.707, 0, 0.707, 0] + [torch.pi / 2, 0.0, 0.0], # for [0.707, 0.707, 0, 0] + [torch.pi / 2, torch.pi / 2, 0.0], # for [0.0, 1.0, 0, 0] + ], + device=sim.device, + ) + ee_goal_abs_wrench_set_b = torch.tensor( + [ + [0.0, 0.0, 10.0, 0.0, -1.0, 0.0], + [0.0, 10.0, 0.0, 0.0, 0.0, 0.0], + [10.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + device=sim.device, + ) + kp_set = torch.tensor( + [ + [200.0, 200.0, 200.0, 200.0, 200.0, 200.0], + [240.0, 240.0, 240.0, 240.0, 240.0, 240.0], + [160.0, 160.0, 160.0, 160.0, 160.0, 160.0], + ], + device=sim.device, + ) + d_ratio_set = torch.tensor( + [ + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0], + [1.1, 1.1, 1.1, 1.1, 1.1, 1.1], + [0.9, 0.9, 0.9, 0.9, 0.9, 0.9], + ], + device=sim.device, + ) + ee_goal_hybrid_set_b = torch.tensor( + [ + [0.6, 0.2, 0.5, 0.0, 0.707, 0.0, 0.707, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.6, -0.29, 0.6, 0.0, 0.707, 0.0, 0.707, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.6, 0.1, 0.8, 0.0, 0.5774, 0.0, 0.8165, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + device=sim.device, + ) + ee_goal_pose_set_tilted_b = torch.tensor( + [ + [0.6, 0.15, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.6, -0.3, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.8, 0.0, 0.5, 0.0, 0.92387953, 0.0, 0.38268343], + ], + device=sim.device, + ) + ee_goal_wrench_set_tilted_task = torch.tensor( + [ + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + ], + device=sim.device, + ) + + # Define goals for the arm [xyz] + target_abs_pos_set_b = ee_goal_abs_pos_set_b.clone() + # Define goals for the arm [xyz + quat_wxyz] + target_abs_pose_set_b = torch.cat([ee_goal_abs_pos_set_b, ee_goal_abs_quad_set_b], dim=-1) + # Define goals for the arm [xyz] + target_rel_pos_set = ee_goal_rel_pos_set.clone() + # Define goals for the arm [xyz + axis-angle] + target_rel_pose_set_b = torch.cat([ee_goal_rel_pos_set, ee_goal_rel_axisangle_set], dim=-1) + # Define goals for the arm [force_xyz + torque_xyz] + target_abs_wrench_set = ee_goal_abs_wrench_set_b.clone() + # Define goals for the arm [xyz + quat_wxyz] and variable kp [kp_xyz + kp_rot_xyz] + target_abs_pose_variable_kp_set = torch.cat([target_abs_pose_set_b, kp_set], dim=-1) + # Define goals for the arm [xyz + quat_wxyz] and the variable imp. [kp_xyz + kp_rot_xyz + d_xyz + d_rot_xyz] + target_abs_pose_variable_set = torch.cat([target_abs_pose_set_b, kp_set, d_ratio_set], dim=-1) + # Define goals for the arm pose [xyz + quat_wxyz] and wrench [force_xyz + torque_xyz] + target_hybrid_set_b = ee_goal_hybrid_set_b.clone() + # Define goals for the arm pose, and wrench, and kp + target_hybrid_variable_kp_set = torch.cat([target_hybrid_set_b, kp_set], dim=-1) + # Define goals for the arm pose [xyz + quat_wxyz] in root and and wrench [force_xyz + torque_xyz] in task frame + target_hybrid_set_tilted = torch.cat([ee_goal_pose_set_tilted_b, ee_goal_wrench_set_tilted_task], dim=-1) + + # Reference frame for targets + frame = "root" + + yield ( + sim, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + target_abs_pos_set_b, + target_abs_pose_set_b, + target_rel_pos_set, + target_rel_pose_set_b, + target_abs_wrench_set, + target_abs_pose_variable_kp_set, + target_abs_pose_variable_set, + target_hybrid_set_b, + target_hybrid_variable_kp_set, + target_hybrid_set_tilted, + frame, + ) + + # Cleanup + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs_without_inertial_decoupling(sim): + """Test absolute pose control with fixed impedance and without inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=[400.0, 400.0, 400.0, 100.0, 100.0, 100.0], + motion_damping_ratio_task=[5.0, 5.0, 5.0, 0.001, 0.001, 0.001], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs_with_partial_inertial_decoupling(sim): + """Test absolute pose control with fixed impedance and partial inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=True, + gravity_compensation=False, + motion_stiffness_task=1000.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs_fixed_impedance_with_gravity_compensation(sim): + """Test absolute pose control with fixed impedance, gravity compensation, and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot_cfg.spawn.rigid_props.disable_gravity = False + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=True, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs(sim): + """Test absolute pose control with fixed impedance and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_rel(sim): + """Test relative pose control with fixed impedance and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + target_rel_pose_set_b, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_rel"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_rel_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs_variable_impedance(sim): + """Test absolute pose control with variable impedance and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + _, + _, + _, + target_abs_pose_variable_set, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="variable", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_variable_set, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_wrench_abs_open_loop(sim): + """Test open loop absolute force control.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + _, + target_abs_wrench_set, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(0.7, 0.7, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(0.2, 0.0, 0.93), + orientation=(0.9848, 0.0, -0.1736, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle2", + obstacle_spawn_cfg, + translation=(0.2, 0.35, 0.7), + orientation=(0.707, 0.707, 0.0, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle3", + obstacle_spawn_cfg, + translation=(0.55, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=50, + debug_vis=False, + force_threshold=0.1, + ) + contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["wrench_abs"], + motion_control_axes_task=[0, 0, 0, 0, 0, 0], + contact_wrench_control_axes_task=[1, 1, 1, 1, 1, 1], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_wrench_set, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_wrench_abs_closed_loop(sim): + """Test closed loop absolute force control.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + _, + target_abs_wrench_set, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(0.7, 0.7, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(0.2, 0.0, 0.93), + orientation=(0.9848, 0.0, -0.1736, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle2", + obstacle_spawn_cfg, + translation=(0.2, 0.35, 0.7), + orientation=(0.707, 0.707, 0.0, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle3", + obstacle_spawn_cfg, + translation=(0.55, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["wrench_abs"], + contact_wrench_stiffness_task=[ + 0.2, + 0.2, + 0.2, + 0.0, + 0.0, + 0.0, + ], # Zero torque feedback as we cannot contact torque + motion_control_axes_task=[0, 0, 0, 0, 0, 0], + contact_wrench_control_axes_task=[1, 1, 1, 1, 1, 1], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_wrench_set, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_hybrid_decoupled_motion(sim): + """Test hybrid control with fixed impedance and partial inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + _, + _, + _, + _, + target_hybrid_set_b, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(1.0, 1.0, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(target_hybrid_set_b[0, 0] + 0.05, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=True, + gravity_compensation=False, + motion_stiffness_task=300.0, + motion_damping_ratio_task=1.0, + contact_wrench_stiffness_task=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], + motion_control_axes_task=[0, 1, 1, 1, 1, 1], + contact_wrench_control_axes_task=[1, 0, 0, 0, 0, 0], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_leftfinger", + ["panda_joint.*"], + target_hybrid_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_hybrid_variable_kp_impedance(sim): + """Test hybrid control with variable kp impedance and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + _, + _, + _, + _, + target_hybrid_set_b, + target_hybrid_variable_kp_set, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(1.0, 1.0, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(target_hybrid_set_b[0, 0] + 0.05, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="variable_kp", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_damping_ratio_task=0.8, + contact_wrench_stiffness_task=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], + motion_control_axes_task=[0, 1, 1, 1, 1, 1], + contact_wrench_control_axes_task=[1, 0, 0, 0, 0, 0], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_leftfinger", + ["panda_joint.*"], + target_hybrid_variable_kp_set, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_taskframe_pose_abs(sim): + """Test absolute pose control in task frame with fixed impedance and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + frame = "task" + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_taskframe_pose_rel(sim): + """Test relative pose control in task frame with fixed impedance and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + target_rel_pose_set_b, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + frame = "task" + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_rel"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_rel_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_taskframe_hybrid(sim): + """Test hybrid control in task frame with fixed impedance and inertial dynamics decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + _, + _, + _, + _, + _, + _, + target_hybrid_set_tilted, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + frame = "task" + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(2.0, 1.5, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(target_hybrid_set_tilted[0, 0] + 0.085, 0.0, 0.3), + orientation=(0.9238795325, 0.0, -0.3826834324, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=400.0, + motion_damping_ratio_task=1.0, + contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], + motion_control_axes_task=[1, 1, 0, 1, 1, 1], + contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_leftfinger", + ["panda_joint.*"], + target_hybrid_set_tilted, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs_without_inertial_decoupling_with_nullspace_centering(sim): + """Test absolute pose control with fixed impedance and nullspace centerin but without inertial decoupling.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=[400.0, 400.0, 400.0, 100.0, 100.0, 100.0], + motion_damping_ratio_task=[5.0, 5.0, 5.0, 0.001, 0.001, 0.001], + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs_with_partial_inertial_decoupling_nullspace_centering(sim): + """Test absolute pose control with fixed impedance, partial inertial decoupling and nullspace centering.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=True, + gravity_compensation=False, + motion_stiffness_task=1000.0, + motion_damping_ratio_task=1.0, + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_pose_abs_with_nullspace_centering(sim): + """Test absolute pose control with fixed impedance, inertial decoupling and nullspace centering.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + target_abs_pose_set_b, + _, + _, + _, + _, + _, + _, + _, + _, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_hand", + ["panda_joint.*"], + target_abs_pose_set_b, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +@pytest.mark.isaacsim_ci +def test_franka_taskframe_hybrid_with_nullspace_centering(sim): + """Test hybrid control in task frame with fixed impedance, inertial decoupling and nullspace centering.""" + ( + sim_context, + num_envs, + robot_cfg, + ee_marker, + goal_marker, + contact_forces, + _, + _, + _, + _, + _, + _, + _, + _, + _, + target_hybrid_set_tilted, + frame, + ) = sim + + robot = Articulation(cfg=robot_cfg) + frame = "task" + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(2.0, 1.5, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(target_hybrid_set_tilted[0, 0] + 0.085, 0.0, 0.3), + orientation=(0.9238795325, 0.0, -0.3826834324, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=400.0, + motion_damping_ratio_task=1.0, + contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], + motion_control_axes_task=[1, 1, 0, 1, 1, 1], + contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=num_envs, device=sim_context.device) + + _run_op_space_controller( + robot, + osc, + "panda_leftfinger", + ["panda_joint.*"], + target_hybrid_set_tilted, + sim_context, + num_envs, + ee_marker, + goal_marker, + contact_forces, + frame, + ) + + +def _run_op_space_controller( + robot: Articulation, + osc: OperationalSpaceController, + ee_frame_name: str, + arm_joint_names: list[str], + target_set: torch.tensor, + sim: sim_utils.SimulationContext, + num_envs: int, + ee_marker: VisualizationMarkers, + goal_marker: VisualizationMarkers, + contact_forces: ContactSensor | None, + frame: str, +): + """Run the operational space controller with the given parameters. + + Args: + robot (Articulation): The robot to control. + osc (OperationalSpaceController): The operational space controller. + ee_frame_name (str): The name of the end-effector frame. + arm_joint_names (list[str]): The names of the arm joints. + target_set (torch.tensor): The target set to track. + sim (sim_utils.SimulationContext): The simulation context. + num_envs (int): The number of environments. + ee_marker (VisualizationMarkers): The end-effector marker. + goal_marker (VisualizationMarkers): The goal marker. + contact_forces (ContactSensor | None): The contact forces sensor. + frame (str): The reference frame for targets. """ - - def _run_op_space_controller( - self, - robot: Articulation, - osc: OperationalSpaceController, - ee_frame_name: str, - arm_joint_names: list[str], - target_set: torch.tensor, - ): - """Run the operational space controller with the given parameters. - - Args: - robot (Articulation): The robot to control. - osc (OperationalSpaceController): The operational space controller. - ee_frame_name (str): The name of the end-effector frame. - arm_joint_names (list[str]): The names of the arm joints. - target_set (torch.tensor): The target set to track. - """ - # Initialize the masks for evaluating target convergence according to selection matrices - self.pos_mask = torch.tensor(osc.cfg.motion_control_axes_task[:3], device=self.sim.device).view(1, 3) - self.rot_mask = torch.tensor(osc.cfg.motion_control_axes_task[3:], device=self.sim.device).view(1, 3) - self.wrench_mask = torch.tensor(osc.cfg.contact_wrench_control_axes_task, device=self.sim.device).view(1, 6) - self.force_mask = self.wrench_mask[:, 0:3] # Take only the force components as we can measure only these - - # Define simulation stepping - sim_dt = self.sim.get_physics_dt() - # Play the simulator - self.sim.reset() - - # Obtain the frame index of the end-effector - ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0] - # Obtain joint indices - arm_joint_ids = robot.find_joints(arm_joint_names)[0] - - # Update existing buffers - # Note: We need to update buffers before the first step for the controller. - robot.update(dt=sim_dt) - - # Get the center of the robot soft joint limits - joint_centers = torch.mean(robot.data.soft_joint_pos_limits[:, arm_joint_ids, :], dim=-1) - - # get the updated states - ( - jacobian_b, - mass_matrix, - gravity, - ee_pose_b, - ee_vel_b, - root_pose_w, - ee_pose_w, - ee_force_b, - joint_pos, - joint_vel, - ) = self._update_states(robot, ee_frame_idx, arm_joint_ids) - - # Track the given target command - current_goal_idx = 0 # Current goal index for the arm - command = torch.zeros( - self.num_envs, osc.action_dim, device=self.sim.device - ) # Generic target command, which can be pose, position, force, etc. - ee_target_pose_b = torch.zeros(self.num_envs, 7, device=self.sim.device) # Target pose in the body frame - ee_target_pose_w = torch.zeros( - self.num_envs, 7, device=self.sim.device - ) # Target pose in the world frame (for marker) - - # Set joint efforts to zero - zero_joint_efforts = torch.zeros(self.num_envs, robot.num_joints, device=self.sim.device) - joint_efforts = torch.zeros(self.num_envs, len(arm_joint_ids), device=self.sim.device) - - # Now we are ready! - for count in range(1501): - # reset every 500 steps - if count % 500 == 0: - # check that we converged to the goal - if count > 0: - self._check_convergence(osc, ee_pose_b, ee_target_pose_b, ee_force_b, command) - # reset joint state to default - default_joint_pos = robot.data.default_joint_pos.clone() - default_joint_vel = robot.data.default_joint_vel.clone() - robot.write_joint_state_to_sim(default_joint_pos, default_joint_vel) - robot.set_joint_effort_target(zero_joint_efforts) # Set zero torques in the initial step - robot.write_data_to_sim() - robot.reset() - # reset contact sensor - if self.contact_forces is not None: - self.contact_forces.reset() - # reset target pose - robot.update(sim_dt) - _, _, _, ee_pose_b, _, _, _, _, _, _ = self._update_states( - robot, ee_frame_idx, arm_joint_ids - ) # at reset, the jacobians are not updated to the latest state - command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = self._update_target( - osc, root_pose_w, ee_pose_b, target_set, current_goal_idx - ) - # set the osc command - osc.reset() - command, task_frame_pose_b = self._convert_to_task_frame( - osc, command=command, ee_target_pose_b=ee_target_pose_b + # Initialize the masks for evaluating target convergence according to selection matrices + pos_mask = torch.tensor(osc.cfg.motion_control_axes_task[:3], device=sim.device).view(1, 3) + rot_mask = torch.tensor(osc.cfg.motion_control_axes_task[3:], device=sim.device).view(1, 3) + wrench_mask = torch.tensor(osc.cfg.contact_wrench_control_axes_task, device=sim.device).view(1, 6) + force_mask = wrench_mask[:, 0:3] # Take only the force components as we can measure only these + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # Play the simulator + sim.reset() + + # Obtain the frame index of the end-effector + ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0] + # Obtain joint indices + arm_joint_ids = robot.find_joints(arm_joint_names)[0] + + # Update existing buffers + # Note: We need to update buffers before the first step for the controller. + robot.update(dt=sim_dt) + + # Get the center of the robot soft joint limits + joint_centers = torch.mean(robot.data.soft_joint_pos_limits[:, arm_joint_ids, :], dim=-1) + + # get the updated states + ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) = _update_states(robot, ee_frame_idx, arm_joint_ids, sim, contact_forces, num_envs) + + # Track the given target command + current_goal_idx = 0 # Current goal index for the arm + command = torch.zeros( + num_envs, osc.action_dim, device=sim.device + ) # Generic target command, which can be pose, position, force, etc. + ee_target_pose_b = torch.zeros(num_envs, 7, device=sim.device) # Target pose in the body frame + ee_target_pose_w = torch.zeros(num_envs, 7, device=sim.device) # Target pose in the world frame (for marker) + + # Set joint efforts to zero + zero_joint_efforts = torch.zeros(num_envs, robot.num_joints, device=sim.device) + joint_efforts = torch.zeros(num_envs, len(arm_joint_ids), device=sim.device) + + # Now we are ready! + for count in range(1501): + # reset every 500 steps + if count % 500 == 0: + # check that we converged to the goal + if count > 0: + _check_convergence( + osc, ee_pose_b, ee_target_pose_b, ee_force_b, command, pos_mask, rot_mask, force_mask, frame ) - osc.set_command( - command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b - ) - else: - # get the updated states - ( - jacobian_b, - mass_matrix, - gravity, - ee_pose_b, - ee_vel_b, - root_pose_w, - ee_pose_w, - ee_force_b, - joint_pos, - joint_vel, - ) = self._update_states(robot, ee_frame_idx, arm_joint_ids) - # compute the joint commands - joint_efforts = osc.compute( - jacobian_b=jacobian_b, - current_ee_pose_b=ee_pose_b, - current_ee_vel_b=ee_vel_b, - current_ee_force_b=ee_force_b, - mass_matrix=mass_matrix, - gravity=gravity, - current_joint_pos=joint_pos, - current_joint_vel=joint_vel, - nullspace_joint_pos_target=joint_centers, - ) - robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) - robot.write_data_to_sim() - - # update marker positions - self.ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) - self.goal_marker.visualize(ee_target_pose_w[:, 0:3], ee_target_pose_w[:, 3:7]) - - # perform step - self.sim.step(render=False) - # update buffers + # reset joint state to default + default_joint_pos = robot.data.default_joint_pos.clone() + default_joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(default_joint_pos, default_joint_vel) + robot.set_joint_effort_target(zero_joint_efforts) # Set zero torques in the initial step + robot.write_data_to_sim() + robot.reset() + # reset contact sensor + if contact_forces is not None: + contact_forces.reset() + # reset target pose robot.update(sim_dt) + _, _, _, ee_pose_b, _, _, _, _, _, _ = _update_states( + robot, ee_frame_idx, arm_joint_ids, sim, contact_forces, num_envs + ) # at reset, the jacobians are not updated to the latest state + command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = _update_target( + osc, root_pose_w, ee_pose_b, target_set, current_goal_idx + ) + # set the osc command + osc.reset() + command, task_frame_pose_b = _convert_to_task_frame( + osc, command=command, ee_target_pose_b=ee_target_pose_b, frame=frame + ) + osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b) + else: + # get the updated states + ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) = _update_states(robot, ee_frame_idx, arm_joint_ids, sim, contact_forces, num_envs) + # compute the joint commands + joint_efforts = osc.compute( + jacobian_b=jacobian_b, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=ee_vel_b, + current_ee_force_b=ee_force_b, + mass_matrix=mass_matrix, + gravity=gravity, + current_joint_pos=joint_pos, + current_joint_vel=joint_vel, + nullspace_joint_pos_target=joint_centers, + ) + robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + + # update marker positions + ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) + goal_marker.visualize(ee_target_pose_w[:, 0:3], ee_target_pose_w[:, 3:7]) + + # perform step + sim.step(render=False) + # update buffers + robot.update(sim_dt) + + +def _update_states( + robot: Articulation, + ee_frame_idx: int, + arm_joint_ids: list[int], + sim: sim_utils.SimulationContext, + contact_forces: ContactSensor | None, + num_envs: int, +): + """Update the states of the robot and obtain the relevant quantities for the operational space controller. + + Args: + robot (Articulation): The robot to control. + ee_frame_idx (int): The index of the end-effector frame. + arm_joint_ids (list[int]): The indices of the arm joints. + sim (sim_utils.SimulationContext): The simulation context. + contact_forces (ContactSensor | None): The contact forces sensor. + num_envs (int): Number of environments. + + Returns: + jacobian_b (torch.tensor): The Jacobian in the root frame. + mass_matrix (torch.tensor): The mass matrix. + gravity (torch.tensor): The gravity vector. + ee_pose_b (torch.tensor): The end-effector pose in the root frame. + ee_vel_b (torch.tensor): The end-effector velocity in the root frame. + root_pose_w (torch.tensor): The root pose in the world frame. + ee_pose_w (torch.tensor): The end-effector pose in the world frame. + ee_force_b (torch.tensor): The end-effector force in the root frame. + joint_pos (torch.tensor): The joint positions. + joint_vel (torch.tensor): The joint velocities. + """ + # obtain dynamics related quantities from simulation + ee_jacobi_idx = ee_frame_idx - 1 + jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] + mass_matrix = robot.root_physx_view.get_generalized_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids] + gravity = robot.root_physx_view.get_gravity_compensation_forces()[:, arm_joint_ids] + # Convert the Jacobian from world to root frame + jacobian_b = jacobian_w.clone() + root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w)) + jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :]) + jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :]) + + # Compute current pose of the end-effector + root_pose_w = robot.data.root_pose_w + ee_pose_w = robot.data.body_pose_w[:, ee_frame_idx] + ee_pos_b, ee_quat_b = subtract_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] + ) + ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) + + # Compute the current velocity of the end-effector + ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame + root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame + relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame + ee_lin_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame + ee_ang_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) + ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1) + + # Calculate the contact force + ee_force_w = torch.zeros(num_envs, 3, device=sim.device) + if contact_forces is not None: # Only modify if it exist + sim_dt = sim.get_physics_dt() + contact_forces.update(sim_dt) # update contact sensor + # Calculate the contact force by averaging over last four time steps (i.e., to smoothen) and + # taking the max of three surfaces as only one should be the contact of interest + ee_force_w, _ = torch.max(torch.mean(contact_forces.data.net_forces_w_history, dim=1), dim=1) + + # This is a simplification, only for the sake of testing. + ee_force_b = ee_force_w + + # Get joint positions and velocities + joint_pos = robot.data.joint_pos[:, arm_joint_ids] + joint_vel = robot.data.joint_vel[:, arm_joint_ids] + + return ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) + + +def _update_target( + osc: OperationalSpaceController, + root_pose_w: torch.tensor, + ee_pose_b: torch.tensor, + target_set: torch.tensor, + current_goal_idx: int, +): + """Update the target for the operational space controller. + + Args: + osc (OperationalSpaceController): The operational space controller. + root_pose_w (torch.tensor): The root pose in the world frame. + ee_pose_b (torch.tensor): The end-effector pose in the body frame. + target_set (torch.tensor): The target set to track. + current_goal_idx (int): The current goal index. + + Returns: + command (torch.tensor): The target command. + ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. + ee_target_pose_w (torch.tensor): The end-effector target pose in the world frame. + next_goal_idx (int): The next goal index. + + Raises: + ValueError: If the target type is undefined. + """ + # update the ee desired command + command = torch.zeros(osc.num_envs, osc.action_dim, device=osc._device) + command[:] = target_set[current_goal_idx] + + # update the ee desired pose + ee_target_pose_b = torch.zeros(osc.num_envs, 7, device=osc._device) + for target_type in osc.cfg.target_types: + if target_type == "pose_abs": + ee_target_pose_b[:] = command[:, :7] + elif target_type == "pose_rel": + ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] = apply_delta_pose( + ee_pose_b[:, :3], ee_pose_b[:, 3:], command[:, :7] + ) + elif target_type == "wrench_abs": + pass # ee_target_pose_b could stay at the root frame for force control, what matters is ee_target_b + else: + raise ValueError("Undefined target_type within _update_target().") - def _update_states( - self, - robot: Articulation, - ee_frame_idx: int, - arm_joint_ids: list[int], - ): - """Update the states of the robot and obtain the relevant quantities for the operational space controller. - - Args: - robot (Articulation): The robot to control. - ee_frame_idx (int): The index of the end-effector frame. - arm_joint_ids (list[int]): The indices of the arm joints. - - Returns: - jacobian_b (torch.tensor): The Jacobian in the root frame. - mass_matrix (torch.tensor): The mass matrix. - gravity (torch.tensor): The gravity vector. - ee_pose_b (torch.tensor): The end-effector pose in the root frame. - ee_vel_b (torch.tensor): The end-effector velocity in the root frame. - root_pose_w (torch.tensor): The root pose in the world frame. - ee_pose_w (torch.tensor): The end-effector pose in the world frame. - ee_force_b (torch.tensor): The end-effector force in the root frame. - joint_pos (torch.tensor): The joint positions. - joint_vel (torch.tensor): The joint velocities. - """ - # obtain dynamics related quantities from simulation - ee_jacobi_idx = ee_frame_idx - 1 - jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] - mass_matrix = robot.root_physx_view.get_generalized_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids] - gravity = robot.root_physx_view.get_gravity_compensation_forces()[:, arm_joint_ids] - # Convert the Jacobian from world to root frame - jacobian_b = jacobian_w.clone() - root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w)) - jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :]) - jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :]) - - # Compute current pose of the end-effector - root_pose_w = robot.data.root_state_w[:, 0:7] - ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] - ee_pos_b, ee_quat_b = subtract_frame_transforms( - root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] - ) - ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) - - # Compute the current velocity of the end-effector - ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame - root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame - relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame - ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame - ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) - ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1) - - # Calculate the contact force - ee_force_w = torch.zeros(self.num_envs, 3, device=self.sim.device) - if self.contact_forces is not None: # Only modify if it exist - sim_dt = self.sim.get_physics_dt() - self.contact_forces.update(sim_dt) # update contact sensor - # Calculate the contact force by averaging over last four time steps (i.e., to smoothen) and - # taking the max of three surfaces as only one should be the contact of interest - ee_force_w, _ = torch.max(torch.mean(self.contact_forces.data.net_forces_w_history, dim=1), dim=1) - - # This is a simplification, only for the sake of testing. - ee_force_b = ee_force_w - - # Get joint positions and velocities - joint_pos = robot.data.joint_pos[:, arm_joint_ids] - joint_vel = robot.data.joint_vel[:, arm_joint_ids] - - return ( - jacobian_b, - mass_matrix, - gravity, - ee_pose_b, - ee_vel_b, - root_pose_w, - ee_pose_w, - ee_force_b, - joint_pos, - joint_vel, - ) - - def _update_target( - self, - osc: OperationalSpaceController, - root_pose_w: torch.tensor, - ee_pose_b: torch.tensor, - target_set: torch.tensor, - current_goal_idx: int, - ): - """Update the target for the operational space controller. - - Args: - osc (OperationalSpaceController): The operational space controller. - root_pose_w (torch.tensor): The root pose in the world frame. - ee_pose_b (torch.tensor): The end-effector pose in the body frame. - target_set (torch.tensor): The target set to track. - current_goal_idx (int): The current goal index. - - Returns: - command (torch.tensor): The target command. - ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. - ee_target_pose_w (torch.tensor): The end-effector target pose in the world frame. - next_goal_idx (int): The next goal index. - - Raises: - ValueError: If the target type is undefined. - """ - # update the ee desired command - command = torch.zeros(self.num_envs, osc.action_dim, device=self.sim.device) - command[:] = target_set[current_goal_idx] - - # update the ee desired pose - ee_target_pose_b = torch.zeros(self.num_envs, 7, device=self.sim.device) - for target_type in osc.cfg.target_types: - if target_type == "pose_abs": - ee_target_pose_b[:] = command[:, :7] - elif target_type == "pose_rel": - ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] = apply_delta_pose( - ee_pose_b[:, :3], ee_pose_b[:, 3:], command[:, :7] - ) - elif target_type == "wrench_abs": - pass # ee_target_pose_b could stay at the root frame for force control, what matters is ee_target_b - else: - raise ValueError("Undefined target_type within _update_target().") + # update the target desired pose in world frame (for marker) + ee_target_pos_w, ee_target_quat_w = combine_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + ) + ee_target_pose_w = torch.cat([ee_target_pos_w, ee_target_quat_w], dim=-1) - # update the target desired pose in world frame (for marker) - ee_target_pos_w, ee_target_quat_w = combine_frame_transforms( - root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] - ) - ee_target_pose_w = torch.cat([ee_target_pos_w, ee_target_quat_w], dim=-1) + next_goal_idx = (current_goal_idx + 1) % len(target_set) - next_goal_idx = (current_goal_idx + 1) % len(target_set) + return command, ee_target_pose_b, ee_target_pose_w, next_goal_idx - return command, ee_target_pose_b, ee_target_pose_w, next_goal_idx - def _convert_to_task_frame( - self, osc: OperationalSpaceController, command: torch.tensor, ee_target_pose_b: torch.tensor - ): - """Convert the target command to the task frame if required. +def _convert_to_task_frame( + osc: OperationalSpaceController, command: torch.tensor, ee_target_pose_b: torch.tensor, frame: str +): + """Convert the target command to the task frame if required. - Args: - osc (OperationalSpaceController): The operational space controller. - command (torch.tensor): The target command to convert. - ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. + Args: + osc (OperationalSpaceController): The operational space controller. + command (torch.tensor): The target command to convert. + ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. + frame (str): The reference frame for targets. - Returns: - command (torch.tensor): The converted target command. - task_frame_pose_b (torch.tensor): The task frame pose in the body frame. + Returns: + command (torch.tensor): The converted target command. + task_frame_pose_b (torch.tensor): The task frame pose in the body frame. - Raises: - ValueError: If the frame is invalid. - """ + Raises: + ValueError: If the frame is invalid. + """ + command = command.clone() + task_frame_pose_b = None + if frame == "root": + # No need to transform anything if they are already in root frame + pass + elif frame == "task": + # Convert target commands from base to the task frame command = command.clone() - task_frame_pose_b = None - if self.frame == "root": - # No need to transform anything if they are already in root frame - pass - elif self.frame == "task": - # Convert target commands from base to the task frame - command = command.clone() - task_frame_pose_b = ee_target_pose_b.clone() - - cmd_idx = 0 - for target_type in osc.cfg.target_types: - if target_type == "pose_abs": - command[:, :3], command[:, 3:7] = subtract_frame_transforms( - task_frame_pose_b[:, :3], task_frame_pose_b[:, 3:], command[:, :3], command[:, 3:7] - ) - cmd_idx += 7 - elif target_type == "pose_rel": - # Compute rotation matrices - R_task_b = matrix_from_quat(task_frame_pose_b[:, 3:]) # Task frame to base frame - R_b_task = R_task_b.mT # Base frame to task frame - # Transform the delta position and orientation from base to task frame - command[:, :3] = (R_b_task @ command[:, :3].unsqueeze(-1)).squeeze(-1) - command[:, 3:7] = (R_b_task @ command[:, 3:7].unsqueeze(-1)).squeeze(-1) - cmd_idx += 6 - elif target_type == "wrench_abs": - # These are already defined in target frame for ee_goal_wrench_set_tilted_task (since it is - # easier), so not transforming - cmd_idx += 6 - else: - raise ValueError("Undefined target_type within _convert_to_task_frame().") - else: - # Raise error for invalid frame - raise ValueError("Invalid frame selection for target setting inside the test_operational_space.") - - return command, task_frame_pose_b - - def _check_convergence( - self, - osc: OperationalSpaceController, - ee_pose_b: torch.tensor, - ee_target_pose_b: torch.tensor, - ee_force_b: torch.tensor, - ee_target_b: torch.tensor, - ): - """Check the convergence to the target. - - Args: - osc (OperationalSpaceController): The operational space controller. - ee_pose_b (torch.tensor): The end-effector pose in the body frame. - ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. - ee_force_b (torch.tensor): The end-effector force in the body frame. - ee_target_b (torch.tensor): The end-effector target in the body frame. - - Raises: - AssertionError: If the convergence is not achieved. - ValueError: If the target type is undefined. - """ + task_frame_pose_b = ee_target_pose_b.clone() + cmd_idx = 0 for target_type in osc.cfg.target_types: if target_type == "pose_abs": - pos_error, rot_error = compute_pose_error( - ee_pose_b[:, 0:3], ee_pose_b[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + command[:, :3], command[:, 3:7] = subtract_frame_transforms( + task_frame_pose_b[:, :3], task_frame_pose_b[:, 3:], command[:, :3], command[:, 3:7] ) - pos_error_norm = torch.norm(pos_error * self.pos_mask, dim=-1) - rot_error_norm = torch.norm(rot_error * self.rot_mask, dim=-1) - # desired error (zer) - des_error = torch.zeros_like(pos_error_norm) - # check convergence - torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=0.1) - torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=0.1) cmd_idx += 7 elif target_type == "pose_rel": - pos_error, rot_error = compute_pose_error( - ee_pose_b[:, 0:3], ee_pose_b[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] - ) - pos_error_norm = torch.norm(pos_error * self.pos_mask, dim=-1) - rot_error_norm = torch.norm(rot_error * self.rot_mask, dim=-1) - # desired error (zer) - des_error = torch.zeros_like(pos_error_norm) - # check convergence - torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=0.1) - torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=0.1) + # Compute rotation matrices + R_task_b = matrix_from_quat(task_frame_pose_b[:, 3:]) # Task frame to base frame + R_b_task = R_task_b.mT # Base frame to task frame + # Transform the delta position and orientation from base to task frame + command[:, :3] = (R_b_task @ command[:, :3].unsqueeze(-1)).squeeze(-1) + command[:, 3:7] = (R_b_task @ command[:, 3:7].unsqueeze(-1)).squeeze(-1) cmd_idx += 6 elif target_type == "wrench_abs": - force_target_b = ee_target_b[:, cmd_idx : cmd_idx + 3].clone() - # Convert to base frame if the target was defined in task frame - if self.frame == "task": - task_frame_pose_b = ee_target_pose_b.clone() - R_task_b = matrix_from_quat(task_frame_pose_b[:, 3:]) - force_target_b[:] = (R_task_b @ force_target_b[:].unsqueeze(-1)).squeeze(-1) - force_error = ee_force_b - force_target_b - force_error_norm = torch.norm( - force_error * self.force_mask, dim=-1 - ) # ignore torque part as we cannot measure it - des_error = torch.zeros_like(force_error_norm) - # check convergence: big threshold here as the force control is not precise when the robot moves - torch.testing.assert_close(force_error_norm, des_error, rtol=0.0, atol=1.0) + # These are already defined in target frame for ee_goal_wrench_set_tilted_task (since it is + # easier), so not transforming cmd_idx += 6 else: - raise ValueError("Undefined target_type within _check_convergence().") - - -if __name__ == "__main__": - run_tests() + raise ValueError("Undefined target_type within _convert_to_task_frame().") + else: + # Raise error for invalid frame + raise ValueError("Invalid frame selection for target setting inside the test_operational_space.") + + return command, task_frame_pose_b + + +def _check_convergence( + osc: OperationalSpaceController, + ee_pose_b: torch.tensor, + ee_target_pose_b: torch.tensor, + ee_force_b: torch.tensor, + ee_target_b: torch.tensor, + pos_mask: torch.tensor, + rot_mask: torch.tensor, + force_mask: torch.tensor, + frame: str, +): + """Check the convergence to the target. + + Args: + osc (OperationalSpaceController): The operational space controller. + ee_pose_b (torch.tensor): The end-effector pose in the body frame. + ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. + ee_force_b (torch.tensor): The end-effector force in the body frame. + ee_target_b (torch.tensor): The end-effector target in the body frame. + pos_mask (torch.tensor): The position mask. + rot_mask (torch.tensor): The rotation mask. + force_mask (torch.tensor): The force mask. + frame (str): The reference frame for targets. + + Raises: + AssertionError: If the convergence is not achieved. + ValueError: If the target type is undefined. + """ + cmd_idx = 0 + for target_type in osc.cfg.target_types: + if target_type == "pose_abs": + pos_error, rot_error = compute_pose_error( + ee_pose_b[:, 0:3], ee_pose_b[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + ) + pos_error_norm = torch.norm(pos_error * pos_mask, dim=-1) + rot_error_norm = torch.norm(rot_error * rot_mask, dim=-1) + # desired error (zer) + des_error = torch.zeros_like(pos_error_norm) + # check convergence + torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=0.1) + torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=0.1) + cmd_idx += 7 + elif target_type == "pose_rel": + pos_error, rot_error = compute_pose_error( + ee_pose_b[:, 0:3], ee_pose_b[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + ) + pos_error_norm = torch.norm(pos_error * pos_mask, dim=-1) + rot_error_norm = torch.norm(rot_error * rot_mask, dim=-1) + # desired error (zer) + des_error = torch.zeros_like(pos_error_norm) + # check convergence + torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=0.1) + torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=0.1) + cmd_idx += 6 + elif target_type == "wrench_abs": + force_target_b = ee_target_b[:, cmd_idx : cmd_idx + 3].clone() + # Convert to base frame if the target was defined in task frame + if frame == "task": + task_frame_pose_b = ee_target_pose_b.clone() + R_task_b = matrix_from_quat(task_frame_pose_b[:, 3:]) + force_target_b[:] = (R_task_b @ force_target_b[:].unsqueeze(-1)).squeeze(-1) + force_error = ee_force_b - force_target_b + force_error_norm = torch.norm( + force_error * force_mask, dim=-1 + ) # ignore torque part as we cannot measure it + des_error = torch.zeros_like(force_error_norm) + # check convergence: big threshold here as the force control is not precise when the robot moves + torch.testing.assert_close(force_error_norm, des_error, rtol=0.0, atol=1.0) + cmd_idx += 6 + else: + raise ValueError("Undefined target_type within _check_convergence().") diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py new file mode 100644 index 000000000000..43ab48ae0590 --- /dev/null +++ b/source/isaaclab/test/controllers/test_pink_ik.py @@ -0,0 +1,445 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +# 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 + +"""Rest everything follows.""" + +import contextlib +import json +import re +from pathlib import Path + +import gymnasium as gym +import numpy as np +import pytest +import torch +from pink.configuration import Configuration +from pink.tasks import FrameTask + +import omni.usd + +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 + + +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) + + +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(env_name, num_envs): + """Create a test environment with the Pink IK controller.""" + 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 + del env_cfg.scene.packing_table + del env_cfg.terminations.object_dropping + del env_cfg.terminations.time_out + return gym.make(env_name, cfg=env_cfg).unwrapped, env_cfg + except Exception as e: + print(f"Failed to create environment: {str(e)}") + raise + + +@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_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]) + + # 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, test_cfg, test_params = env_and_cfg + + 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="upper_body_ik") + pink_controllers = action_term._ik_controllers + articulation = action_term._asset + + # Initialize Pink Configuration for forward kinematics + 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.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, + "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"], + } + + +@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 + + 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): + """Run a movement test with the given configuration.""" + env = test_setup["env"] + num_joints_in_robot_hands = test_setup["num_joints_in_robot_hands"] + + left_hand_poses = np.array(test_config["left_hand_pose"], dtype=np.float32) + right_hand_poses = np.array(test_config["right_hand_pose"], dtype=np.float32) + + curr_pose_idx = 0 + test_counter = 0 + num_runs = 0 + + 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: + aux_function(num_runs) + + # Create actions from hand poses and joint positions + 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 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["left_eef_urdf_link_name"], + test_setup["right_eef_urdf_link_name"], + ) + print_debug_info(errors, test_counter) + 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: + test_counter += 1 + 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): + """Get the position and orientation of a link.""" + link_index = env.scene["robot"].data.body_names.index(link_name) + link_states = env.scene._articulations["robot"]._data.body_link_state_w + link_pose = link_states[:, link_index, :7] + return link_pose[:, :3], link_pose[:, 3:7] + + +def calculate_rotation_error(current_rot, target_rot): + """Calculate the rotation error between current and target orientations in axis-angle format.""" + if isinstance(target_rot, torch.Tensor): + target_rot_tensor = ( + target_rot.unsqueeze(0).expand(current_rot.shape[0], -1) if target_rot.dim() == 1 else target_rot + ) + else: + target_rot_tensor = torch.tensor(target_rot, device=current_rot.device) + if target_rot_tensor.dim() == 1: + target_rot_tensor = target_rot_tensor.unsqueeze(0).expand(current_rot.shape[0], -1) + + return axis_angle_from_quat( + quat_from_matrix(matrix_from_quat(target_rot_tensor) * matrix_from_quat(quat_inv(current_rot))) + ) + + +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"] + 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"] + + # Get current hand positions and orientations + left_hand_pos, left_hand_rot = get_link_pose(env, left_target_link_name) + right_hand_pos, right_hand_rot = get_link_pose(env, right_target_link_name) + + # Create setpoint tensors + device = env.device + 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) + + # Calculate position and rotation errors + left_pos_error = left_hand_pose_setpoint[:, :3] - left_hand_pos + right_pos_error = right_hand_pose_setpoint[:, :3] - right_hand_pos + left_rot_error = calculate_rotation_error(left_hand_rot, left_hand_pose_setpoint[:, 3:]) + right_rot_error = calculate_rotation_error(right_hand_rot, right_hand_pose_setpoint[:, 3:]) + + # Calculate PD controller errors + ik_controller = pink_controllers[0] + isaaclab_controlled_joint_ids = action_term._isaaclab_controlled_joint_ids + + # 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 (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 + 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 = ( + torch.tensor(left_target_pos - left_curr_pos, device=device, dtype=torch.float32) + .unsqueeze(0) + .repeat(num_envs, 1) + ) + right_pd_error = ( + torch.tensor(right_target_pos - right_curr_pos, device=device, dtype=torch.float32) + .unsqueeze(0) + .repeat(num_envs, 1) + ) + + return { + "left_pos_error": left_pos_error, + "right_pos_error": right_pos_error, + "left_rot_error": left_rot_error, + "right_rot_error": right_rot_error, + "left_pd_error": left_pd_error, + "right_pd_error": right_pd_error, + } + + +def verify_errors(errors, test_setup, tolerances): + """Verify that all error metrics are within tolerance.""" + env = test_setup["env"] + device = env.device + num_envs = env.num_envs + zero_tensor = torch.zeros(num_envs, device=device) + + for hand in ["left", "right"]: + # Check PD controller errors + pd_error_norm = torch.norm(errors[f"{hand}_pd_error"], dim=1) + torch.testing.assert_close( + pd_error_norm, + zero_tensor, + rtol=0.0, + atol=tolerances["pd_position"], + msg=( + f"{hand.capitalize()} hand PD controller error ({pd_error_norm.item():.6f}) exceeds tolerance" + f" ({tolerances['pd_position']:.6f})" + ), + ) + + # Check IK position errors + pos_error_norm = torch.norm(errors[f"{hand}_pos_error"], dim=1) + torch.testing.assert_close( + pos_error_norm, + zero_tensor, + rtol=0.0, + atol=tolerances["position"], + msg=( + f"{hand.capitalize()} hand IK position error ({pos_error_norm.item():.6f}) exceeds tolerance" + f" ({tolerances['position']:.6f})" + ), + ) + + # Check rotation errors + rot_error_max = torch.max(errors[f"{hand}_rot_error"]) + torch.testing.assert_close( + rot_error_max, + torch.zeros_like(rot_error_max), + rtol=0.0, + atol=tolerances["rotation"], + msg=( + f"{hand.capitalize()} hand IK rotation error ({rot_error_max.item():.6f}) exceeds tolerance" + f" ({tolerances['rotation']:.6f})" + ), + ) + + +def print_debug_info(errors, test_counter): + """Print debug information about the current state.""" + print(f"\nTest iteration {test_counter + 1}:") + for hand in ["left", "right"]: + print(f"Measured {hand} hand position error:", errors[f"{hand}_pos_error"]) + print(f"Measured {hand} hand rotation error:", errors[f"{hand}_rot_error"]) + print(f"Measured {hand} hand PD error:", errors[f"{hand}_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 000000000000..6bde4c30a1b6 --- /dev/null +++ b/source/isaaclab/test/controllers/test_pink_ik_components.py @@ -0,0 +1,309 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test 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 + +from pathlib import Path + +import numpy as np +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 000000000000..cb1a305c50da --- /dev/null +++ b/source/isaaclab/test/controllers/urdfs/test_urdf_two_link_robot.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/source/isaaclab/test/deps/isaacsim/check_camera.py b/source/isaaclab/test/deps/isaacsim/check_camera.py index c4f788720900..81f481f23e3f 100644 --- a/source/isaaclab/test/deps/isaacsim/check_camera.py +++ b/source/isaaclab/test/deps/isaacsim/check_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -41,22 +41,18 @@ """Rest everything follows.""" -import numpy as np import os import random -try: - import isaacsim.storage.native as nucleus_utils -except ModuleNotFoundError: - import isaacsim.core.utils.nucleus as nucleus_utils +import numpy as np +from PIL import Image, ImageChops +import isaacsim.core.utils.nucleus as nucleus_utils import isaacsim.core.utils.prims as prim_utils import omni.replicator.core as rep from isaacsim.core.api.world import World from isaacsim.core.prims import Articulation, RigidPrim, SingleGeometryPrim, SingleRigidPrim -from isaacsim.core.utils.carb import set_carb_setting from isaacsim.core.utils.viewports import set_camera_view -from PIL import Image, ImageChops from pxr import Gf, UsdGeom # check nucleus connection @@ -85,7 +81,7 @@ def main(): world.get_physics_context().enable_flatcache(True) # Enable hydra scene-graph instancing # this is needed to visualize the scene when flatcache is enabled - set_carb_setting(world._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + world._settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) # Populate scene # Ground @@ -126,7 +122,7 @@ def main(): # Robot prim_utils.create_prim( "/World/Robot", - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_instanceable.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_c/anymal_c.usd", translation=(0.0, 0.0, 0.6), ) # Setup camera sensor on the robot diff --git a/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py b/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py index e98e43673b3d..0be9a55bd4cb 100644 --- a/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py +++ b/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -30,27 +30,31 @@ """Rest everything follows.""" +import logging + import torch import isaacsim.core.utils.nucleus as nucleus_utils import isaacsim.core.utils.prims as prim_utils import isaacsim.core.utils.stage as stage_utils import omni.kit.commands -import omni.log import omni.physx from isaacsim.core.api.world import World from isaacsim.core.prims import Articulation -from isaacsim.core.utils.carb import set_carb_setting from isaacsim.core.utils.viewports import set_camera_view from pxr import PhysxSchema, UsdPhysics +# import logger +logger = logging.getLogger(__name__) + + # check nucleus connection if nucleus_utils.get_assets_root_path() is None: msg = ( "Unable to perform Nucleus login on Omniverse. Assets root path is not set.\n" "\tPlease check: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus" ) - omni.log.error(msg) + logger.error(msg) raise RuntimeError(msg) @@ -75,7 +79,7 @@ def main(): # Enable hydra scene-graph instancing # this is needed to visualize the scene when flatcache is enabled - set_carb_setting(world._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + world._settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) # Spawn things into stage # Ground-plane diff --git a/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py b/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py index 573e6e1d6329..57c016d7522d 100644 --- a/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py +++ b/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -40,30 +40,28 @@ """Rest everything follows.""" +import logging import os -import torch - -import omni.log -try: - import isaacsim.storage.native as nucleus_utils -except ModuleNotFoundError: - import isaacsim.core.utils.nucleus as nucleus_utils +import torch +import isaacsim.core.utils.nucleus as nucleus_utils import isaacsim.core.utils.prims as prim_utils from isaacsim.core.api.world import World from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import Articulation -from isaacsim.core.utils.carb import set_carb_setting from isaacsim.core.utils.viewports import set_camera_view +# import logger +logger = logging.getLogger(__name__) + # check nucleus connection if nucleus_utils.get_assets_root_path() is None: msg = ( "Unable to perform Nucleus login on Omniverse. Assets root path is not set.\n" "\tPlease check: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus" ) - omni.log.error(msg) + logger.error(msg) raise RuntimeError(msg) @@ -89,7 +87,7 @@ def main(): # Enable hydra scene-graph instancing # this is needed to visualize the scene when flatcache is enabled - set_carb_setting(world._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + world._settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) # Create interface to clone the scene cloner = GridCloner(spacing=2.0) @@ -110,7 +108,7 @@ def main(): usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd" root_prim_path = "/World/envs/env_.*/Robot/base" elif args_cli.asset == "oige": - usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_instanceable.usd" + usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_c/anymal_c.usd" root_prim_path = "/World/envs/env_.*/Robot" elif os.path.exists(args_cli.asset): usd_path = args_cli.asset diff --git a/source/isaaclab/test/deps/isaacsim/check_ref_count.py b/source/isaaclab/test/deps/isaacsim/check_ref_count.py index 41a3cd64f96b..7927b8cb01a1 100644 --- a/source/isaaclab/test/deps/isaacsim/check_ref_count.py +++ b/source/isaaclab/test/deps/isaacsim/check_ref_count.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,19 +35,18 @@ import ctypes import gc -import torch # noqa: F401 - -import omni.log +import logging -try: - import isaacsim.storage.native as nucleus_utils -except ModuleNotFoundError: - import isaacsim.core.utils.nucleus as nucleus_utils +import torch # noqa: F401 +import isaacsim.core.utils.nucleus as nucleus_utils import isaacsim.core.utils.prims as prim_utils from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.prims import Articulation -from isaacsim.core.utils.carb import set_carb_setting + +# import logger +logger = logging.getLogger(__name__) + # check nucleus connection if nucleus_utils.get_assets_root_path() is None: @@ -55,7 +54,7 @@ "Unable to perform Nucleus login on Omniverse. Assets root path is not set.\n" "\tPlease check: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus" ) - omni.log.error(msg) + logger.error(msg) raise RuntimeError(msg) @@ -110,7 +109,7 @@ def main(): # Enable hydra scene-graph instancing # this is needed to visualize the scene when flatcache is enabled - set_carb_setting(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + sim._settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) # Create a dummy tensor for testing # Uncommenting the following line will yield a reference count of 1 for the robot (as desired) diff --git a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py index c02bfbda18ef..170bf1b9683c 100644 --- a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py +++ b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -47,9 +47,9 @@ import isaacsim.core.utils.prims as prim_utils import omni.replicator.core as rep +from isaacsim.core.api.objects import DynamicSphere from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner -from isaacsim.core.objects import DynamicSphere from isaacsim.core.prims import RigidPrim from isaacsim.core.utils.viewports import set_camera_view diff --git a/source/isaaclab/test/deps/test_scipy.py b/source/isaaclab/test/deps/test_scipy.py index 3989dd2cc25a..d697716aad7a 100644 --- a/source/isaaclab/test/deps/test_scipy.py +++ b/source/isaaclab/test/deps/test_scipy.py @@ -1,75 +1,63 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause # isort: off import warnings +import pytest warnings.filterwarnings("ignore", category=DeprecationWarning) # isort: on import numpy as np import scipy.interpolate as interpolate -import unittest -from isaaclab.app import run_tests - -class TestScipyOperations(unittest.TestCase): - """Tests for assuring scipy related operations used in Isaac Lab.""" - - def test_interpolation(self): - """Test scipy interpolation 2D method.""" - # parameters - size = (10.0, 12.0) - horizontal_scale = 0.1 - vertical_scale = 0.005 - downsampled_scale = 0.2 - noise_range = (-0.02, 0.1) - noise_step = 0.02 - # switch parameters to discrete units - # -- horizontal scale - width_pixels = int(size[0] / horizontal_scale) - length_pixels = int(size[1] / horizontal_scale) - # -- downsampled scale - width_downsampled = int(size[0] / downsampled_scale) - length_downsampled = int(size[1] / downsampled_scale) - # -- height - height_min = int(noise_range[0] / vertical_scale) - height_max = int(noise_range[1] / vertical_scale) - height_step = int(noise_step / 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, size[0] * horizontal_scale, width_downsampled) - y = np.linspace(0, size[1] * horizontal_scale, length_downsampled) - - # interpolate the sampled heights to obtain the height field - x_upsampled = np.linspace(0, size[0] * horizontal_scale, width_pixels) - y_upsampled = np.linspace(0, size[1] * horizontal_scale, length_pixels) - # -- method 1: interp2d (this will be deprecated in the future 1.12 release) - func_interp2d = interpolate.interp2d(y, x, height_field_downsampled, kind="cubic") - z_upsampled_interp2d = func_interp2d(y_upsampled, x_upsampled) - # -- method 2: RectBivariateSpline (alternate to interp2d) - func_RectBiVariate = interpolate.RectBivariateSpline(x, y, height_field_downsampled) - z_upsampled_RectBivariant = func_RectBiVariate(x_upsampled, y_upsampled) - # -- method 3: RegularGridInterpolator (recommended from scipy but slow!) - # Ref: https://github.com/scipy/scipy/issues/18010 - func_RegularGridInterpolator = interpolate.RegularGridInterpolator( - (x, y), height_field_downsampled, method="cubic" - ) - xx_upsampled, yy_upsampled = np.meshgrid(x_upsampled, y_upsampled, indexing="ij", sparse=True) - z_upsampled_RegularGridInterpolator = func_RegularGridInterpolator((xx_upsampled, yy_upsampled)) - - # check if the interpolated height field is the same as the sampled height field - np.testing.assert_allclose(z_upsampled_interp2d, z_upsampled_RectBivariant, atol=1e-14) - np.testing.assert_allclose(z_upsampled_RectBivariant, z_upsampled_RegularGridInterpolator, atol=1e-14) - np.testing.assert_allclose(z_upsampled_RegularGridInterpolator, z_upsampled_interp2d, atol=1e-14) - - -if __name__ == "__main__": - run_tests() +@pytest.mark.isaacsim_ci +def test_interpolation(): + """Test scipy interpolation 2D method.""" + # parameters + size = (10.0, 12.0) + horizontal_scale = 0.1 + vertical_scale = 0.005 + downsampled_scale = 0.2 + noise_range = (-0.02, 0.1) + noise_step = 0.02 + # switch parameters to discrete units + # -- horizontal scale + width_pixels = int(size[0] / horizontal_scale) + length_pixels = int(size[1] / horizontal_scale) + # -- downsampled scale + width_downsampled = int(size[0] / downsampled_scale) + length_downsampled = int(size[1] / downsampled_scale) + # -- height + height_min = int(noise_range[0] / vertical_scale) + height_max = int(noise_range[1] / vertical_scale) + height_step = int(noise_step / 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, size[0] * horizontal_scale, width_downsampled) + y = np.linspace(0, size[1] * horizontal_scale, length_downsampled) + + # interpolate the sampled heights to obtain the height field + x_upsampled = np.linspace(0, size[0] * horizontal_scale, width_pixels) + y_upsampled = np.linspace(0, size[1] * horizontal_scale, length_pixels) + # -- method 1: RegularGridInterpolator (replacing deprecated interp2d) + func_RegularGridInterpolator = interpolate.RegularGridInterpolator((x, y), height_field_downsampled, method="cubic") + xx_upsampled, yy_upsampled = np.meshgrid(x_upsampled, y_upsampled, indexing="ij", sparse=True) + z_upsampled_RegularGridInterpolator = func_RegularGridInterpolator((xx_upsampled, yy_upsampled)) + # -- method 2: RectBivariateSpline (alternate to interp2d) + func_RectBiVariate = interpolate.RectBivariateSpline(x, y, height_field_downsampled) + z_upsampled_RectBivariant = func_RectBiVariate(x_upsampled, y_upsampled) + + # check if the interpolated height field is the same as the sampled height field + np.testing.assert_allclose(z_upsampled_RegularGridInterpolator, z_upsampled_RectBivariant, atol=1e-2, rtol=1e-2) + np.testing.assert_allclose(z_upsampled_RectBivariant, z_upsampled_RegularGridInterpolator, atol=1e-2, rtol=1e-2) + np.testing.assert_allclose( + z_upsampled_RegularGridInterpolator, z_upsampled_RegularGridInterpolator, atol=1e-2, rtol=1e-2 + ) diff --git a/source/isaaclab/test/deps/test_torch.py b/source/isaaclab/test/deps/test_torch.py index f3769ca2915f..6a50110757de 100644 --- a/source/isaaclab/test/deps/test_torch.py +++ b/source/isaaclab/test/deps/test_torch.py @@ -1,154 +1,156 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +import pytest import torch import torch.utils.benchmark as benchmark -import unittest - -from isaaclab.app import run_tests - - -class TestTorchOperations(unittest.TestCase): - """Tests for assuring torch related operations used in Isaac Lab.""" - - def test_array_slicing(self): - """Check that using ellipsis and slices work for torch tensors.""" - - size = (400, 300, 5) - my_tensor = torch.rand(size, device="cuda:0") - - self.assertEqual(my_tensor[..., 0].shape, (400, 300)) - self.assertEqual(my_tensor[:, :, 0].shape, (400, 300)) - self.assertEqual(my_tensor[slice(None), slice(None), 0].shape, (400, 300)) - with self.assertRaises(IndexError): - my_tensor[..., ..., 0] - - self.assertEqual(my_tensor[0, ...].shape, (300, 5)) - self.assertEqual(my_tensor[0, :, :].shape, (300, 5)) - self.assertEqual(my_tensor[0, slice(None), slice(None)].shape, (300, 5)) - self.assertEqual(my_tensor[0, ..., ...].shape, (300, 5)) - - self.assertEqual(my_tensor[..., 0, 0].shape, (400,)) - self.assertEqual(my_tensor[slice(None), 0, 0].shape, (400,)) - self.assertEqual(my_tensor[:, 0, 0].shape, (400,)) - - def test_array_circular(self): - """Check circular buffer implementation in torch.""" - - size = (10, 30, 5) - my_tensor = torch.rand(size, device="cuda:0") - - # roll up the tensor without cloning - my_tensor_1 = my_tensor.clone() - my_tensor_1[:, 1:, :] = my_tensor_1[:, :-1, :] - my_tensor_1[:, 0, :] = my_tensor[:, -1, :] - # check that circular buffer works as expected - error = torch.max(torch.abs(my_tensor_1 - my_tensor.roll(1, dims=1))) - self.assertNotEqual(error.item(), 0.0) - self.assertFalse(torch.allclose(my_tensor_1, my_tensor.roll(1, dims=1))) - - # roll up the tensor with cloning - my_tensor_2 = my_tensor.clone() - my_tensor_2[:, 1:, :] = my_tensor_2[:, :-1, :].clone() - my_tensor_2[:, 0, :] = my_tensor[:, -1, :] - # check that circular buffer works as expected - error = torch.max(torch.abs(my_tensor_2 - my_tensor.roll(1, dims=1))) - self.assertEqual(error.item(), 0.0) - self.assertTrue(torch.allclose(my_tensor_2, my_tensor.roll(1, dims=1))) - - # roll up the tensor with detach operation - my_tensor_3 = my_tensor.clone() - my_tensor_3[:, 1:, :] = my_tensor_3[:, :-1, :].detach() - my_tensor_3[:, 0, :] = my_tensor[:, -1, :] - # check that circular buffer works as expected - error = torch.max(torch.abs(my_tensor_3 - my_tensor.roll(1, dims=1))) - self.assertNotEqual(error.item(), 0.0) - self.assertFalse(torch.allclose(my_tensor_3, my_tensor.roll(1, dims=1))) - - # roll up the tensor with roll operation - my_tensor_4 = my_tensor.clone() - my_tensor_4 = my_tensor_4.roll(1, dims=1) - my_tensor_4[:, 0, :] = my_tensor[:, -1, :] - # check that circular buffer works as expected - error = torch.max(torch.abs(my_tensor_4 - my_tensor.roll(1, dims=1))) - self.assertEqual(error.item(), 0.0) - self.assertTrue(torch.allclose(my_tensor_4, my_tensor.roll(1, dims=1))) - - def test_array_circular_copy(self): - """Check that circular buffer implementation in torch is copying data.""" - - size = (10, 30, 5) - my_tensor = torch.rand(size, device="cuda:0") - my_tensor_clone = my_tensor.clone() - - # roll up the tensor - my_tensor_1 = my_tensor.clone() - my_tensor_1[:, 1:, :] = my_tensor_1[:, :-1, :].clone() - my_tensor_1[:, 0, :] = my_tensor[:, -1, :] - # change the source tensor - my_tensor[:, 0, :] = 1000 - # check that circular buffer works as expected - self.assertFalse(torch.allclose(my_tensor_1, my_tensor.roll(1, dims=1))) - self.assertTrue(torch.allclose(my_tensor_1, my_tensor_clone.roll(1, dims=1))) - - def test_array_multi_indexing(self): - """Check multi-indexing works for torch tensors.""" - - size = (400, 300, 5) - my_tensor = torch.rand(size, device="cuda:0") - - # this fails since array indexing cannot be broadcasted!! - with self.assertRaises(IndexError): - my_tensor[[0, 1, 2, 3], [0, 1, 2, 3, 4]] - - def test_array_single_indexing(self): - """Check how indexing effects the returned tensor.""" - - size = (400, 300, 5) - my_tensor = torch.rand(size, device="cuda:0") - - # obtain a slice of the tensor - my_slice = my_tensor[0, ...] - self.assertEqual(my_slice.untyped_storage().data_ptr(), my_tensor.untyped_storage().data_ptr()) - - # obtain a slice over ranges - my_slice = my_tensor[0:2, ...] - self.assertEqual(my_slice.untyped_storage().data_ptr(), my_tensor.untyped_storage().data_ptr()) - - # obtain a slice over list - my_slice = my_tensor[[0, 1], ...] - self.assertNotEqual(my_slice.untyped_storage().data_ptr(), my_tensor.untyped_storage().data_ptr()) - - # obtain a slice over tensor - my_slice = my_tensor[torch.tensor([0, 1]), ...] - self.assertNotEqual(my_slice.untyped_storage().data_ptr(), my_tensor.untyped_storage().data_ptr()) - - def test_logical_or(self): - """Test bitwise or operation.""" - - size = (400, 300, 5) - my_tensor_1 = torch.rand(size, device="cuda:0") > 0.5 - my_tensor_2 = torch.rand(size, device="cuda:0") < 0.5 - - # check the speed of logical or - timer_logical_or = benchmark.Timer( - stmt="torch.logical_or(my_tensor_1, my_tensor_2)", - globals={"my_tensor_1": my_tensor_1, "my_tensor_2": my_tensor_2}, - ) - timer_bitwise_or = benchmark.Timer( - stmt="my_tensor_1 | my_tensor_2", globals={"my_tensor_1": my_tensor_1, "my_tensor_2": my_tensor_2} - ) - - print("Time for logical or:", timer_logical_or.timeit(number=1000)) - print("Time for bitwise or:", timer_bitwise_or.timeit(number=1000)) - # check that logical or works as expected - output_logical_or = torch.logical_or(my_tensor_1, my_tensor_2) - output_bitwise_or = my_tensor_1 | my_tensor_2 - - self.assertTrue(torch.allclose(output_logical_or, output_bitwise_or)) - - -if __name__ == "__main__": - run_tests() + + +@pytest.mark.isaacsim_ci +def test_array_slicing(): + """Check that using ellipsis and slices work for torch tensors.""" + + size = (400, 300, 5) + my_tensor = torch.rand(size, device="cuda:0") + + assert my_tensor[..., 0].shape == (400, 300) + assert my_tensor[:, :, 0].shape == (400, 300) + assert my_tensor[slice(None), slice(None), 0].shape == (400, 300) + with pytest.raises(IndexError): + my_tensor[..., ..., 0] + + assert my_tensor[0, ...].shape == (300, 5) + assert my_tensor[0, :, :].shape == (300, 5) + assert my_tensor[0, slice(None), slice(None)].shape == (300, 5) + assert my_tensor[0, ..., ...].shape == (300, 5) + + assert my_tensor[..., 0, 0].shape == (400,) + assert my_tensor[slice(None), 0, 0].shape == (400,) + assert my_tensor[:, 0, 0].shape == (400,) + + +@pytest.mark.isaacsim_ci +def test_array_circular(): + """Check circular buffer implementation in torch.""" + + size = (10, 30, 5) + my_tensor = torch.rand(size, device="cuda:0") + + # roll up the tensor without cloning + my_tensor_1 = my_tensor.clone() + my_tensor_1[:, 1:, :] = my_tensor_1[:, :-1, :] + my_tensor_1[:, 0, :] = my_tensor[:, -1, :] + # check that circular buffer works as expected + error = torch.max(torch.abs(my_tensor_1 - my_tensor.roll(1, dims=1))) + assert error.item() != 0.0 + assert not torch.allclose(my_tensor_1, my_tensor.roll(1, dims=1)) + + # roll up the tensor with cloning + my_tensor_2 = my_tensor.clone() + my_tensor_2[:, 1:, :] = my_tensor_2[:, :-1, :].clone() + my_tensor_2[:, 0, :] = my_tensor[:, -1, :] + # check that circular buffer works as expected + error = torch.max(torch.abs(my_tensor_2 - my_tensor.roll(1, dims=1))) + assert error.item() == 0.0 + assert torch.allclose(my_tensor_2, my_tensor.roll(1, dims=1)) + + # roll up the tensor with detach operation + my_tensor_3 = my_tensor.clone() + my_tensor_3[:, 1:, :] = my_tensor_3[:, :-1, :].detach() + my_tensor_3[:, 0, :] = my_tensor[:, -1, :] + # check that circular buffer works as expected + error = torch.max(torch.abs(my_tensor_3 - my_tensor.roll(1, dims=1))) + assert error.item() != 0.0 + assert not torch.allclose(my_tensor_3, my_tensor.roll(1, dims=1)) + + # roll up the tensor with roll operation + my_tensor_4 = my_tensor.clone() + my_tensor_4 = my_tensor_4.roll(1, dims=1) + my_tensor_4[:, 0, :] = my_tensor[:, -1, :] + # check that circular buffer works as expected + error = torch.max(torch.abs(my_tensor_4 - my_tensor.roll(1, dims=1))) + assert error.item() == 0.0 + assert torch.allclose(my_tensor_4, my_tensor.roll(1, dims=1)) + + +@pytest.mark.isaacsim_ci +def test_array_circular_copy(): + """Check that circular buffer implementation in torch is copying data.""" + + size = (10, 30, 5) + my_tensor = torch.rand(size, device="cuda:0") + my_tensor_clone = my_tensor.clone() + + # roll up the tensor + my_tensor_1 = my_tensor.clone() + my_tensor_1[:, 1:, :] = my_tensor_1[:, :-1, :].clone() + my_tensor_1[:, 0, :] = my_tensor[:, -1, :] + # change the source tensor + my_tensor[:, 0, :] = 1000 + # check that circular buffer works as expected + assert not torch.allclose(my_tensor_1, my_tensor.roll(1, dims=1)) + assert torch.allclose(my_tensor_1, my_tensor_clone.roll(1, dims=1)) + + +@pytest.mark.isaacsim_ci +def test_array_multi_indexing(): + """Check multi-indexing works for torch tensors.""" + + size = (400, 300, 5) + my_tensor = torch.rand(size, device="cuda:0") + + # this fails since array indexing cannot be broadcasted!! + with pytest.raises(IndexError): + my_tensor[[0, 1, 2, 3], [0, 1, 2, 3, 4]] + + +@pytest.mark.isaacsim_ci +def test_array_single_indexing(): + """Check how indexing effects the returned tensor.""" + + size = (400, 300, 5) + my_tensor = torch.rand(size, device="cuda:0") + + # obtain a slice of the tensor + my_slice = my_tensor[0, ...] + assert my_slice.untyped_storage().data_ptr() == my_tensor.untyped_storage().data_ptr() + + # obtain a slice over ranges + my_slice = my_tensor[0:2, ...] + assert my_slice.untyped_storage().data_ptr() == my_tensor.untyped_storage().data_ptr() + + # obtain a slice over list + my_slice = my_tensor[[0, 1], ...] + assert my_slice.untyped_storage().data_ptr() != my_tensor.untyped_storage().data_ptr() + + # obtain a slice over tensor + my_slice = my_tensor[torch.tensor([0, 1]), ...] + assert my_slice.untyped_storage().data_ptr() != my_tensor.untyped_storage().data_ptr() + + +@pytest.mark.isaacsim_ci +def test_logical_or(): + """Test bitwise or operation.""" + + size = (400, 300, 5) + my_tensor_1 = torch.rand(size, device="cuda:0") > 0.5 + my_tensor_2 = torch.rand(size, device="cuda:0") < 0.5 + + # check the speed of logical or + timer_logical_or = benchmark.Timer( + stmt="torch.logical_or(my_tensor_1, my_tensor_2)", + globals={"my_tensor_1": my_tensor_1, "my_tensor_2": my_tensor_2}, + ) + timer_bitwise_or = benchmark.Timer( + stmt="my_tensor_1 | my_tensor_2", globals={"my_tensor_1": my_tensor_1, "my_tensor_2": my_tensor_2} + ) + + print("Time for logical or:", timer_logical_or.timeit(number=1000)) + print("Time for bitwise or:", timer_bitwise_or.timeit(number=1000)) + # check that logical or works as expected + output_logical_or = torch.logical_or(my_tensor_1, my_tensor_2) + output_bitwise_or = my_tensor_1 | my_tensor_2 + + assert torch.allclose(output_logical_or, output_bitwise_or) diff --git a/source/isaaclab/test/devices/check_keyboard.py b/source/isaaclab/test/devices/check_keyboard.py index 9fdcf20f2510..c1a8b07bef82 100644 --- a/source/isaaclab/test/devices/check_keyboard.py +++ b/source/isaaclab/test/devices/check_keyboard.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -23,9 +23,8 @@ import ctypes -from isaacsim.core.api.simulation_context import SimulationContext - -from isaaclab.devices import Se3Keyboard +from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg +from isaaclab.sim import SimulationCfg, SimulationContext def print_cb(): @@ -41,10 +40,10 @@ def quit_cb(): def main(): # Load kit helper - sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01) + sim = SimulationContext(SimulationCfg(dt=0.01)) # Create teleoperation interface - teleop_interface = Se3Keyboard(pos_sensitivity=0.1, rot_sensitivity=0.1) + teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.1, rot_sensitivity=0.1)) # Add teleoperation callbacks # available key buttons: https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.KeyboardInput teleop_interface.add_callback("L", print_cb) diff --git a/source/isaaclab/test/devices/test_device_constructors.py b/source/isaaclab/test/devices/test_device_constructors.py new file mode 100644 index 000000000000..fb1bded4d61a --- /dev/null +++ b/source/isaaclab/test/devices/test_device_constructors.py @@ -0,0 +1,613 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import importlib +import json +from typing import cast + +import pytest +import torch + +# Import device classes to test +from isaaclab.devices import ( + DeviceCfg, + HaplyDevice, + HaplyDeviceCfg, + OpenXRDevice, + OpenXRDeviceCfg, + Se2Gamepad, + Se2GamepadCfg, + Se2Keyboard, + Se2KeyboardCfg, + Se2SpaceMouse, + Se2SpaceMouseCfg, + Se3Gamepad, + Se3GamepadCfg, + Se3Keyboard, + Se3KeyboardCfg, + Se3SpaceMouse, + Se3SpaceMouseCfg, +) +from isaaclab.devices.openxr import XrCfg +from isaaclab.devices.openxr.retargeters import GripperRetargeterCfg, Se3AbsRetargeterCfg + +# Import teleop device factory for testing +from isaaclab.devices.teleop_device_factory import create_teleop_device + + +@pytest.fixture +def mock_environment(mocker): + """Set up common mock objects for tests.""" + # Create mock objects that will be used across tests + carb_mock = mocker.MagicMock() + omni_mock = mocker.MagicMock() + appwindow_mock = mocker.MagicMock() + keyboard_mock = mocker.MagicMock() + gamepad_mock = mocker.MagicMock() + input_mock = mocker.MagicMock() + settings_mock = mocker.MagicMock() + hid_mock = mocker.MagicMock() + device_mock = mocker.MagicMock() + + # Set up the mocks to return appropriate objects + omni_mock.appwindow.get_default_app_window.return_value = appwindow_mock + appwindow_mock.get_keyboard.return_value = keyboard_mock + appwindow_mock.get_gamepad.return_value = gamepad_mock + carb_mock.input.acquire_input_interface.return_value = input_mock + carb_mock.settings.get_settings.return_value = settings_mock + + # Mock keyboard event types + carb_mock.input.KeyboardEventType.KEY_PRESS = 1 + carb_mock.input.KeyboardEventType.KEY_RELEASE = 2 + + # Mock carb events used by OpenXRDevice + events_mock = mocker.MagicMock() + events_mock.type_from_string.return_value = 0 + carb_mock.events = events_mock + + # Mock the SpaceMouse + hid_mock.enumerate.return_value = [{"product_string": "SpaceMouse Compact", "vendor_id": 123, "product_id": 456}] + hid_mock.device.return_value = device_mock + + # Mock OpenXR + # xr_core_mock = mocker.MagicMock() + message_bus_mock = mocker.MagicMock() + singleton_mock = mocker.MagicMock() + omni_mock.kit.xr.core.XRCore.get_singleton.return_value = singleton_mock + singleton_mock.get_message_bus.return_value = message_bus_mock + omni_mock.kit.xr.core.XRPoseValidityFlags.POSITION_VALID = 1 + omni_mock.kit.xr.core.XRPoseValidityFlags.ORIENTATION_VALID = 2 + + # Mock Haply WebSocket + websockets_mock = mocker.MagicMock() + websocket_mock = mocker.MagicMock() + websockets_mock.connect.return_value.__aenter__.return_value = websocket_mock + + return { + "carb": carb_mock, + "omni": omni_mock, + "appwindow": appwindow_mock, + "keyboard": keyboard_mock, + "gamepad": gamepad_mock, + "input": input_mock, + "settings": settings_mock, + "hid": hid_mock, + "device": device_mock, + "websockets": websockets_mock, + "websocket": websocket_mock, + } + + +""" +Test keyboard devices. +""" + + +def test_se2keyboard_constructors(mock_environment, mocker): + """Test constructor for Se2Keyboard.""" + # Test config-based constructor + config = Se2KeyboardCfg( + v_x_sensitivity=0.9, + v_y_sensitivity=0.5, + omega_z_sensitivity=1.2, + ) + device_mod = importlib.import_module("isaaclab.devices.keyboard.se2_keyboard") + mocker.patch.dict("sys.modules", {"carb": mock_environment["carb"], "omni": mock_environment["omni"]}) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "omni", mock_environment["omni"]) + + keyboard = Se2Keyboard(config) + + # Verify configuration was applied correctly + assert keyboard.v_x_sensitivity == 0.9 + assert keyboard.v_y_sensitivity == 0.5 + assert keyboard.omega_z_sensitivity == 1.2 + + # Test advance() returns expected type + result = keyboard.advance() + assert isinstance(result, torch.Tensor) + assert result.shape == (3,) # (v_x, v_y, omega_z) + + +def test_se3keyboard_constructors(mock_environment, mocker): + """Test constructor for Se3Keyboard.""" + # Test config-based constructor + config = Se3KeyboardCfg( + pos_sensitivity=0.5, + rot_sensitivity=0.9, + ) + device_mod = importlib.import_module("isaaclab.devices.keyboard.se3_keyboard") + mocker.patch.dict("sys.modules", {"carb": mock_environment["carb"], "omni": mock_environment["omni"]}) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "omni", mock_environment["omni"]) + + keyboard = Se3Keyboard(config) + + # Verify configuration was applied correctly + assert keyboard.pos_sensitivity == 0.5 + assert keyboard.rot_sensitivity == 0.9 + + # Test advance() returns expected type + result = keyboard.advance() + assert isinstance(result, torch.Tensor) + assert result.shape == (7,) # (pos_x, pos_y, pos_z, rot_x, rot_y, rot_z, gripper) + + +""" +Test gamepad devices. +""" + + +def test_se2gamepad_constructors(mock_environment, mocker): + """Test constructor for Se2Gamepad.""" + # Test config-based constructor + config = Se2GamepadCfg( + v_x_sensitivity=1.1, + v_y_sensitivity=0.6, + omega_z_sensitivity=1.2, + dead_zone=0.02, + ) + device_mod = importlib.import_module("isaaclab.devices.gamepad.se2_gamepad") + mocker.patch.dict("sys.modules", {"carb": mock_environment["carb"], "omni": mock_environment["omni"]}) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "omni", mock_environment["omni"]) + + gamepad = Se2Gamepad(config) + + # Verify configuration was applied correctly + assert gamepad.v_x_sensitivity == 1.1 + assert gamepad.v_y_sensitivity == 0.6 + assert gamepad.omega_z_sensitivity == 1.2 + assert gamepad.dead_zone == 0.02 + + # Test advance() returns expected type + result = gamepad.advance() + assert isinstance(result, torch.Tensor) + assert result.shape == (3,) # (v_x, v_y, omega_z) + + +def test_se3gamepad_constructors(mock_environment, mocker): + """Test constructor for Se3Gamepad.""" + # Test config-based constructor + config = Se3GamepadCfg( + pos_sensitivity=1.1, + rot_sensitivity=1.7, + dead_zone=0.02, + ) + device_mod = importlib.import_module("isaaclab.devices.gamepad.se3_gamepad") + mocker.patch.dict("sys.modules", {"carb": mock_environment["carb"], "omni": mock_environment["omni"]}) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "omni", mock_environment["omni"]) + + gamepad = Se3Gamepad(config) + + # Verify configuration was applied correctly + assert gamepad.pos_sensitivity == 1.1 + assert gamepad.rot_sensitivity == 1.7 + assert gamepad.dead_zone == 0.02 + + # Test advance() returns expected type + result = gamepad.advance() + assert isinstance(result, torch.Tensor) + assert result.shape == (7,) # (pos_x, pos_y, pos_z, rot_x, rot_y, rot_z, gripper) + + +""" +Test spacemouse devices. +""" + + +def test_se2spacemouse_constructors(mock_environment, mocker): + """Test constructor for Se2SpaceMouse.""" + # Test config-based constructor + config = Se2SpaceMouseCfg( + v_x_sensitivity=0.9, + v_y_sensitivity=0.5, + omega_z_sensitivity=1.2, + ) + device_mod = importlib.import_module("isaaclab.devices.spacemouse.se2_spacemouse") + mocker.patch.dict("sys.modules", {"hid": mock_environment["hid"]}) + mocker.patch.object(device_mod, "hid", mock_environment["hid"]) + + spacemouse = Se2SpaceMouse(config) + + # Verify configuration was applied correctly + assert spacemouse.v_x_sensitivity == 0.9 + assert spacemouse.v_y_sensitivity == 0.5 + assert spacemouse.omega_z_sensitivity == 1.2 + + # Test advance() returns expected type + mock_environment["device"].read.return_value = [1, 0, 0, 0, 0] + result = spacemouse.advance() + assert isinstance(result, torch.Tensor) + assert result.shape == (3,) # (v_x, v_y, omega_z) + + +def test_se3spacemouse_constructors(mock_environment, mocker): + """Test constructor for Se3SpaceMouse.""" + # Test config-based constructor + config = Se3SpaceMouseCfg( + pos_sensitivity=0.5, + rot_sensitivity=0.9, + ) + device_mod = importlib.import_module("isaaclab.devices.spacemouse.se3_spacemouse") + mocker.patch.dict("sys.modules", {"hid": mock_environment["hid"]}) + mocker.patch.object(device_mod, "hid", mock_environment["hid"]) + + spacemouse = Se3SpaceMouse(config) + + # Verify configuration was applied correctly + assert spacemouse.pos_sensitivity == 0.5 + assert spacemouse.rot_sensitivity == 0.9 + + # Test advance() returns expected type + mock_environment["device"].read.return_value = [1, 0, 0, 0, 0, 0, 0] + result = spacemouse.advance() + assert isinstance(result, torch.Tensor) + assert result.shape == (7,) # (pos_x, pos_y, pos_z, rot_x, rot_y, rot_z, gripper) + + +""" +Test OpenXR devices. +""" + + +def test_openxr_constructors(mock_environment, mocker): + """Test constructor for OpenXRDevice.""" + # Test config-based constructor with custom XrCfg + xr_cfg = XrCfg( + anchor_pos=(1.0, 2.0, 3.0), + anchor_rot=(0.0, 0.1, 0.2, 0.3), + near_plane=0.2, + ) + config = OpenXRDeviceCfg(xr_cfg=xr_cfg) + + # Create mock retargeters + mock_controller_retargeter = mocker.MagicMock() + mock_head_retargeter = mocker.MagicMock() + retargeters = [mock_controller_retargeter, mock_head_retargeter] + + device_mod = importlib.import_module("isaaclab.devices.openxr.openxr_device") + mocker.patch.dict( + "sys.modules", + { + "carb": mock_environment["carb"], + "omni.kit.xr.core": mock_environment["omni"].kit.xr.core, + "isaacsim.core.prims": mocker.MagicMock(), + }, + ) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) + mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) + mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") + + # Configure the mock to return a string for prim_path + mock_instance = mock_single_xform.return_value + mock_instance.prim_path = "/XRAnchor" + + # Create the device using the factory + device = OpenXRDevice(config) + + # Verify the device was created successfully + assert device._xr_cfg == xr_cfg + + # Test with retargeters + device = OpenXRDevice(cfg=config, retargeters=retargeters) + + # Verify retargeters were correctly assigned as a list + assert device._retargeters == retargeters + + # Test with config and retargeters + device = OpenXRDevice(cfg=config, retargeters=retargeters) + + # Verify both config and retargeters were correctly assigned + assert device._xr_cfg == xr_cfg + assert device._retargeters == retargeters + + # Test reset functionality + device.reset() + + +""" +Test Haply devices. +""" + + +def test_haply_constructors(mock_environment, mocker): + """Test constructor for HaplyDevice.""" + # Test config-based constructor + config = HaplyDeviceCfg( + websocket_uri="ws://localhost:10001", + pos_sensitivity=1.5, + data_rate=250.0, + ) + + # Mock the websockets module and asyncio + device_mod = importlib.import_module("isaaclab.devices.haply.se3_haply") + mocker.patch.dict("sys.modules", {"websockets": mock_environment["websockets"]}) + mocker.patch.object(device_mod, "websockets", mock_environment["websockets"]) + + # Mock asyncio to prevent actual async operations + asyncio_mock = mocker.MagicMock() + mocker.patch.object(device_mod, "asyncio", asyncio_mock) + + # Mock threading to prevent actual thread creation + threading_mock = mocker.MagicMock() + thread_instance = mocker.MagicMock() + threading_mock.Thread.return_value = thread_instance + thread_instance.is_alive.return_value = False + mocker.patch.object(device_mod, "threading", threading_mock) + + # Mock time.time() for connection timeout simulation + time_mock = mocker.MagicMock() + time_mock.time.side_effect = [0.0, 0.1, 0.2, 0.3, 6.0] # Will timeout + mocker.patch.object(device_mod, "time", time_mock) + + # Create sample WebSocket response data + ws_response = { + "inverse3": [ + { + "device_id": "test_inverse3_123", + "state": {"cursor_position": {"x": 0.1, "y": 0.2, "z": 0.3}}, + } + ], + "wireless_verse_grip": [ + { + "device_id": "test_versegrip_456", + "state": { + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}, + "buttons": {"a": False, "b": False, "c": False}, + }, + } + ], + } + + # Configure websocket mock to return JSON data + mock_environment["websocket"].recv = mocker.AsyncMock(return_value=json.dumps(ws_response)) + mock_environment["websocket"].send = mocker.AsyncMock() + + # The constructor will raise RuntimeError due to timeout, which is expected in test + with pytest.raises(RuntimeError, match="Failed to connect both Inverse3 and VerseGrip devices"): + haply = HaplyDevice(config) + + # Now test successful connection by mocking time to not timeout + time_mock.time.side_effect = [0.0, 0.1, 0.2, 0.3, 0.4] # Won't timeout + + # Mock the connection status + mocker.patch.object(device_mod.HaplyDevice, "_start_websocket_thread") + haply = device_mod.HaplyDevice.__new__(device_mod.HaplyDevice) + haply._sim_device = config.sim_device + haply.websocket_uri = config.websocket_uri + haply.pos_sensitivity = config.pos_sensitivity + haply.data_rate = config.data_rate + haply.limit_force = config.limit_force + haply.connected = True + haply.inverse3_device_id = "test_inverse3_123" + haply.verse_grip_device_id = "test_versegrip_456" + haply.data_lock = threading_mock.Lock() + haply.force_lock = threading_mock.Lock() + haply._connected_lock = threading_mock.Lock() + haply._additional_callbacks = {} + haply._prev_buttons = {"a": False, "b": False, "c": False} + haply._websocket_thread = None # Initialize to prevent AttributeError in __del__ + haply.running = True + haply.cached_data = { + "position": torch.tensor([0.1, 0.2, 0.3], dtype=torch.float32).numpy(), + "quaternion": torch.tensor([0.0, 0.0, 0.0, 1.0], dtype=torch.float32).numpy(), + "buttons": {"a": False, "b": False, "c": False}, + "inverse3_connected": True, + "versegrip_connected": True, + } + haply.feedback_force = {"x": 0.0, "y": 0.0, "z": 0.0} + + # Verify configuration was applied correctly + assert haply.websocket_uri == "ws://localhost:10001" + assert haply.pos_sensitivity == 1.5 + assert haply.data_rate == 250.0 + + # Test advance() returns expected type + result = haply.advance() + assert isinstance(result, torch.Tensor) + assert result.shape == (10,) # (pos_x, pos_y, pos_z, qx, qy, qz, qw, btn_a, btn_b, btn_c) + + # Test push_force with tensor (single force vector) + forces_within = torch.tensor([[1.0, 1.5, -0.5]], dtype=torch.float32) + position_zero = torch.tensor([0], dtype=torch.long) + haply.push_force(forces_within, position_zero) + assert haply.feedback_force["x"] == pytest.approx(1.0) + assert haply.feedback_force["y"] == pytest.approx(1.5) + assert haply.feedback_force["z"] == pytest.approx(-0.5) + + # Test push_force with tensor (force limiting, default limit is 2.0 N) + forces_exceed = torch.tensor([[5.0, -10.0, 1.5]], dtype=torch.float32) + haply.push_force(forces_exceed, position_zero) + assert haply.feedback_force["x"] == pytest.approx(2.0) + assert haply.feedback_force["y"] == pytest.approx(-2.0) + assert haply.feedback_force["z"] == pytest.approx(1.5) + + # Test push_force with position tensor (single index) + forces_multi = torch.tensor([[1.0, 2.0, 3.0], [0.5, 0.8, -0.3], [0.1, 0.2, 0.3]], dtype=torch.float32) + position_single = torch.tensor([1], dtype=torch.long) + haply.push_force(forces_multi, position=position_single) + assert haply.feedback_force["x"] == pytest.approx(0.5) + assert haply.feedback_force["y"] == pytest.approx(0.8) + assert haply.feedback_force["z"] == pytest.approx(-0.3) + + # Test push_force with position tensor (multiple indices) + position_multi = torch.tensor([0, 2], dtype=torch.long) + haply.push_force(forces_multi, position=position_multi) + # Should sum forces[0] and forces[2]: [1.0+0.1, 2.0+0.2, 3.0+0.3] = [1.1, 2.2, 3.3] + # But clipped to [-2.0, 2.0]: [1.1, 2.0, 2.0] + assert haply.feedback_force["x"] == pytest.approx(1.1) + assert haply.feedback_force["y"] == pytest.approx(2.0) + assert haply.feedback_force["z"] == pytest.approx(2.0) + + # Test reset functionality + haply.reset() + assert haply.feedback_force == {"x": 0.0, "y": 0.0, "z": 0.0} + + +""" +Test teleop device factory. +""" + + +def test_create_teleop_device_basic(mock_environment, mocker): + """Test creating devices using the teleop device factory.""" + # Create device configuration + keyboard_cfg = Se3KeyboardCfg(pos_sensitivity=0.8, rot_sensitivity=1.2) + + # Create devices configuration dictionary + devices_cfg: dict[str, DeviceCfg] = {"test_keyboard": keyboard_cfg} + + # Mock Se3Keyboard class + device_mod = importlib.import_module("isaaclab.devices.keyboard.se3_keyboard") + mocker.patch.dict("sys.modules", {"carb": mock_environment["carb"], "omni": mock_environment["omni"]}) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "omni", mock_environment["omni"]) + + # Create the device using the factory + device = create_teleop_device("test_keyboard", devices_cfg) + + # Verify the device was created correctly + assert isinstance(device, Se3Keyboard) + assert device.pos_sensitivity == 0.8 + assert device.rot_sensitivity == 1.2 + + +def test_create_teleop_device_with_callbacks(mock_environment, mocker): + """Test creating device with callbacks.""" + # Create device configuration + xr_cfg = XrCfg(anchor_pos=(0.0, 0.0, 0.0), anchor_rot=(1.0, 0.0, 0.0, 0.0), near_plane=0.15) + openxr_cfg = OpenXRDeviceCfg(xr_cfg=xr_cfg) + + # Create devices configuration dictionary + devices_cfg: dict[str, DeviceCfg] = {"test_xr": openxr_cfg} + + # Create mock callbacks + button_a_callback = mocker.MagicMock() + button_b_callback = mocker.MagicMock() + callbacks = {"button_a": button_a_callback, "button_b": button_b_callback} + + # Mock OpenXRDevice class and dependencies + device_mod = importlib.import_module("isaaclab.devices.openxr.openxr_device") + mocker.patch.dict( + "sys.modules", + { + "carb": mock_environment["carb"], + "omni.kit.xr.core": mock_environment["omni"].kit.xr.core, + "isaacsim.core.prims": mocker.MagicMock(), + }, + ) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) + mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) + mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") + + # Configure the mock to return a string for prim_path + mock_instance = mock_single_xform.return_value + mock_instance.prim_path = "/XRAnchor" + + # Create the device using the factory + device = create_teleop_device("test_xr", devices_cfg, callbacks) + + # Verify the device was created correctly + assert isinstance(device, OpenXRDevice) + + # Verify callbacks were registered by the factory + assert set(device._additional_callbacks.keys()) == {"button_a", "button_b"} + + +def test_create_teleop_device_with_retargeters(mock_environment, mocker): + """Test creating device with retargeters.""" + # Create retargeter configurations + retargeter_cfg1 = Se3AbsRetargeterCfg() + retargeter_cfg2 = GripperRetargeterCfg() + + # Create device configuration with retargeters + xr_cfg = XrCfg() + device_cfg = OpenXRDeviceCfg(xr_cfg=xr_cfg, retargeters=[retargeter_cfg1, retargeter_cfg2]) + + # Create devices configuration dictionary + devices_cfg: dict[str, DeviceCfg] = {"test_xr": device_cfg} + + # Mock OpenXRDevice class and dependencies + device_mod = importlib.import_module("isaaclab.devices.openxr.openxr_device") + mocker.patch.dict( + "sys.modules", + { + "carb": mock_environment["carb"], + "omni.kit.xr.core": mock_environment["omni"].kit.xr.core, + "isaacsim.core.prims": mocker.MagicMock(), + }, + ) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) + mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) + mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) + mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") + + # Configure the mock to return a string for prim_path + mock_instance = mock_single_xform.return_value + mock_instance.prim_path = "/XRAnchor" + + # Create the device using the factory + device = create_teleop_device("test_xr", devices_cfg) + + # Verify retargeters were created + assert len(device._retargeters) == 2 + + +def test_create_teleop_device_device_not_found(): + """Test error when device name is not found in configuration.""" + # Create devices configuration dictionary + devices_cfg: dict[str, DeviceCfg] = {"keyboard": Se3KeyboardCfg()} + + # Try to create a non-existent device + with pytest.raises(ValueError, match="Device 'gamepad' not found"): + create_teleop_device("gamepad", devices_cfg) + + +def test_create_teleop_device_unsupported_config(): + """Test error when device configuration type is not supported.""" + + # Create a custom unsupported configuration class + class UnsupportedCfg: + pass + + # Create devices configuration dictionary with unsupported config + devices_cfg: dict[str, DeviceCfg] = cast(dict[str, DeviceCfg], {"unsupported": UnsupportedCfg()}) + + # Try to create a device with unsupported configuration + with pytest.raises(ValueError, match="does not declare class_type"): + create_teleop_device("unsupported", devices_cfg) diff --git a/source/isaaclab/test/devices/test_oxr_device.py b/source/isaaclab/test/devices/test_oxr_device.py new file mode 100644 index 000000000000..6402d8e0c187 --- /dev/null +++ b/source/isaaclab/test/devices/test_oxr_device.py @@ -0,0 +1,276 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Ignore private usage of variables warning. +# pyright: reportPrivateUsage=none + +from __future__ import annotations + +from isaaclab.app import AppLauncher + +# Can set this to False to see the GUI for debugging. +HEADLESS = True + +# Launch omniverse app. +app_launcher = AppLauncher(headless=HEADLESS) +simulation_app = app_launcher.app + +import importlib + +import numpy as np +import pytest +import torch + +import carb +import omni.usd +from isaacsim.core.prims import XFormPrim + +from isaaclab.devices import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.utils import configclass + + +class NoOpRetargeter(RetargeterBase): + """A no-op retargeter that requests hand and head tracking but returns empty tensor.""" + + def __init__(self, cfg: RetargeterCfg): + super().__init__(cfg) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + """Request hand and head tracking to trigger data collection.""" + return [ + RetargeterBase.Requirement.HAND_TRACKING, + RetargeterBase.Requirement.HEAD_TRACKING, + ] + + def retarget(self, data): + """Return empty tensor.""" + return torch.tensor([], device=self._sim_device) + + +@configclass +class EmptyManagerCfg: + """Empty manager.""" + + pass + + +@configclass +class EmptySceneCfg(InteractiveSceneCfg): + """Configuration for an empty scene.""" + + pass + + +@configclass +class EmptyEnvCfg(ManagerBasedEnvCfg): + """Configuration for the empty test environment.""" + + scene: EmptySceneCfg = EmptySceneCfg(num_envs=1, env_spacing=1.0) + actions: EmptyManagerCfg = EmptyManagerCfg() + observations: EmptyManagerCfg = EmptyManagerCfg() + + def __post_init__(self): + """Post initialization.""" + self.decimation = 5 + self.episode_length_s = 30.0 + self.sim.dt = 0.01 # 100Hz + self.sim.render_interval = 2 + + +@pytest.fixture +def mock_xrcore(mocker): + """Set up a mock for XRCore and related classes.""" + # Create mock for XRCore and XRPoseValidityFlags + xr_core_mock = mocker.MagicMock() + xr_pose_validity_flags_mock = mocker.MagicMock() + + # Set up the validity flags + xr_pose_validity_flags_mock.POSITION_VALID = 1 + xr_pose_validity_flags_mock.ORIENTATION_VALID = 2 + + # Setup the singleton pattern used by XRCore + singleton_mock = mocker.MagicMock() + xr_core_mock.get_singleton.return_value = singleton_mock + + # Setup message bus for teleop commands + message_bus_mock = mocker.MagicMock() + singleton_mock.get_message_bus.return_value = message_bus_mock + message_bus_mock.create_subscription_to_pop_by_type.return_value = mocker.MagicMock() + + # Setup input devices (left hand, right hand, head) + left_hand_mock = mocker.MagicMock() + right_hand_mock = mocker.MagicMock() + head_mock = mocker.MagicMock() + + def get_input_device_mock(device_path): + device_map = { + "/user/hand/left": left_hand_mock, + "/user/hand/right": right_hand_mock, + "/user/head": head_mock, + } + return device_map.get(device_path) + + singleton_mock.get_input_device.side_effect = get_input_device_mock + + # Setup the joint poses for hand tracking + joint_pose_mock = mocker.MagicMock() + joint_pose_mock.validity_flags = ( + xr_pose_validity_flags_mock.POSITION_VALID | xr_pose_validity_flags_mock.ORIENTATION_VALID + ) + + pose_matrix_mock = mocker.MagicMock() + pose_matrix_mock.ExtractTranslation.return_value = [0.1, 0.2, 0.3] + + rotation_quat_mock = mocker.MagicMock() + rotation_quat_mock.GetImaginary.return_value = [0.1, 0.2, 0.3] + rotation_quat_mock.GetReal.return_value = 0.9 + + pose_matrix_mock.ExtractRotationQuat.return_value = rotation_quat_mock + joint_pose_mock.pose_matrix = pose_matrix_mock + + joint_poses = {"palm": joint_pose_mock, "wrist": joint_pose_mock} + left_hand_mock.get_all_virtual_world_poses.return_value = joint_poses + right_hand_mock.get_all_virtual_world_poses.return_value = joint_poses + + head_mock.get_virtual_world_pose.return_value = pose_matrix_mock + + # Patch the modules + device_mod = importlib.import_module("isaaclab.devices.openxr.openxr_device") + mocker.patch.object(device_mod, "XRCore", xr_core_mock) + mocker.patch.object(device_mod, "XRPoseValidityFlags", xr_pose_validity_flags_mock) + + return { + "XRCore": xr_core_mock, + "XRPoseValidityFlags": xr_pose_validity_flags_mock, + "singleton": singleton_mock, + "message_bus": message_bus_mock, + "left_hand": left_hand_mock, + "right_hand": right_hand_mock, + "head": head_mock, + } + + +@pytest.fixture +def empty_env(): + """Fixture to create and cleanup an empty environment.""" + # Create a new stage + omni.usd.get_context().new_stage() + # Create environment with config + env_cfg = EmptyEnvCfg() + env = ManagerBasedEnv(cfg=env_cfg) + + yield env, env_cfg + + # Cleanup + env.close() + + +@pytest.mark.isaacsim_ci +def test_xr_anchor(empty_env, mock_xrcore): + """Test XR anchor creation and configuration.""" + env, env_cfg = empty_env + env_cfg.xr = XrCfg(anchor_pos=(1, 2, 3), anchor_rot=(0, 1, 0, 0)) + + device = OpenXRDevice(OpenXRDeviceCfg(xr_cfg=env_cfg.xr)) + + # Check that the xr anchor prim is created with the correct pose + xr_anchor_prim = XFormPrim("/World/XRAnchor") + assert xr_anchor_prim.is_valid() + + position, orientation = xr_anchor_prim.get_world_poses() + np.testing.assert_almost_equal(position.tolist(), [[1, 2, 3]]) + np.testing.assert_almost_equal(orientation.tolist(), [[0, 1, 0, 0]]) + + # Check that xr anchor mode and custom anchor are set correctly + assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor" + assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor" + + device.reset() + + +@pytest.mark.isaacsim_ci +def test_xr_anchor_default(empty_env, mock_xrcore): + """Test XR anchor creation with default configuration.""" + env, _ = empty_env + # Create a proper config object with default values + device = OpenXRDevice(OpenXRDeviceCfg()) + + # Check that the xr anchor prim is created with the correct default pose + xr_anchor_prim = XFormPrim("/World/XRAnchor") + assert xr_anchor_prim.is_valid() + + position, orientation = xr_anchor_prim.get_world_poses() + np.testing.assert_almost_equal(position.tolist(), [[0, 0, 0]]) + np.testing.assert_almost_equal(orientation.tolist(), [[1, 0, 0, 0]]) + + # Check that xr anchor mode and custom anchor are set correctly + assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor" + assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor" + + device.reset() + + +@pytest.mark.isaacsim_ci +def test_xr_anchor_multiple_devices(empty_env, mock_xrcore): + """Test XR anchor behavior with multiple devices.""" + env, _ = empty_env + # Create proper config objects with default values + device_1 = OpenXRDevice(OpenXRDeviceCfg()) + device_2 = OpenXRDevice(OpenXRDeviceCfg()) + + # Check that the xr anchor prim is created with the correct default pose + xr_anchor_prim = XFormPrim("/World/XRAnchor") + assert xr_anchor_prim.is_valid() + + position, orientation = xr_anchor_prim.get_world_poses() + np.testing.assert_almost_equal(position.tolist(), [[0, 0, 0]]) + np.testing.assert_almost_equal(orientation.tolist(), [[1, 0, 0, 0]]) + + # Check that xr anchor mode and custom anchor are set correctly + assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor" + assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor" + + device_1.reset() + device_2.reset() + + +@pytest.mark.isaacsim_ci +def test_get_raw_data(empty_env, mock_xrcore): + """Test the _get_raw_data method returns correctly formatted tracking data.""" + env, _ = empty_env + # Create a proper config object with default values and a no-op retargeter to trigger data collection + retargeter = NoOpRetargeter(RetargeterCfg()) + device = OpenXRDevice(OpenXRDeviceCfg(), retargeters=[retargeter]) + + # Get raw tracking data + raw_data = device._get_raw_data() + + # Check that the data structure is as expected + from isaaclab.devices.device_base import DeviceBase + + assert DeviceBase.TrackingTarget.HAND_LEFT in raw_data + assert DeviceBase.TrackingTarget.HAND_RIGHT in raw_data + assert DeviceBase.TrackingTarget.HEAD in raw_data + + # Check left hand joints + left_hand = raw_data[DeviceBase.TrackingTarget.HAND_LEFT] + assert "palm" in left_hand + assert "wrist" in left_hand + + # Check that joint pose format is correct + palm_pose = left_hand["palm"] + assert len(palm_pose) == 7 # [x, y, z, qw, qx, qy, qz] + np.testing.assert_almost_equal(palm_pose[:3], [0.1, 0.2, 0.3]) # Position + np.testing.assert_almost_equal(palm_pose[3:], [0.9, 0.1, 0.2, 0.3]) # Orientation + + # Check head pose + head_pose = raw_data[DeviceBase.TrackingTarget.HEAD] + assert len(head_pose) == 7 + np.testing.assert_almost_equal(head_pose[:3], [0.1, 0.2, 0.3]) # Position + np.testing.assert_almost_equal(head_pose[3:], [0.9, 0.1, 0.2, 0.3]) # Orientation diff --git a/source/isaaclab/test/devices/test_retargeters.py b/source/isaaclab/test/devices/test_retargeters.py new file mode 100644 index 000000000000..c080c4a43d9c --- /dev/null +++ b/source/isaaclab/test/devices/test_retargeters.py @@ -0,0 +1,370 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Unit tests for retargeters. +""" + +from isaaclab.app import AppLauncher + +# Can set this to False to see the GUI for debugging. +HEADLESS = True + +# Launch omniverse app. +app_launcher = AppLauncher(headless=HEADLESS) +simulation_app = app_launcher.app + +import sys +import unittest +from unittest.mock import MagicMock, patch + +import numpy as np +import torch + +# Mock dependencies that might require a running simulation or specific hardware +sys.modules["isaaclab.markers"] = MagicMock() +sys.modules["isaaclab.markers.config"] = MagicMock() +sys.modules["isaaclab.sim"] = MagicMock() +sys.modules["isaaclab.sim.SimulationContext"] = MagicMock() + +# Mock SimulationContext instance +mock_sim_context = MagicMock() +mock_sim_context.get_rendering_dt.return_value = 0.016 # 60Hz +sys.modules["isaaclab.sim"].SimulationContext.instance.return_value = mock_sim_context + + +# Import after mocking +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_lower_body_standing import ( + G1LowerBodyStandingRetargeter, + G1LowerBodyStandingRetargeterCfg, +) +from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_motion_controller_locomotion import ( + G1LowerBodyStandingMotionControllerRetargeter, + G1LowerBodyStandingMotionControllerRetargeterCfg, +) +from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeter, GripperRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_abs_retargeter import Se3AbsRetargeter, Se3AbsRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeter, Se3RelRetargeterCfg + +# Mock dex retargeting utils +with patch.dict( + sys.modules, + { + "isaaclab.devices.openxr.retargeters.humanoid.unitree.inspire.g1_dex_retargeting_utils": MagicMock(), + "isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1_t2_dex_retargeting_utils": MagicMock(), + "isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_dex_retargeting_utils": MagicMock(), + }, +): + from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import ( + GR1T2Retargeter, + GR1T2RetargeterCfg, + ) + from isaaclab.devices.openxr.retargeters.humanoid.unitree.inspire.g1_upper_body_retargeter import ( + UnitreeG1Retargeter, + UnitreeG1RetargeterCfg, + ) + from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_motion_ctrl_gripper import ( + G1TriHandUpperBodyMotionControllerGripperRetargeter, + G1TriHandUpperBodyMotionControllerGripperRetargeterCfg, + ) + from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_motion_ctrl_retargeter import ( + G1TriHandUpperBodyMotionControllerRetargeter, + G1TriHandUpperBodyMotionControllerRetargeterCfg, + ) + from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeter, + G1TriHandUpperBodyRetargeterCfg, + ) + + +class TestSe3AbsRetargeter(unittest.TestCase): + def setUp(self): + self.cfg = Se3AbsRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, enable_visualization=False, sim_device="cpu" + ) + self.retargeter = Se3AbsRetargeter(self.cfg) + + def test_retarget_defaults(self): + # Mock input data + wrist_pose = np.array([0.1, 0.2, 0.3, 1.0, 0.0, 0.0, 0.0]) + thumb_tip_pose = np.array([0.15, 0.25, 0.35, 1.0, 0.0, 0.0, 0.0]) + index_tip_pose = np.array([0.15, 0.20, 0.35, 1.0, 0.0, 0.0, 0.0]) + + data = { + DeviceBase.TrackingTarget.HAND_RIGHT: { + "wrist": wrist_pose, + "thumb_tip": thumb_tip_pose, + "index_tip": index_tip_pose, + } + } + + result = self.retargeter.retarget(data) + + self.assertIsInstance(result, torch.Tensor) + self.assertEqual(result.shape, (7,)) + np.testing.assert_allclose(result[:3].numpy(), wrist_pose[:3], rtol=1e-5) + self.assertAlmostEqual(torch.norm(result[3:]).item(), 1.0, places=4) + + def test_pinch_position(self): + self.cfg.use_wrist_position = False + retargeter = Se3AbsRetargeter(self.cfg) + + wrist_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + thumb_tip_pose = np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + index_tip_pose = np.array([3.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + + data = { + DeviceBase.TrackingTarget.HAND_RIGHT: { + "wrist": wrist_pose, + "thumb_tip": thumb_tip_pose, + "index_tip": index_tip_pose, + } + } + + result = retargeter.retarget(data) + expected_pos = np.array([2.0, 0.0, 0.0]) + np.testing.assert_allclose(result[:3].numpy(), expected_pos, rtol=1e-5) + + +class TestSe3RelRetargeter(unittest.TestCase): + def setUp(self): + self.cfg = Se3RelRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, + enable_visualization=False, + sim_device="cpu", + delta_pos_scale_factor=1.0, + delta_rot_scale_factor=1.0, + alpha_pos=1.0, + alpha_rot=1.0, + ) + self.retargeter = Se3RelRetargeter(self.cfg) + + def test_retarget_movement(self): + wrist_pose_1 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + thumb_tip_pose_1 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + index_tip_pose_1 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + + data_1 = { + DeviceBase.TrackingTarget.HAND_LEFT: { + "wrist": wrist_pose_1, + "thumb_tip": thumb_tip_pose_1, + "index_tip": index_tip_pose_1, + } + } + + _ = self.retargeter.retarget(data_1) + + wrist_pose_2 = np.array([0.1, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + thumb_tip_pose_2 = np.array([0.1, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + index_tip_pose_2 = np.array([0.1, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + + data_2 = { + DeviceBase.TrackingTarget.HAND_LEFT: { + "wrist": wrist_pose_2, + "thumb_tip": thumb_tip_pose_2, + "index_tip": index_tip_pose_2, + } + } + + result = self.retargeter.retarget(data_2) + self.assertEqual(result.shape, (6,)) + np.testing.assert_allclose(result[:3].numpy(), [0.1, 0.0, 0.0], rtol=1e-4) + + +class TestGripperRetargeter(unittest.TestCase): + def setUp(self): + self.cfg = GripperRetargeterCfg(bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device="cpu") + self.retargeter = GripperRetargeter(self.cfg) + + def test_gripper_logic(self): + data_open = { + DeviceBase.TrackingTarget.HAND_RIGHT: { + "thumb_tip": np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]), + "index_tip": np.array([0.1, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]), + } + } + result = self.retargeter.retarget(data_open) + self.assertEqual(result.item(), 1.0) + + data_close = { + DeviceBase.TrackingTarget.HAND_RIGHT: { + "thumb_tip": np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]), + "index_tip": np.array([0.02, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]), + } + } + result = self.retargeter.retarget(data_close) + self.assertEqual(result.item(), -1.0) + + +class TestG1LowerBodyStandingRetargeter(unittest.TestCase): + def test_retarget(self): + cfg = G1LowerBodyStandingRetargeterCfg(hip_height=0.8, sim_device="cpu") + retargeter = G1LowerBodyStandingRetargeter(cfg) + result = retargeter.retarget({}) + self.assertTrue(torch.equal(result, torch.tensor([0.0, 0.0, 0.0, 0.8]))) + + +class TestUnitreeG1Retargeter(unittest.TestCase): + @patch( + "isaaclab.devices.openxr.retargeters.humanoid.unitree.inspire.g1_upper_body_retargeter.UnitreeG1DexRetargeting" + ) + def test_retarget(self, mock_dex_retargeting_cls): + mock_dex_retargeting = mock_dex_retargeting_cls.return_value + mock_dex_retargeting.get_joint_names.return_value = ["joint1", "joint2"] + mock_dex_retargeting.get_left_joint_names.return_value = ["joint1"] + mock_dex_retargeting.get_right_joint_names.return_value = ["joint2"] + mock_dex_retargeting.compute_left.return_value = np.array([0.1]) + mock_dex_retargeting.compute_right.return_value = np.array([0.2]) + + cfg = UnitreeG1RetargeterCfg( + enable_visualization=False, sim_device="cpu", hand_joint_names=["joint1", "joint2"] + ) + retargeter = UnitreeG1Retargeter(cfg) + + wrist_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + data = { + DeviceBase.TrackingTarget.HAND_LEFT: {"wrist": wrist_pose}, + DeviceBase.TrackingTarget.HAND_RIGHT: {"wrist": wrist_pose}, + } + + result = retargeter.retarget(data) + self.assertEqual(result.shape, (16,)) + + +class TestGR1T2Retargeter(unittest.TestCase): + @patch("isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter.GR1TR2DexRetargeting") + def test_retarget(self, mock_dex_retargeting_cls): + mock_dex_retargeting = mock_dex_retargeting_cls.return_value + mock_dex_retargeting.get_joint_names.return_value = ["joint1", "joint2"] + mock_dex_retargeting.get_left_joint_names.return_value = ["joint1"] + mock_dex_retargeting.get_right_joint_names.return_value = ["joint2"] + mock_dex_retargeting.compute_left.return_value = np.array([0.1]) + mock_dex_retargeting.compute_right.return_value = np.array([0.2]) + + cfg = GR1T2RetargeterCfg(enable_visualization=False, sim_device="cpu", hand_joint_names=["joint1", "joint2"]) + retargeter = GR1T2Retargeter(cfg) + + wrist_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + data = { + DeviceBase.TrackingTarget.HAND_LEFT: {"wrist": wrist_pose}, + DeviceBase.TrackingTarget.HAND_RIGHT: {"wrist": wrist_pose}, + } + + result = retargeter.retarget(data) + self.assertEqual(result.shape, (16,)) + + +class TestG1LowerBodyStandingMotionControllerRetargeter(unittest.TestCase): + def test_retarget(self): + cfg = G1LowerBodyStandingMotionControllerRetargeterCfg( + hip_height=0.8, movement_scale=1.0, rotation_scale=1.0, sim_device="cpu" + ) + retargeter = G1LowerBodyStandingMotionControllerRetargeter(cfg) + + # Mock input data + # Inputs array structure: [thumbstick_x, thumbstick_y, trigger, squeeze, button_0, button_1, padding] + left_inputs = np.zeros(7) + left_inputs[0] = 0.5 # thumbstick x + left_inputs[1] = 0.5 # thumbstick y + + right_inputs = np.zeros(7) + right_inputs[0] = -0.5 # thumbstick x + right_inputs[1] = -0.5 # thumbstick y + + data = { + DeviceBase.TrackingTarget.CONTROLLER_LEFT: [np.zeros(7), left_inputs], + DeviceBase.TrackingTarget.CONTROLLER_RIGHT: [np.zeros(7), right_inputs], + } + + result = retargeter.retarget(data) + # Output: [-left_thumbstick_y, -left_thumbstick_x, -right_thumbstick_x, hip_height] + # hip_height modified by right_thumbstick_y + + self.assertEqual(result.shape, (4,)) + self.assertAlmostEqual(result[0].item(), -0.5) # -left y + self.assertAlmostEqual(result[1].item(), -0.5) # -left x + self.assertAlmostEqual(result[2].item(), 0.5) # -right x + # Check hip height modification logic if needed, but basic execution is key here + + +class TestG1TriHandUpperBodyMotionControllerGripperRetargeter(unittest.TestCase): + def test_retarget(self): + cfg = G1TriHandUpperBodyMotionControllerGripperRetargeterCfg( + threshold_high=0.6, threshold_low=0.4, sim_device="cpu" + ) + retargeter = G1TriHandUpperBodyMotionControllerGripperRetargeter(cfg) + + pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + inputs_trigger_high = np.zeros(7) + inputs_trigger_high[2] = 0.8 # Trigger + + inputs_trigger_low = np.zeros(7) + inputs_trigger_low[2] = 0.2 # Trigger + + data = { + DeviceBase.TrackingTarget.CONTROLLER_LEFT: [pose, inputs_trigger_high], + DeviceBase.TrackingTarget.CONTROLLER_RIGHT: [pose, inputs_trigger_low], + } + + result = retargeter.retarget(data) + # Output: [left_state, right_state, left_wrist(7), right_wrist(7)] + self.assertEqual(result.shape, (16,)) + self.assertEqual(result[0].item(), 1.0) # Left closed + self.assertEqual(result[1].item(), 0.0) # Right open + + +class TestG1TriHandUpperBodyMotionControllerRetargeter(unittest.TestCase): + def test_retarget(self): + cfg = G1TriHandUpperBodyMotionControllerRetargeterCfg( + hand_joint_names=["dummy"] * 14, # Not really used in logic, just passed to config + sim_device="cpu", + enable_visualization=False, + ) + retargeter = G1TriHandUpperBodyMotionControllerRetargeter(cfg) + + pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + inputs = np.zeros(7) + + data = { + DeviceBase.TrackingTarget.CONTROLLER_LEFT: [pose, inputs], + DeviceBase.TrackingTarget.CONTROLLER_RIGHT: [pose, inputs], + } + + result = retargeter.retarget(data) + # Output: [left_wrist(7), right_wrist(7), hand_joints(14)] + self.assertEqual(result.shape, (28,)) + + +class TestG1TriHandUpperBodyRetargeter(unittest.TestCase): + @patch( + "isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter.G1TriHandDexRetargeting" + ) + def test_retarget(self, mock_dex_retargeting_cls): + mock_dex_retargeting = mock_dex_retargeting_cls.return_value + mock_dex_retargeting.get_joint_names.return_value = ["joint1", "joint2"] + mock_dex_retargeting.get_left_joint_names.return_value = ["joint1"] + mock_dex_retargeting.get_right_joint_names.return_value = ["joint2"] + mock_dex_retargeting.compute_left.return_value = np.array([0.1]) + mock_dex_retargeting.compute_right.return_value = np.array([0.2]) + + cfg = G1TriHandUpperBodyRetargeterCfg( + enable_visualization=False, sim_device="cpu", hand_joint_names=["joint1", "joint2"] + ) + retargeter = G1TriHandUpperBodyRetargeter(cfg) + + wrist_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + data = { + DeviceBase.TrackingTarget.HAND_LEFT: {"wrist": wrist_pose}, + DeviceBase.TrackingTarget.HAND_RIGHT: {"wrist": wrist_pose}, + } + + result = retargeter.retarget(data) + # Output: [left_wrist(7), right_wrist(7), joints(2)] + self.assertEqual(result.shape, (16,)) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py index 83d1729f24c3..c6169c94d197 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py +++ b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -92,7 +92,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], diff --git a/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py b/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py index 31131d15f7fc..fb7622ae67cd 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py +++ b/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/envs/test_action_state_recorder_term.py b/source/isaaclab/test/envs/test_action_state_recorder_term.py index 2bdcd421fdc5..16ae866dfce2 100644 --- a/source/isaaclab/test/envs/test_action_state_recorder_term.py +++ b/source/isaaclab/test/envs/test_action_state_recorder_term.py @@ -1,26 +1,25 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import gymnasium as gym import shutil import tempfile -import torch -import unittest import uuid +import gymnasium as gym +import pytest +import torch + import carb import omni.usd @@ -30,94 +29,88 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestActionStateRecorderManagerCfg(unittest.TestCase): - """Test cases for ActionStateRecorderManagerCfg recorder terms.""" - - @classmethod - def setUpClass(cls): - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - def setUp(self): - # create a temporary directory to store the test datasets - self.temp_dir = tempfile.mkdtemp() - - def tearDown(self): - # delete the temporary directory after the test - shutil.rmtree(self.temp_dir) - - def test_action_state_reocrder_terms(self): - """Check action state recorder terms.""" - for task_name in ["Isaac-Lift-Cube-Franka-v0"]: - for device in ["cuda:0", "cpu"]: - for num_envs in [1, 2]: - with self.subTest(task_name=task_name, device=device): - omni.usd.get_context().new_stage() - - dummy_dataset_filename = f"{uuid.uuid4()}.hdf5" - - # parse configuration - env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) - # set recorder configurations for this test - env_cfg.recorders: ActionStateRecorderManagerCfg = ActionStateRecorderManagerCfg() - env_cfg.recorders.dataset_export_dir_path = self.temp_dir - env_cfg.recorders.dataset_filename = dummy_dataset_filename - - # create environment - env = gym.make(task_name, cfg=env_cfg) - - # reset all environment instances to trigger post-reset recorder callbacks - env.reset() - self.check_initial_state_recorder_term(env) - - # reset only one environment that is not the first one - env.unwrapped.reset(env_ids=torch.tensor([num_envs - 1], device=env.unwrapped.device)) - self.check_initial_state_recorder_term(env) - - # close the environment - env.close() - - def check_initial_state_recorder_term(self, env): - """Check values recorded by the initial state recorder terms. - - Args: - env: Environment instance. - """ - current_state = env.unwrapped.scene.get_state(is_relative=True) - for env_id in range(env.unwrapped.num_envs): - recorded_initial_state = env.unwrapped.recorder_manager.get_episode(env_id).get_initial_state() - are_states_equal, output_log = self.compare_states(recorded_initial_state, current_state, env_id) - self.assertTrue(are_states_equal, msg=output_log) - - def compare_states(self, compared_state, ground_truth_state, ground_truth_env_id) -> (bool, str): - """Compare a state with the given ground_truth. - - Args: - compared_state: State to be compared. - ground_truth_state: Ground truth state. - ground_truth_env_id: Index of the environment in the ground_truth states to be compared. - - Returns: - bool: True if states match, False otherwise. - str: Error log if states don't match. - """ - for asset_type in ["articulation", "rigid_object"]: - for asset_name in ground_truth_state[asset_type].keys(): - for state_name in ground_truth_state[asset_type][asset_name].keys(): - runtime_asset_state = ground_truth_state[asset_type][asset_name][state_name][ground_truth_env_id] - dataset_asset_state = compared_state[asset_type][asset_name][state_name][0] - if len(dataset_asset_state) != len(runtime_asset_state): - return False, f"State shape of {state_name} for asset {asset_name} don't match" - for i in range(len(dataset_asset_state)): - if abs(dataset_asset_state[i] - runtime_asset_state[i]) > 0.01: - return ( - False, - f'State ["{asset_type}"]["{asset_name}"]["{state_name}"][{i}] don\'t match\r\n', - ) - return True, "" - - -if __name__ == "__main__": - run_tests() +@pytest.fixture(scope="session", autouse=True) +def setup_carb_settings(): + """Set up carb settings to prevent simulation getting stuck.""" + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + +@pytest.fixture +def temp_dir(): + """Create a temporary directory for test datasets.""" + temp_dir = tempfile.mkdtemp() + yield temp_dir + shutil.rmtree(temp_dir) + + +def compare_states(compared_state, ground_truth_state, ground_truth_env_id) -> tuple[bool, str]: + """Compare a state with the given ground_truth. + + Args: + compared_state: State to be compared. + ground_truth_state: Ground truth state. + ground_truth_env_id: Index of the environment in the ground_truth states to be compared. + + Returns: + bool: True if states match, False otherwise. + str: Error log if states don't match. + """ + for asset_type in ["articulation", "rigid_object"]: + for asset_name in ground_truth_state[asset_type].keys(): + for state_name in ground_truth_state[asset_type][asset_name].keys(): + runtime_asset_state = ground_truth_state[asset_type][asset_name][state_name][ground_truth_env_id] + dataset_asset_state = compared_state[asset_type][asset_name][state_name][0] + if len(dataset_asset_state) != len(runtime_asset_state): + return False, f"State shape of {state_name} for asset {asset_name} don't match" + for i in range(len(dataset_asset_state)): + if abs(dataset_asset_state[i] - runtime_asset_state[i]) > 0.01: + return ( + False, + f'State ["{asset_type}"]["{asset_name}"]["{state_name}"][{i}] don\'t match\r\n', + ) + return True, "" + + +def check_initial_state_recorder_term(env): + """Check values recorded by the initial state recorder terms. + + Args: + env: Environment instance. + """ + current_state = env.unwrapped.scene.get_state(is_relative=True) + for env_id in range(env.unwrapped.num_envs): + recorded_initial_state = env.unwrapped.recorder_manager.get_episode(env_id).get_initial_state() + are_states_equal, output_log = compare_states(recorded_initial_state, current_state, env_id) + assert are_states_equal, output_log + + +@pytest.mark.parametrize("task_name", ["Isaac-Lift-Cube-Franka-v0"]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 2]) +def test_action_state_recorder_terms(task_name, device, num_envs, temp_dir): + """Check action state recorder terms.""" + omni.usd.get_context().new_stage() + + dummy_dataset_filename = f"{uuid.uuid4()}.hdf5" + + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # set recorder configurations for this test + env_cfg.recorders = ActionStateRecorderManagerCfg() + env_cfg.recorders.dataset_export_dir_path = temp_dir + env_cfg.recorders.dataset_filename = dummy_dataset_filename + + # create environment + env = gym.make(task_name, cfg=env_cfg) + + # reset all environment instances to trigger post-reset recorder callbacks + env.reset() + check_initial_state_recorder_term(env) + + # reset only one environment that is not the first one + env.unwrapped.reset(env_ids=torch.tensor([num_envs - 1], device=env.unwrapped.device)) + check_initial_state_recorder_term(env) + + # close the environment + env.close() diff --git a/source/isaaclab/test/envs/test_color_randomization.py b/source/isaaclab/test/envs/test_color_randomization.py new file mode 100644 index 000000000000..619c7b3368fc --- /dev/null +++ b/source/isaaclab/test/envs/test_color_randomization.py @@ -0,0 +1,173 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script tests the functionality of texture randomization applied to the cartpole scene. +""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +app_launcher = AppLauncher(headless=True, enable_cameras=True) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import math + +import pytest +import torch + +import omni.usd + +import isaaclab.envs.mdp as mdp +from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg +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.utils import configclass +from isaaclab.utils.version import get_isaac_sim_version + +from isaaclab_tasks.manager_based.classic.cartpole.cartpole_env_cfg import CartpoleSceneCfg + + +@configclass +class ActionsCfg: + """Action specifications for the environment.""" + + joint_efforts = mdp.JointEffortActionCfg(asset_name="robot", joint_names=["slider_to_cart"], scale=5.0) + + +@configclass +class ObservationsCfg: + """Observation specifications for the environment.""" + + @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.""" + + # on prestartup apply a new set of textures + # note from @mayank: Changed from 'reset' to 'prestartup' to make test pass. + # The error happens otherwise on Kit thread which is not the main thread. + cart_texture_randomizer = EventTerm( + func=mdp.randomize_visual_color, + mode="prestartup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=["cart"]), + "colors": {"r": (0.0, 1.0), "g": (0.0, 1.0), "b": (0.0, 1.0)}, + "event_name": "cart_color_randomizer", + }, + ) + + # on reset apply a new set of textures + pole_texture_randomizer = EventTerm( + func=mdp.randomize_visual_color, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=["pole"]), + "colors": {"r": (0.0, 1.0), "g": (0.0, 1.0), "b": (0.0, 1.0)}, + "event_name": "pole_color_randomizer", + }, + ) + + 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.1, 0.1), + }, + ) + + 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.125 * math.pi, 0.125 * math.pi), + "velocity_range": (-0.01 * math.pi, 0.01 * math.pi), + }, + ) + + +@configclass +class CartpoleEnvCfg(ManagerBasedEnvCfg): + """Configuration for the cartpole environment.""" + + # Scene settings + scene = CartpoleSceneCfg(env_spacing=2.5) + + # Basic settings + actions = ActionsCfg() + observations = ObservationsCfg() + events = EventCfg() + + def __post_init__(self): + """Post initialization.""" + # viewer settings + self.viewer.eye = [4.5, 0.0, 6.0] + self.viewer.lookat = [0.0, 0.0, 2.0] + # step settings + self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz + # simulation settings + self.sim.dt = 0.005 # sim step every 5ms: 200Hz + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_color_randomization(device): + """Test color randomization for cartpole environment.""" + # skip test if stage in memory is not supported + if get_isaac_sim_version().major < 5: + pytest.skip("Color randomization test hangs in this version of Isaac Sim") + + # Create a new stage + omni.usd.get_context().new_stage() + + try: + # Set the arguments + env_cfg = CartpoleEnvCfg() + env_cfg.scene.num_envs = 16 + env_cfg.scene.replicate_physics = False + env_cfg.sim.device = device + + # Setup base environment + env = ManagerBasedEnv(cfg=env_cfg) + + try: + # Simulate physics + with torch.inference_mode(): + for count in range(50): + # Reset every few steps to check nothing breaks + if count % 10 == 0: + env.reset() + # Sample random actions + joint_efforts = torch.randn_like(env.action_manager.action) + # Step the environment + env.step(joint_efforts) + finally: + env.close() + finally: + # Clean up stage + omni.usd.get_context().close_stage() diff --git a/source/isaaclab/test/envs/test_direct_marl_env.py b/source/isaaclab/test/envs/test_direct_marl_env.py index 8bc244067638..d7ebd04610b4 100644 --- a/source/isaaclab/test/envs/test_direct_marl_env.py +++ b/source/isaaclab/test/envs/test_direct_marl_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,18 +10,14 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests - -# Can set this to False to see the GUI for debugging -HEADLESS = True +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest +import pytest import omni.usd @@ -57,39 +53,28 @@ class EmptyEnvCfg(DirectMARLEnvCfg): return EmptyEnvCfg() -class TestDirectMARLEnv(unittest.TestCase): - """Test for direct MARL env class""" - - """ - Tests - """ - - def test_initialization(self): - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - # create a new stage - omni.usd.get_context().new_stage() - try: - # create environment - env = DirectMARLEnv(cfg=get_empty_base_env_cfg(device=device)) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the DirectMARLEnv environment. Error: {e}") - - # check multi-agent config - self.assertEqual(env.num_agents, 2) - self.assertEqual(env.max_num_agents, 2) - # check spaces - self.assertEqual(env.state_space.shape, (7,)) - self.assertEqual(len(env.observation_spaces), 2) - self.assertEqual(len(env.action_spaces), 2) - # close the environment - env.close() - - -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_initialization(device): + """Test initialization of DirectMARLEnv.""" + # create a new stage + omni.usd.get_context().new_stage() + try: + # create environment + env = DirectMARLEnv(cfg=get_empty_base_env_cfg(device=device)) + except Exception as e: + if "env" in locals() and hasattr(env, "_is_closed"): + env.close() + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the DirectMARLEnv environment. Error: {e}") + + # check multi-agent config + assert env.num_agents == 2 + assert env.max_num_agents == 2 + # check spaces + assert env.state_space.shape == (7,) + assert len(env.observation_spaces) == 2 + assert len(env.action_spaces) == 2 + # close the environment + env.close() diff --git a/source/isaaclab/test/envs/test_env_rendering_logic.py b/source/isaaclab/test/envs/test_env_rendering_logic.py index 426bb8e2b4c3..70f0a01f212a 100644 --- a/source/isaaclab/test/envs/test_env_rendering_logic.py +++ b/source/isaaclab/test/envs/test_env_rendering_logic.py @@ -1,21 +1,20 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app # need to set "enable_cameras" true to be able to do rendering tests -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" +import pytest import torch -import unittest import omni.usd @@ -110,99 +109,100 @@ def _get_dones(self): return Env(cfg=EnvCfg()) -class TestEnvRenderingLogic(unittest.TestCase): - """Test the rendering logic of the different environment workflows.""" +@pytest.fixture +def physics_callback(): + """Create a physics callback for tracking physics steps.""" + physics_time = 0.0 + num_physics_steps = 0 + + def callback(dt): + nonlocal physics_time, num_physics_steps + physics_time += dt + num_physics_steps += 1 + + return callback, lambda: (physics_time, num_physics_steps) + + +@pytest.fixture +def render_callback(): + """Create a render callback for tracking render steps.""" + render_time = 0.0 + num_render_steps = 0 - def _physics_callback(self, dt): - # called at every physics step - self.physics_time += dt - self.num_physics_steps += 1 - - def _render_callback(self, event): - # called at every render step - self.render_time += event.payload["dt"] - self.num_render_steps += 1 - - def test_env_rendering_logic(self): - for env_type in ["manager_based_env", "manager_based_rl_env", "direct_rl_env"]: - for render_interval in [1, 2, 4, 8, 10]: - with self.subTest(env_type=env_type, render_interval=render_interval): - # time tracking variables - self.physics_time = 0.0 - self.render_time = 0.0 - # step tracking variables - self.num_physics_steps = 0 - self.num_render_steps = 0 - - # create a new stage - omni.usd.get_context().new_stage() - try: - # create environment - if env_type == "manager_based_env": - env = create_manager_based_env(render_interval) - elif env_type == "manager_based_rl_env": - env = create_manager_based_rl_env(render_interval) - else: - env = create_direct_rl_env(render_interval) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment {env_type}. Error: {e}") - - # enable the flag to render the environment - # note: this is only done for the unit testing to "fake" camera rendering. - # normally this is set to True when cameras are created. - env.sim.set_setting("/isaaclab/render/rtx_sensors", True) - - # disable the app from shutting down when the environment is closed - # FIXME: Why is this needed in this test but not in the other tests? - # Without it, the test will exit after the environment is closed - env.sim._app_control_on_stop_handle = None # type: ignore - - # check that we are in partial rendering mode for the environment - # this is enabled due to app launcher setting "enable_cameras=True" - self.assertEqual(env.sim.render_mode, SimulationContext.RenderMode.PARTIAL_RENDERING) - - # add physics and render callbacks - env.sim.add_physics_callback("physics_step", self._physics_callback) - env.sim.add_render_callback("render_step", self._render_callback) - - # create a zero action tensor for stepping the environment - actions = torch.zeros((env.num_envs, 0), device=env.device) - - # run the environment and check the rendering logic - for i in range(50): - # apply zero actions - env.step(action=actions) - - # check that we have completed the correct number of physics steps - self.assertEqual( - self.num_physics_steps, (i + 1) * env.cfg.decimation, msg="Physics steps mismatch" - ) - # check that we have simulated physics for the correct amount of time - self.assertAlmostEqual( - self.physics_time, self.num_physics_steps * env.cfg.sim.dt, msg="Physics time mismatch" - ) - - # check that we have completed the correct number of rendering steps - self.assertEqual( - self.num_render_steps, - (i + 1) * env.cfg.decimation // env.cfg.sim.render_interval, - msg="Render steps mismatch", - ) - # check that we have rendered for the correct amount of time - self.assertAlmostEqual( - self.render_time, - self.num_render_steps * env.cfg.sim.dt * env.cfg.sim.render_interval, - msg="Render time mismatch", - ) - - # close the environment - env.close() - - -if __name__ == "__main__": - run_tests() + def callback(event): + nonlocal render_time, num_render_steps + render_time += event.payload["dt"] + num_render_steps += 1 + + return callback, lambda: (render_time, num_render_steps) + + +@pytest.mark.parametrize("env_type", ["manager_based_env", "manager_based_rl_env", "direct_rl_env"]) +@pytest.mark.parametrize("render_interval", [1, 2, 4, 8, 10]) +def test_env_rendering_logic(env_type, render_interval, physics_callback, render_callback): + """Test the rendering logic of the different environment workflows.""" + physics_cb, get_physics_stats = physics_callback + render_cb, get_render_stats = render_callback + + # create a new stage + omni.usd.get_context().new_stage() + try: + # create environment + if env_type == "manager_based_env": + env = create_manager_based_env(render_interval) + elif env_type == "manager_based_rl_env": + env = create_manager_based_rl_env(render_interval) + else: + env = create_direct_rl_env(render_interval) + except Exception as e: + if "env" in locals() and hasattr(env, "_is_closed"): + env.close() + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment {env_type}. Error: {e}") + + # enable the flag to render the environment + # note: this is only done for the unit testing to "fake" camera rendering. + # normally this is set to True when cameras are created. + env.sim.set_setting("/isaaclab/render/rtx_sensors", True) + + # disable the app from shutting down when the environment is closed + # FIXME: Why is this needed in this test but not in the other tests? + # Without it, the test will exit after the environment is closed + env.sim._app_control_on_stop_handle = None # type: ignore + + # check that we are in partial rendering mode for the environment + # this is enabled due to app launcher setting "enable_cameras=True" + assert env.sim.render_mode == SimulationContext.RenderMode.PARTIAL_RENDERING + + # add physics and render callbacks + env.sim.add_physics_callback("physics_step", physics_cb) + env.sim.add_render_callback("render_step", render_cb) + + # create a zero action tensor for stepping the environment + actions = torch.zeros((env.num_envs, 0), device=env.device) + + # run the environment and check the rendering logic + for i in range(50): + # apply zero actions + env.step(action=actions) + + # check that we have completed the correct number of physics steps + _, num_physics_steps = get_physics_stats() + assert num_physics_steps == (i + 1) * env.cfg.decimation, "Physics steps mismatch" + # check that we have simulated physics for the correct amount of time + physics_time, _ = get_physics_stats() + assert abs(physics_time - num_physics_steps * env.cfg.sim.dt) < 1e-6, "Physics time mismatch" + + # check that we have completed the correct number of rendering steps + _, num_render_steps = get_render_stats() + assert num_render_steps == (i + 1) * env.cfg.decimation // env.cfg.sim.render_interval, "Render steps mismatch" + # check that we have rendered for the correct amount of time + render_time, _ = get_render_stats() + assert abs(render_time - num_render_steps * env.cfg.sim.dt * env.cfg.sim.render_interval) < 1e-6, ( + "Render time mismatch" + ) + + # close the environment + env.close() diff --git a/source/isaaclab/test/envs/test_manager_based_env.py b/source/isaaclab/test/envs/test_manager_based_env.py index 883ac6c7c6ce..7ec9ef2d43f8 100644 --- a/source/isaaclab/test/envs/test_manager_based_env.py +++ b/source/isaaclab/test/envs/test_manager_based_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,23 +10,21 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests - -# Can set this to False to see the GUI for debugging -HEADLESS = True +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" +import pytest import torch -import unittest import omni.usd from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.utils import configclass @@ -38,6 +36,22 @@ class EmptyManagerCfg: pass +@configclass +class EmptyObservationWithHistoryCfg: + """Empty observation with history specifications for the environment.""" + + @configclass + class EmptyObservationGroupWithHistoryCfg(ObsGroup): + """Empty observation with history specifications for the environment.""" + + dummy_term: ObsTerm = ObsTerm(func=lambda env: torch.randn(env.num_envs, 1, device=env.device)) + + def __post_init__(self): + self.history_length = 5 + + empty_observation: EmptyObservationGroupWithHistoryCfg = EmptyObservationGroupWithHistoryCfg() + + @configclass class EmptySceneCfg(InteractiveSceneCfg): """Configuration for an empty scene.""" @@ -71,37 +85,116 @@ def __post_init__(self): return EmptyEnvCfg() -class TestManagerBasedEnv(unittest.TestCase): - """Test for manager-based env class""" +def get_empty_base_env_cfg_with_history(device: str = "cuda:0", num_envs: int = 1, env_spacing: float = 1.0): + """Generate base environment config based on device""" - """ - Tests - """ + @configclass + class EmptyEnvWithHistoryCfg(ManagerBasedEnvCfg): + """Configuration for the empty test environment.""" - def test_initialization(self): - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - # create a new stage - omni.usd.get_context().new_stage() - # create environment - env = ManagerBasedEnv(cfg=get_empty_base_env_cfg(device=device)) - # check size of action manager terms - self.assertEqual(env.action_manager.total_action_dim, 0) - self.assertEqual(len(env.action_manager.active_terms), 0) - self.assertEqual(len(env.action_manager.action_term_dim), 0) - # check size of observation manager terms - self.assertEqual(len(env.observation_manager.active_terms), 0) - self.assertEqual(len(env.observation_manager.group_obs_dim), 0) - self.assertEqual(len(env.observation_manager.group_obs_term_dim), 0) - self.assertEqual(len(env.observation_manager.group_obs_concatenate), 0) - # create actions of correct size (1,0) - act = torch.randn_like(env.action_manager.action) - # step environment to verify setup - for _ in range(2): - obs, ext = env.step(action=act) - # close the environment - env.close() - - -if __name__ == "__main__": - run_tests() + # Scene settings + scene: EmptySceneCfg = EmptySceneCfg(num_envs=num_envs, env_spacing=env_spacing) + # Basic settings + actions: EmptyManagerCfg = EmptyManagerCfg() + observations: EmptyObservationWithHistoryCfg = EmptyObservationWithHistoryCfg() + + def __post_init__(self): + """Post initialization.""" + # step settings + self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz + # simulation settings + self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.render_interval = self.decimation # render every 4 sim steps + # pass device down from test + self.sim.device = device + + return EmptyEnvWithHistoryCfg() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_initialization(device): + """Test initialization of ManagerBasedEnv.""" + # create a new stage + omni.usd.get_context().new_stage() + # create environment + env = ManagerBasedEnv(cfg=get_empty_base_env_cfg(device=device)) + # check size of action manager terms + assert env.action_manager.total_action_dim == 0 + assert len(env.action_manager.active_terms) == 0 + assert len(env.action_manager.action_term_dim) == 0 + # check size of observation manager terms + assert len(env.observation_manager.active_terms) == 0 + assert len(env.observation_manager.group_obs_dim) == 0 + assert len(env.observation_manager.group_obs_term_dim) == 0 + assert len(env.observation_manager.group_obs_concatenate) == 0 + # create actions of correct size (1,0) + act = torch.randn_like(env.action_manager.action) + # step environment to verify setup + for _ in range(2): + obs, ext = env.step(action=act) + # close the environment + env.close() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_observation_history_changes_only_after_step(device): + """Test observation history of ManagerBasedEnv. + + The history buffer should only change after a step is taken. + """ + # create a new stage + omni.usd.get_context().new_stage() + # create environment with history length of 5 + env = ManagerBasedEnv(cfg=get_empty_base_env_cfg_with_history(device=device)) + + # check if history buffer is empty + for group_name in env.observation_manager._group_obs_term_names: + group_term_names = env.observation_manager._group_obs_term_names[group_name] + for term_name in group_term_names: + torch.testing.assert_close( + env.observation_manager._group_obs_term_history_buffer[group_name][term_name].current_length, + torch.zeros((env.num_envs,), device=device, dtype=torch.int64), + ) + + # check if history buffer is empty after compute + env.observation_manager.compute() + for group_name in env.observation_manager._group_obs_term_names: + group_term_names = env.observation_manager._group_obs_term_names[group_name] + for term_name in group_term_names: + torch.testing.assert_close( + env.observation_manager._group_obs_term_history_buffer[group_name][term_name].current_length, + torch.zeros((env.num_envs,), device=device, dtype=torch.int64), + ) + + # check if history buffer is not empty after step + act = torch.randn_like(env.action_manager.action) + env.step(act) + group_obs = dict() + for group_name in env.observation_manager._group_obs_term_names: + group_term_names = env.observation_manager._group_obs_term_names[group_name] + group_obs[group_name] = dict() + for term_name in group_term_names: + torch.testing.assert_close( + env.observation_manager._group_obs_term_history_buffer[group_name][term_name].current_length, + torch.ones((env.num_envs,), device=device, dtype=torch.int64), + ) + group_obs[group_name][term_name] = env.observation_manager._group_obs_term_history_buffer[group_name][ + term_name + ].buffer + + # check if history buffer is not empty after compute and is the same as the buffer after step + env.observation_manager.compute() + for group_name in env.observation_manager._group_obs_term_names: + group_term_names = env.observation_manager._group_obs_term_names[group_name] + for term_name in group_term_names: + torch.testing.assert_close( + env.observation_manager._group_obs_term_history_buffer[group_name][term_name].current_length, + torch.ones((env.num_envs,), device=device, dtype=torch.int64), + ) + assert torch.allclose( + group_obs[group_name][term_name], + env.observation_manager._group_obs_term_history_buffer[group_name][term_name].buffer, + ) + + # close the environment + env.close() 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 new file mode 100644 index 000000000000..72525ddb8e03 --- /dev/null +++ b/source/isaaclab/test/envs/test_manager_based_rl_env_obs_spaces.py @@ -0,0 +1,141 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test texture randomization in the cartpole scene using pytest.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +import gymnasium as gym +import numpy as np +import pytest +import torch + +import omni.usd + +from isaaclab.envs import ManagerBasedRLEnv + +from isaaclab_tasks.manager_based.classic.cartpole.cartpole_camera_env_cfg import ( + CartpoleDepthCameraEnvCfg, + CartpoleRGBCameraEnvCfg, +) +from isaaclab_tasks.manager_based.locomotion.velocity.config.anymal_c.rough_env_cfg import AnymalCRoughEnvCfg + + +@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). + + Before the fix, only the last term in each non-concatenated group would be present + in the observation space Dict. This test ensures all terms are correctly included. + """ + from isaaclab_tasks.manager_based.manipulation.stack.config.franka.stack_joint_pos_env_cfg import ( + FrankaCubeStackEnvCfg, + ) + + # new USD stage + omni.usd.get_context().new_stage() + + # configure the stack env - it has multiple non-concatenated observation groups + env_cfg = FrankaCubeStackEnvCfg() + env_cfg.scene.num_envs = 2 # keep num_envs small for testing + env_cfg.sim.device = device + + env = ManagerBasedRLEnv(cfg=env_cfg) + + # Verify that observation space is properly structured + assert isinstance(env.observation_space, gym.spaces.Dict), "Top-level observation space should be Dict" + + # Test 'policy' group - should have 9 terms (not just the last one due to the bug) + assert "policy" in env.observation_space.spaces, "Policy group missing from observation space" + policy_space = env.observation_space.spaces["policy"] + assert isinstance(policy_space, gym.spaces.Dict), "Policy group should be Dict space" + + expected_policy_terms = [ + "actions", + "joint_pos", + "joint_vel", + "object", + "cube_positions", + "cube_orientations", + "eef_pos", + "eef_quat", + "gripper_pos", + ] + + # This is the key test - before the fix, only "gripper_pos" (last term) would be present + assert len(policy_space.spaces) == len(expected_policy_terms), ( + f"Policy group should have {len(expected_policy_terms)} terms, got {len(policy_space.spaces)}:" + f" {list(policy_space.spaces.keys())}" + ) + + for term_name in expected_policy_terms: + assert term_name in policy_space.spaces, f"Term '{term_name}' missing from policy group" + assert isinstance(policy_space.spaces[term_name], gym.spaces.Box), f"Term '{term_name}' should be Box space" + + # Test 'subtask_terms' group - should have 3 terms (not just the last one) + assert "subtask_terms" in env.observation_space.spaces, "Subtask_terms group missing from observation space" + subtask_space = env.observation_space.spaces["subtask_terms"] + assert isinstance(subtask_space, gym.spaces.Dict), "Subtask_terms group should be Dict space" + + expected_subtask_terms = ["grasp_1", "stack_1", "grasp_2"] + + # Before the fix, only "grasp_2" (last term) would be present + assert len(subtask_space.spaces) == len(expected_subtask_terms), ( + f"Subtask_terms group should have {len(expected_subtask_terms)} terms, got {len(subtask_space.spaces)}:" + f" {list(subtask_space.spaces.keys())}" + ) + + for term_name in expected_subtask_terms: + assert term_name in subtask_space.spaces, f"Term '{term_name}' missing from subtask_terms group" + assert isinstance(subtask_space.spaces[term_name], gym.spaces.Box), f"Term '{term_name}' should be Box space" + + # Test that we can get observations and they match the space structure + env.reset() + action = torch.tensor(env.action_space.sample(), device=env.device) + obs, reward, terminated, truncated, info = env.step(action) + + # Verify all terms are present in actual observations + for term_name in expected_policy_terms: + assert term_name in obs["policy"], f"Term '{term_name}' missing from policy observation" + + for term_name in expected_subtask_terms: + 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/envs/test_manager_based_rl_env_ui.py b/source/isaaclab/test/envs/test_manager_based_rl_env_ui.py index 93010ae6ef36..f35c11a1c401 100644 --- a/source/isaaclab/test/envs/test_manager_based_rl_env_ui.py +++ b/source/isaaclab/test/envs/test_manager_based_rl_env_ui.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,19 +10,12 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher -# Can set this to False to see the GUI for debugging -HEADLESS = True - -# launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" -import unittest - import carb import omni.usd from isaacsim.core.utils.extensions import enable_extension @@ -64,7 +57,7 @@ class EmptyEnvCfg(ManagerBasedRLEnvCfg): rewards: EmptyManagerCfg = EmptyManagerCfg() terminations: EmptyManagerCfg = EmptyManagerCfg() # Define window - ui_window_class_type: ManagerBasedRLEnvWindow = ManagerBasedRLEnvWindow + ui_window_class_type: type[ManagerBasedRLEnvWindow] = ManagerBasedRLEnvWindow def __post_init__(self): """Post initialization.""" @@ -81,24 +74,14 @@ def __post_init__(self): return EmptyEnvCfg() -class TestManagerBasedRLEnvUI(unittest.TestCase): - """Test for manager-based RL env class UI""" - - """ - Tests - """ - - def test_ui_window(self): - device = "cuda:0" - # override sim setting to enable UI - carb.settings.get_settings().set_bool("/app/window/enabled", True) - # create a new stage - omni.usd.get_context().new_stage() - # create environment - env = ManagerBasedRLEnv(cfg=get_empty_base_env_cfg(device=device)) - # close the environment - env.close() - - -if __name__ == "__main__": - run_tests() +def test_ui_window(): + """Test UI window of ManagerBasedRLEnv.""" + device = "cuda:0" + # override sim setting to enable UI + carb.settings.get_settings().set_bool("/app/window/enabled", True) + # create a new stage + omni.usd.get_context().new_stage() + # create environment + env = ManagerBasedRLEnv(cfg=get_empty_base_env_cfg(device=device)) + # close the environment + env.close() diff --git a/source/isaaclab/test/envs/test_modify_env_param_curr_term.py b/source/isaaclab/test/envs/test_modify_env_param_curr_term.py new file mode 100644 index 000000000000..a23a29f38606 --- /dev/null +++ b/source/isaaclab/test/envs/test_modify_env_param_curr_term.py @@ -0,0 +1,104 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test texture randomization in the cartpole scene using pytest.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import pytest +import torch + +import omni.usd + +import isaaclab.envs.mdp as mdp +from isaaclab.assets import Articulation +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.classic.cartpole.cartpole_env_cfg import CartpoleEnvCfg + + +def replace_value(env, env_id, data, value, num_steps): + if env.common_step_counter > num_steps and data != value: + return value + # use the sentinel to indicate โ€œno changeโ€ + return mdp.modify_env_param.NO_CHANGE + + +@configclass +class CurriculumsCfg: + modify_observation_joint_pos = CurrTerm( + # test writing a term's func. + func=mdp.modify_term_cfg, + params={ + "address": "observations.policy.joint_pos_rel.func", + "modify_fn": replace_value, + "modify_params": {"value": mdp.joint_pos, "num_steps": 1}, + }, + ) + + # test writing a term's param that involves dictionary. + modify_reset_joint_pos = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "events.reset_cart_position.params.position_range", + "modify_fn": replace_value, + "modify_params": {"value": (-0.0, 0.0), "num_steps": 1}, + }, + ) + + # test writing a non_term env parameter using modify_env_param. + modify_episode_max_length = CurrTerm( + func=mdp.modify_env_param, + params={ + "address": "cfg.episode_length_s", + "modify_fn": replace_value, + "modify_params": {"value": 20, "num_steps": 1}, + }, + ) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_curriculum_modify_env_param(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 = CartpoleEnvCfg() + env_cfg.scene.num_envs = 16 + env_cfg.curriculum = CurriculumsCfg() + env_cfg.sim.device = device + + env = ManagerBasedRLEnv(cfg=env_cfg) + robot: Articulation = env.scene["robot"] + + # run a few steps under inference mode + with torch.inference_mode(): + for count in range(3): + env.reset() + actions = torch.randn_like(env.action_manager.action) + + if count == 0: + # test before curriculum kicks in, value agrees with default configuration + joint_ids = env.event_manager.cfg.reset_cart_position.params["asset_cfg"].joint_ids + assert env.observation_manager.cfg.policy.joint_pos_rel.func == mdp.joint_pos_rel + assert torch.any(robot.data.joint_pos[:, joint_ids] != 0.0) + assert env.max_episode_length_s == env_cfg.episode_length_s + + if count == 2: + # test after curriculum makes effect, value agrees with new values + assert env.observation_manager.cfg.policy.joint_pos_rel.func == mdp.joint_pos + joint_ids = env.event_manager.cfg.reset_cart_position.params["asset_cfg"].joint_ids + assert torch.all(robot.data.joint_pos[:, joint_ids] == 0.0) + assert env.max_episode_length_s == 20 + + env.step(actions) + + env.close() diff --git a/source/isaaclab/test/envs/test_null_command_term.py b/source/isaaclab/test/envs/test_null_command_term.py index b897b6db56cc..c394fc94d5ce 100644 --- a/source/isaaclab/test/envs/test_null_command_term.py +++ b/source/isaaclab/test/envs/test_null_command_term.py @@ -1,51 +1,48 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest from collections import namedtuple -from isaaclab.envs.mdp import NullCommandCfg +import pytest +from isaaclab.envs.mdp import NullCommandCfg -class TestNullCommandTerm(unittest.TestCase): - """Test cases for null command generator.""" - def setUp(self) -> None: - self.env = namedtuple("ManagerBasedRLEnv", ["num_envs", "dt", "device"])(20, 0.1, "cpu") +@pytest.fixture +def env(): + """Create a dummy environment.""" + return namedtuple("ManagerBasedRLEnv", ["num_envs", "dt", "device"])(20, 0.1, "cpu") - def test_str(self): - """Test the string representation of the command manager.""" - cfg = NullCommandCfg() - command_term = cfg.class_type(cfg, self.env) - # print the expected string - print() - print(command_term) - def test_compute(self): - """Test the compute function. For null command generator, it does nothing.""" - cfg = NullCommandCfg() - command_term = cfg.class_type(cfg, self.env) +def test_str(env): + """Test the string representation of the command manager.""" + cfg = NullCommandCfg() + command_term = cfg.class_type(cfg, env) + # print the expected string + print() + print(command_term) - # test the reset function - command_term.reset() - # test the compute function - command_term.compute(dt=self.env.dt) - # expect error - with self.assertRaises(RuntimeError): - command_term.command +def test_compute(env): + """Test the compute function. For null command generator, it does nothing.""" + cfg = NullCommandCfg() + command_term = cfg.class_type(cfg, env) -if __name__ == "__main__": - run_tests() + # test the reset function + command_term.reset() + # test the compute function + command_term.compute(dt=env.dt) + # expect error + with pytest.raises(RuntimeError): + command_term.command diff --git a/source/isaaclab/test/envs/test_scale_randomization.py b/source/isaaclab/test/envs/test_scale_randomization.py index 122f011f5317..282c6b2a3d85 100644 --- a/source/isaaclab/test/envs/test_scale_randomization.py +++ b/source/isaaclab/test/envs/test_scale_randomization.py @@ -1,8 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause + """ This script checks the functionality of scale randomization. """ @@ -11,7 +12,7 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -19,8 +20,8 @@ """Rest everything follows.""" +import pytest import torch -import unittest import omni.usd from pxr import Sdf @@ -29,11 +30,10 @@ import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg, RigidObject, RigidObjectCfg from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg -from isaaclab.managers import ActionTerm, ActionTermCfg +from isaaclab.managers import ActionTerm, ActionTermCfg, SceneEntityCfg 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.scene import InteractiveSceneCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass @@ -275,71 +275,76 @@ def __post_init__(self): # simulation settings self.sim.dt = 0.01 self.sim.physics_material = self.scene.terrain.physics_material - - -class TestScaleRandomization(unittest.TestCase): - """Test for texture randomization""" - - """ - Tests - """ - - def test_scale_randomization(self): - """Main function.""" - - # setup base environment - env = ManagerBasedEnv(cfg=CubeEnvCfg()) - # setup target position commands - target_position = torch.rand(env.num_envs, 3, device=env.device) * 2 - target_position[:, 2] += 2.0 - # offset all targets so that they move to the world origin - target_position -= env.scene.env_origins - - stage = omni.usd.get_context().get_stage() - - # test to make sure all assets in the scene are created - all_prim_paths = sim_utils.find_matching_prim_paths("/World/envs/env_.*/cube.*/.*") - self.assertEqual(len(all_prim_paths), (env.num_envs * 2)) - - # test to make sure randomized values are truly random - applied_scaling_randomization = set() - prim_paths = sim_utils.find_matching_prim_paths("/World/envs/env_.*/cube1") - - for i in range(3): - prim_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_paths[i]) - scale_spec = prim_spec.GetAttributeAtPath(prim_paths[i] + ".xformOp:scale") - if scale_spec.default in applied_scaling_randomization: - raise ValueError( - "Detected repeat in applied scale values - indication scaling randomization is not working." - ) - applied_scaling_randomization.add(scale_spec.default) - - # test to make sure that fixed values are assigned correctly - prim_paths = sim_utils.find_matching_prim_paths("/World/envs/env_.*/cube2") - for i in range(3): - prim_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_paths[i]) - scale_spec = prim_spec.GetAttributeAtPath(prim_paths[i] + ".xformOp:scale") - self.assertEqual(tuple(scale_spec.default), (1.0, 1.0, 1.0)) - - # simulate physics - with torch.inference_mode(): - for count in range(200): - # reset every few steps to check nothing breaks - if count % 100 == 0: - env.reset() - # step the environment - env.step(target_position) - - env.close() - - def test_scale_randomization_failure_replicate_physics(self): - with self.assertRaises(ValueError): - cfg_failure = CubeEnvCfg() - cfg_failure.scene.replicate_physics = True - env = ManagerBasedEnv(cfg_failure) - + self.sim.render_interval = self.decimation + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_scale_randomization(device): + """Test scale randomization for cube environment.""" + # create a new stage + omni.usd.get_context().new_stage() + + # set the device + env_cfg = CubeEnvCfg() + env_cfg.sim.device = device + + # setup base environment + env = ManagerBasedEnv(cfg=env_cfg) + # setup target position commands + target_position = torch.rand(env.num_envs, 3, device=env.device) * 2 + target_position[:, 2] += 2.0 + # offset all targets so that they move to the world origin + target_position -= env.scene.env_origins + + # test to make sure all assets in the scene are created + all_prim_paths = sim_utils.find_matching_prim_paths("/World/envs/env_.*/cube.*/.*") + assert len(all_prim_paths) == (env.num_envs * 2) + + # test to make sure randomized values are truly random + applied_scaling_randomization = set() + prim_paths = sim_utils.find_matching_prim_paths("/World/envs/env_.*/cube1") + + # get the stage + stage = omni.usd.get_context().get_stage() + + # check if the scale values are truly random + for i in range(3): + prim_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_paths[i]) + scale_spec = prim_spec.GetAttributeAtPath(prim_paths[i] + ".xformOp:scale") + if scale_spec.default in applied_scaling_randomization: + raise ValueError( + "Detected repeat in applied scale values - indication scaling randomization is not working." + ) + applied_scaling_randomization.add(scale_spec.default) + + # test to make sure that fixed values are assigned correctly + prim_paths = sim_utils.find_matching_prim_paths("/World/envs/env_.*/cube2") + for i in range(3): + prim_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_paths[i]) + scale_spec = prim_spec.GetAttributeAtPath(prim_paths[i] + ".xformOp:scale") + assert tuple(scale_spec.default) == (1.0, 1.0, 1.0) + + # simulate physics + with torch.inference_mode(): + for count in range(200): + # reset every few steps to check nothing breaks + if count % 100 == 0: + env.reset() + # step the environment + env.step(target_position) + + env.close() + + +def test_scale_randomization_failure_replicate_physics(): + """Test scale randomization failure when replicate physics is set to True.""" + # create a new stage + omni.usd.get_context().new_stage() + # set the arguments + cfg_failure = CubeEnvCfg() + cfg_failure.scene.replicate_physics = True + + # run the test + with pytest.raises(RuntimeError): + env = ManagerBasedEnv(cfg_failure) env.close() - - -if __name__ == "__main__": - run_tests() diff --git a/source/isaaclab/test/envs/test_spaces_utils.py b/source/isaaclab/test/envs/test_spaces_utils.py index e7eb9f8760fe..f170173ea384 100644 --- a/source/isaaclab/test/envs/test_spaces_utils.py +++ b/source/isaaclab/test/envs/test_spaces_utils.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,166 +10,153 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests - -# Can set this to False to see the GUI for debugging -HEADLESS = True +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=HEADLESS) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import numpy as np import torch -import unittest from gymnasium.spaces import Box, Dict, Discrete, MultiDiscrete, Tuple from isaaclab.envs.utils.spaces import deserialize_space, sample_space, serialize_space, spec_to_gym_space -class TestSpacesUtils(unittest.TestCase): - """Test for spaces utils' functions""" - - """ - Tests - """ - - def test_spec_to_gym_space(self): - # fundamental spaces - # Box - space = spec_to_gym_space(1) - self.assertIsInstance(space, Box) - self.assertEqual(space.shape, (1,)) - space = spec_to_gym_space([1, 2, 3, 4, 5]) - self.assertIsInstance(space, Box) - self.assertEqual(space.shape, (1, 2, 3, 4, 5)) - space = spec_to_gym_space(Box(low=-1.0, high=1.0, shape=(1, 2))) - self.assertIsInstance(space, Box) - # Discrete - space = spec_to_gym_space({2}) - self.assertIsInstance(space, Discrete) - self.assertEqual(space.n, 2) - space = spec_to_gym_space(Discrete(2)) - self.assertIsInstance(space, Discrete) - # MultiDiscrete - space = spec_to_gym_space([{1}, {2}, {3}]) - self.assertIsInstance(space, MultiDiscrete) - self.assertEqual(space.nvec.shape, (3,)) - space = spec_to_gym_space(MultiDiscrete(np.array([1, 2, 3]))) - self.assertIsInstance(space, MultiDiscrete) - # composite spaces - # Tuple - space = spec_to_gym_space(([1, 2, 3, 4, 5], {2}, [{1}, {2}, {3}])) - self.assertIsInstance(space, Tuple) - self.assertEqual(len(space), 3) - self.assertIsInstance(space[0], Box) - self.assertIsInstance(space[1], Discrete) - self.assertIsInstance(space[2], MultiDiscrete) - space = spec_to_gym_space(Tuple((Box(-1, 1, shape=(1,)), Discrete(2)))) - self.assertIsInstance(space, Tuple) - # Dict - space = spec_to_gym_space({"box": [1, 2, 3, 4, 5], "discrete": {2}, "multi_discrete": [{1}, {2}, {3}]}) - self.assertIsInstance(space, Dict) - self.assertEqual(len(space), 3) - self.assertIsInstance(space["box"], Box) - self.assertIsInstance(space["discrete"], Discrete) - self.assertIsInstance(space["multi_discrete"], MultiDiscrete) - space = spec_to_gym_space(Dict({"box": Box(-1, 1, shape=(1,)), "discrete": Discrete(2)})) - self.assertIsInstance(space, Dict) - - def test_sample_space(self): - device = "cpu" - # fundamental spaces - # Box - sample = sample_space(Box(low=-1.0, high=1.0, shape=(1, 2)), device, batch_size=1) - self.assertIsInstance(sample, torch.Tensor) - self._check_tensorized(sample, batch_size=1) - # Discrete - sample = sample_space(Discrete(2), device, batch_size=2) - self.assertIsInstance(sample, torch.Tensor) - self._check_tensorized(sample, batch_size=2) - # MultiDiscrete - sample = sample_space(MultiDiscrete(np.array([1, 2, 3])), device, batch_size=3) - self.assertIsInstance(sample, torch.Tensor) - self._check_tensorized(sample, batch_size=3) - # composite spaces - # Tuple - sample = sample_space(Tuple((Box(-1, 1, shape=(1,)), Discrete(2))), device, batch_size=4) - self.assertIsInstance(sample, (tuple, list)) - self._check_tensorized(sample, batch_size=4) - # Dict - sample = sample_space(Dict({"box": Box(-1, 1, shape=(1,)), "discrete": Discrete(2)}), device, batch_size=5) - self.assertIsInstance(sample, dict) - self._check_tensorized(sample, batch_size=5) - - def test_space_serialization_deserialization(self): - # fundamental spaces - # Box - space = 1 - output = deserialize_space(serialize_space(space)) - self.assertEqual(space, output) - space = [1, 2, 3, 4, 5] - output = deserialize_space(serialize_space(space)) - self.assertEqual(space, output) - space = Box(low=-1.0, high=1.0, shape=(1, 2)) - output = deserialize_space(serialize_space(space)) - self.assertIsInstance(output, Box) - self.assertTrue((space.low == output.low).all()) - self.assertTrue((space.high == output.high).all()) - self.assertEqual(space.shape, output.shape) - # Discrete - space = {2} - output = deserialize_space(serialize_space(space)) - self.assertEqual(space, output) - space = Discrete(2) - output = deserialize_space(serialize_space(space)) - self.assertIsInstance(output, Discrete) - self.assertEqual(space.n, output.n) - # MultiDiscrete - space = [{1}, {2}, {3}] - output = deserialize_space(serialize_space(space)) - self.assertEqual(space, output) - space = MultiDiscrete(np.array([1, 2, 3])) - output = deserialize_space(serialize_space(space)) - self.assertIsInstance(output, MultiDiscrete) - self.assertTrue((space.nvec == output.nvec).all()) - # composite spaces - # Tuple - space = ([1, 2, 3, 4, 5], {2}, [{1}, {2}, {3}]) - output = deserialize_space(serialize_space(space)) - self.assertEqual(space, output) - space = Tuple((Box(-1, 1, shape=(1,)), Discrete(2))) - output = deserialize_space(serialize_space(space)) - self.assertIsInstance(output, Tuple) - self.assertEqual(len(output), 2) - self.assertIsInstance(output[0], Box) - self.assertIsInstance(output[1], Discrete) - # Dict - space = {"box": [1, 2, 3, 4, 5], "discrete": {2}, "multi_discrete": [{1}, {2}, {3}]} - output = deserialize_space(serialize_space(space)) - self.assertEqual(space, output) - space = Dict({"box": Box(-1, 1, shape=(1,)), "discrete": Discrete(2)}) - output = deserialize_space(serialize_space(space)) - self.assertIsInstance(output, Dict) - self.assertEqual(len(output), 2) - self.assertIsInstance(output["box"], Box) - self.assertIsInstance(output["discrete"], Discrete) - - """ - Helper functions. - """ - - def _check_tensorized(self, sample, batch_size): - if isinstance(sample, (tuple, list)): - list(map(self._check_tensorized, sample, [batch_size] * len(sample))) - elif isinstance(sample, dict): - list(map(self._check_tensorized, sample.values(), [batch_size] * len(sample))) - else: - self.assertIsInstance(sample, torch.Tensor) - self.assertEqual(sample.shape[0], batch_size) - - -if __name__ == "__main__": - run_tests() +def test_spec_to_gym_space(): + """Test conversion of specs to gym spaces.""" + # fundamental spaces + # Box + space = spec_to_gym_space(1) + assert isinstance(space, Box) + assert space.shape == (1,) + space = spec_to_gym_space([1, 2, 3, 4, 5]) + assert isinstance(space, Box) + assert space.shape == (1, 2, 3, 4, 5) + space = spec_to_gym_space(Box(low=-1.0, high=1.0, shape=(1, 2))) + assert isinstance(space, Box) + # Discrete + space = spec_to_gym_space({2}) + assert isinstance(space, Discrete) + assert space.n == 2 + space = spec_to_gym_space(Discrete(2)) + assert isinstance(space, Discrete) + # MultiDiscrete + space = spec_to_gym_space([{1}, {2}, {3}]) + assert isinstance(space, MultiDiscrete) + assert space.nvec.shape == (3,) + space = spec_to_gym_space(MultiDiscrete(np.array([1, 2, 3]))) + assert isinstance(space, MultiDiscrete) + # composite spaces + # Tuple + space = spec_to_gym_space(([1, 2, 3, 4, 5], {2}, [{1}, {2}, {3}])) + assert isinstance(space, Tuple) + assert len(space) == 3 + assert isinstance(space[0], Box) + assert isinstance(space[1], Discrete) + assert isinstance(space[2], MultiDiscrete) + space = spec_to_gym_space(Tuple((Box(-1, 1, shape=(1,)), Discrete(2)))) + assert isinstance(space, Tuple) + # Dict + space = spec_to_gym_space({"box": [1, 2, 3, 4, 5], "discrete": {2}, "multi_discrete": [{1}, {2}, {3}]}) + assert isinstance(space, Dict) + assert len(space) == 3 + assert isinstance(space["box"], Box) + assert isinstance(space["discrete"], Discrete) + assert isinstance(space["multi_discrete"], MultiDiscrete) + space = spec_to_gym_space(Dict({"box": Box(-1, 1, shape=(1,)), "discrete": Discrete(2)})) + assert isinstance(space, Dict) + + +def test_sample_space(): + """Test sampling from gym spaces.""" + device = "cpu" + # fundamental spaces + # Box + sample = sample_space(Box(low=-1.0, high=1.0, shape=(1, 2)), device, batch_size=1) + assert isinstance(sample, torch.Tensor) + _check_tensorized(sample, batch_size=1) + # Discrete + sample = sample_space(Discrete(2), device, batch_size=2) + assert isinstance(sample, torch.Tensor) + _check_tensorized(sample, batch_size=2) + # MultiDiscrete + sample = sample_space(MultiDiscrete(np.array([1, 2, 3])), device, batch_size=3) + assert isinstance(sample, torch.Tensor) + _check_tensorized(sample, batch_size=3) + # composite spaces + # Tuple + sample = sample_space(Tuple((Box(-1, 1, shape=(1,)), Discrete(2))), device, batch_size=4) + assert isinstance(sample, (tuple, list)) + _check_tensorized(sample, batch_size=4) + # Dict + sample = sample_space(Dict({"box": Box(-1, 1, shape=(1,)), "discrete": Discrete(2)}), device, batch_size=5) + assert isinstance(sample, dict) + _check_tensorized(sample, batch_size=5) + + +def test_space_serialization_deserialization(): + """Test serialization and deserialization of gym spaces.""" + # fundamental spaces + # Box + space = 1 + output = deserialize_space(serialize_space(space)) + assert space == output + space = [1, 2, 3, 4, 5] + output = deserialize_space(serialize_space(space)) + assert space == output + space = Box(low=-1.0, high=1.0, shape=(1, 2)) + output = deserialize_space(serialize_space(space)) + assert isinstance(output, Box) + assert (space.low == output.low).all() + assert (space.high == output.high).all() + assert space.shape == output.shape + # Discrete + space = {2} + output = deserialize_space(serialize_space(space)) + assert space == output + space = Discrete(2) + output = deserialize_space(serialize_space(space)) + assert isinstance(output, Discrete) + assert space.n == output.n + # MultiDiscrete + space = [{1}, {2}, {3}] + output = deserialize_space(serialize_space(space)) + assert space == output + space = MultiDiscrete(np.array([1, 2, 3])) + output = deserialize_space(serialize_space(space)) + assert isinstance(output, MultiDiscrete) + assert (space.nvec == output.nvec).all() + # composite spaces + # Tuple + space = ([1, 2, 3, 4, 5], {2}, [{1}, {2}, {3}]) + output = deserialize_space(serialize_space(space)) + assert space == output + space = Tuple((Box(-1, 1, shape=(1,)), Discrete(2))) + output = deserialize_space(serialize_space(space)) + assert isinstance(output, Tuple) + assert len(output) == 2 + assert isinstance(output[0], Box) + assert isinstance(output[1], Discrete) + # Dict + space = {"box": [1, 2, 3, 4, 5], "discrete": {2}, "multi_discrete": [{1}, {2}, {3}]} + output = deserialize_space(serialize_space(space)) + assert space == output + space = Dict({"box": Box(-1, 1, shape=(1,)), "discrete": Discrete(2)}) + output = deserialize_space(serialize_space(space)) + assert isinstance(output, Dict) + assert len(output) == 2 + assert isinstance(output["box"], Box) + assert isinstance(output["discrete"], Discrete) + + +def _check_tensorized(sample, batch_size): + """Helper function to check if a sample is properly tensorized.""" + if isinstance(sample, (tuple, list)): + list(map(_check_tensorized, sample, [batch_size] * len(sample))) + elif isinstance(sample, dict): + list(map(_check_tensorized, sample.values(), [batch_size] * len(sample))) + else: + assert isinstance(sample, torch.Tensor) + assert sample.shape[0] == batch_size diff --git a/source/isaaclab/test/envs/test_texture_randomization.py b/source/isaaclab/test/envs/test_texture_randomization.py index cab137571016..e2cbe7d54486 100644 --- a/source/isaaclab/test/envs/test_texture_randomization.py +++ b/source/isaaclab/test/envs/test_texture_randomization.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,7 +9,7 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -18,8 +18,11 @@ """Rest everything follows.""" import math + +import pytest import torch -import unittest + +import omni.usd import isaaclab.envs.mdp as mdp from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg @@ -64,10 +67,12 @@ def __post_init__(self) -> None: class EventCfg: """Configuration for events.""" - # on reset apply a new set of textures + # on prestartup apply a new set of textures + # note from @mayank: Changed from 'reset' to 'prestartup' to make test pass. + # The error happens otherwise on Kit thread which is not the main thread. cart_texture_randomizer = EventTerm( func=mdp.randomize_visual_texture_material, - mode="reset", + mode="prestartup", params={ "asset_cfg": SceneEntityCfg("robot", body_names=["cart"]), "texture_paths": [ @@ -83,6 +88,7 @@ class EventCfg: }, ) + # on reset apply a new set of textures pole_texture_randomizer = EventTerm( func=mdp.randomize_visual_texture_material, mode="reset", @@ -122,6 +128,36 @@ class EventCfg: ) +@configclass +class EventCfgFallback: + """Configuration for events that tests the fallback mechanism.""" + + # Test fallback when /visuals pattern doesn't match + test_fallback_texture_randomizer = EventTerm( + func=mdp.randomize_visual_texture_material, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=["slider"]), + "texture_paths": [ + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Bamboo_Planks/Bamboo_Planks_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Cherry/Cherry_BaseColor.png", + ], + "event_name": "test_fallback_texture_randomizer", + "texture_rotation": (0.0, 0.0), + }, + ) + + 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.1, 0.1), + }, + ) + + @configclass class CartpoleEnvCfg(ManagerBasedEnvCfg): """Configuration for the cartpole environment.""" @@ -145,45 +181,55 @@ def __post_init__(self): self.sim.dt = 0.005 # sim step every 5ms: 200Hz -class TestTextureRandomization(unittest.TestCase): - """Test for texture randomization""" +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_texture_randomization(device): + """Test texture randomization for cartpole environment.""" + # Create a new stage + omni.usd.get_context().new_stage() - """ - Tests - """ - - def test_texture_randomization(self): - # set the arguments + try: + # Set the arguments env_cfg = CartpoleEnvCfg() env_cfg.scene.num_envs = 16 env_cfg.scene.replicate_physics = False + env_cfg.sim.device = device - # setup base environment + # Setup base environment env = ManagerBasedEnv(cfg=env_cfg) - # simulate physics - with torch.inference_mode(): - for count in range(50): - # reset every few steps to check nothing breaks - if count % 10 == 0: - env.reset() - # sample random actions - joint_efforts = torch.randn_like(env.action_manager.action) - # step the environment - env.step(joint_efforts) - - env.close() - - def test_texture_randomization_failure_replicate_physics(self): - with self.assertRaises(ValueError): - cfg_failure = CartpoleEnvCfg() - cfg_failure.scene.num_envs = 16 - cfg_failure.scene.replicate_physics = True + try: + # Simulate physics + with torch.inference_mode(): + for count in range(50): + # Reset every few steps to check nothing breaks + if count % 10 == 0: + env.reset() + # Sample random actions + joint_efforts = torch.randn_like(env.action_manager.action) + # Step the environment + env.step(joint_efforts) + finally: + env.close() + finally: + # Clean up stage + omni.usd.get_context().close_stage() + + +def test_texture_randomization_failure_replicate_physics(): + """Test texture randomization failure when replicate physics is set to True.""" + # Create a new stage + omni.usd.get_context().new_stage() + + try: + # Set the arguments + cfg_failure = CartpoleEnvCfg() + cfg_failure.scene.num_envs = 16 + cfg_failure.scene.replicate_physics = True + + # Test that creating the environment raises RuntimeError + with pytest.raises(RuntimeError): env = ManagerBasedEnv(cfg_failure) - - env.close() - - -if __name__ == "__main__": - # run the main function - run_tests() + env.close() + finally: + # Clean up stage + omni.usd.get_context().close_stage() diff --git a/source/isaaclab/test/managers/test_event_manager.py b/source/isaaclab/test/managers/test_event_manager.py index d90ac5863997..171cc8be65e9 100644 --- a/source/isaaclab/test/managers/test_event_manager.py +++ b/source/isaaclab/test/managers/test_event_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,18 +8,23 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from collections.abc import Sequence + +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import torch -import unittest + from collections import namedtuple -from isaaclab.managers import EventManager, EventTermCfg +import pytest +import torch + +from isaaclab.envs import ManagerBasedEnv +from isaaclab.managers import EventManager, EventTermCfg, ManagerTermBase, ManagerTermBaseCfg from isaaclab.sim import SimulationContext from isaaclab.utils import configclass @@ -47,313 +52,356 @@ def increment_dummy2_by_one(env, env_ids: torch.Tensor): env.dummy2[env_ids] += 1 -class TestEventManager(unittest.TestCase): - """Test cases for various situations with event manager.""" - - def setUp(self) -> None: - # create values - num_envs = 32 - device = "cpu" - # create dummy tensors - dummy1 = torch.zeros((num_envs, 2), device=device) - dummy2 = torch.zeros((num_envs, 10), device=device) - # create sim - sim = SimulationContext() - # create dummy environment - self.env = DummyEnv(num_envs, 0.01, device, sim, dummy1, dummy2) - - def test_str(self): - """Test the string representation of the event manager.""" - cfg = { - "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), - "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), - "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}), - "term_4": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 2}), - } - self.event_man = EventManager(cfg, self.env) - - # print the expected string - print() - print(self.event_man) - - def test_config_equivalence(self): - """Test the equivalence of event manager created from different config types.""" - # create from dictionary - cfg = { - "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), - "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), - "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}), - } - event_man_from_dict = EventManager(cfg, self.env) - - # create from config class - @configclass - class MyEventManagerCfg: - """Event manager config with no type annotations.""" - - term_1 = EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)) - term_2 = EventTermCfg(func=reset_dummy1_to_zero, mode="reset") - term_3 = EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}) - - cfg = MyEventManagerCfg() - event_man_from_cfg = EventManager(cfg, self.env) - - # create from config class - @configclass - class MyEventManagerAnnotatedCfg: - """Event manager config with type annotations.""" - - term_1: EventTermCfg = EventTermCfg( - func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1) - ) - term_2: EventTermCfg = EventTermCfg(func=reset_dummy1_to_zero, mode="reset") - term_3: EventTermCfg = EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}) - - cfg = MyEventManagerAnnotatedCfg() - event_man_from_annotated_cfg = EventManager(cfg, self.env) - - # check equivalence - # parsed terms - self.assertDictEqual(event_man_from_dict.active_terms, event_man_from_annotated_cfg.active_terms) - self.assertDictEqual(event_man_from_cfg.active_terms, event_man_from_annotated_cfg.active_terms) - self.assertDictEqual(event_man_from_dict.active_terms, event_man_from_cfg.active_terms) - # parsed term configs - self.assertDictEqual(event_man_from_dict._mode_term_cfgs, event_man_from_annotated_cfg._mode_term_cfgs) - self.assertDictEqual(event_man_from_cfg._mode_term_cfgs, event_man_from_annotated_cfg._mode_term_cfgs) - self.assertDictEqual(event_man_from_dict._mode_term_cfgs, event_man_from_cfg._mode_term_cfgs) - - def test_active_terms(self): - """Test the correct reading of active terms.""" - cfg = { - "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), - "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), - "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}), - "term_4": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 2}), - } - self.event_man = EventManager(cfg, self.env) - - self.assertEqual(len(self.event_man.active_terms), 3) - self.assertEqual(len(self.event_man.active_terms["interval"]), 1) - self.assertEqual(len(self.event_man.active_terms["reset"]), 1) - self.assertEqual(len(self.event_man.active_terms["custom"]), 2) - - def test_config_empty(self): - """Test the creation of reward manager with empty config.""" - self.event_man = EventManager(None, self.env) - self.assertEqual(len(self.event_man.active_terms), 0) - - # print the expected string - print() - print(self.event_man) - - def test_invalid_event_func_module(self): - """Test the handling of invalid event function's module in string representation.""" - cfg = { - "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), - "term_2": EventTermCfg(func="a:reset_dummy1_to_zero", mode="reset"), - } - with self.assertRaises(ValueError): - self.event_man = EventManager(cfg, self.env) - - def test_invalid_event_config(self): - """Test the handling of invalid event function's config parameters.""" - cfg = { - "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), - "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), - "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom"), - } - with self.assertRaises(ValueError): - self.event_man = EventManager(cfg, self.env) - - def test_apply_interval_mode_without_global_time(self): - """Test the application of event terms that are in interval mode without global time. - - During local time, each environment instance has its own time for the interval term. - """ - # make two intervals -- one is fixed and the other is random - term_1_interval_range_s = (10 * self.env.dt, 10 * self.env.dt) - term_2_interval_range_s = (2 * self.env.dt, 10 * self.env.dt) - - cfg = { - "term_1": EventTermCfg( - func=increment_dummy1_by_one, - mode="interval", - interval_range_s=term_1_interval_range_s, - is_global_time=False, - ), - "term_2": EventTermCfg( - func=increment_dummy2_by_one, - mode="interval", - interval_range_s=term_2_interval_range_s, - is_global_time=False, - ), - } - - self.event_man = EventManager(cfg, self.env) - - # obtain the initial time left for the interval terms - term_2_interval_time = self.event_man._interval_term_time_left[1].clone() - expected_dummy2_value = torch.zeros_like(self.env.dummy2) - - for count in range(50): - # apply the event terms - self.event_man.apply("interval", dt=self.env.dt) - # manually decrement the interval time for term2 since it is randomly sampled - term_2_interval_time -= self.env.dt - - # check the values - # we increment the dummy1 by 1 every 10 steps. at the 9th count (aka 10th apply), the value should be 1 - torch.testing.assert_close(self.env.dummy1, (count + 1) // 10 * torch.ones_like(self.env.dummy1)) - - # we increment the dummy2 by 1 every 2 to 10 steps based on the random interval - expected_dummy2_value += term_2_interval_time.unsqueeze(1) < 1e-6 - torch.testing.assert_close(self.env.dummy2, expected_dummy2_value) - - # check the time sampled at the end of the interval is valid - # -- fixed interval - if (count + 1) % 10 == 0: - term_1_interval_time_init = self.event_man._interval_term_time_left[0].clone() - expected_time_interval_init = torch.full_like(term_1_interval_time_init, term_1_interval_range_s[1]) - torch.testing.assert_close(term_1_interval_time_init, expected_time_interval_init) - # -- random interval - env_ids = (term_2_interval_time < 1e-6).nonzero(as_tuple=True)[0] - if len(env_ids) > 0: - term_2_interval_time[env_ids] = self.event_man._interval_term_time_left[1][env_ids] - - def test_apply_interval_mode_with_global_time(self): - """Test the application of event terms that are in interval mode with global time. - - During global time, all the environment instances share the same time for the interval term. - """ - # make two intervals -- one is fixed and the other is random - term_1_interval_range_s = (10 * self.env.dt, 10 * self.env.dt) - term_2_interval_range_s = (2 * self.env.dt, 10 * self.env.dt) - - cfg = { - "term_1": EventTermCfg( - func=increment_dummy1_by_one, - mode="interval", - interval_range_s=term_1_interval_range_s, - is_global_time=True, - ), - "term_2": EventTermCfg( - func=increment_dummy2_by_one, - mode="interval", - interval_range_s=term_2_interval_range_s, - is_global_time=True, - ), - } - - self.event_man = EventManager(cfg, self.env) - - # obtain the initial time left for the interval terms - term_2_interval_time = self.event_man._interval_term_time_left[1].clone() - expected_dummy2_value = torch.zeros_like(self.env.dummy2) - - for count in range(50): - # apply the event terms - self.event_man.apply("interval", dt=self.env.dt) - # manually decrement the interval time for term2 since it is randomly sampled - term_2_interval_time -= self.env.dt - - # check the values - # we increment the dummy1 by 1 every 10 steps. at the 9th count (aka 10th apply), the value should be 1 - torch.testing.assert_close(self.env.dummy1, (count + 1) // 10 * torch.ones_like(self.env.dummy1)) - - # we increment the dummy2 by 1 every 2 to 10 steps based on the random interval - expected_dummy2_value += term_2_interval_time < 1e-6 - torch.testing.assert_close(self.env.dummy2, expected_dummy2_value) - - # check the time sampled at the end of the interval is valid - # -- fixed interval - if (count + 1) % 10 == 0: - term_1_interval_time_init = self.event_man._interval_term_time_left[0].clone() - expected_time_interval_init = torch.full_like(term_1_interval_time_init, term_1_interval_range_s[1]) - torch.testing.assert_close(term_1_interval_time_init, expected_time_interval_init) - # -- random interval - if term_2_interval_time < 1e-6: - term_2_interval_time = self.event_man._interval_term_time_left[1].clone() - - def test_apply_reset_mode(self): - """Test the application of event terms that are in reset mode.""" - cfg = { - "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="reset"), - "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset", min_step_count_between_reset=10), - } - - self.event_man = EventManager(cfg, self.env) - - # manually keep track of the expected values for dummy1 and trigger count - expected_dummy1_value = torch.zeros_like(self.env.dummy1) - term_2_trigger_step_id = torch.zeros((self.env.num_envs,), dtype=torch.int32, device=self.env.device) - - for count in range(50): - # apply the event terms for all the env ids - if count % 3 == 0: - self.event_man.apply("reset", global_env_step_count=count) - - # we increment the dummy1 by 1 every call to reset mode due to term 1 - expected_dummy1_value[:] += 1 - # manually update the expected value for term 2 - if (count - term_2_trigger_step_id[0]) >= 10 or count == 0: - expected_dummy1_value = torch.zeros_like(self.env.dummy1) - term_2_trigger_step_id[:] = count - - # check the values of trigger count - # -- term 1 - expected_trigger_count = torch.full( - (self.env.num_envs,), 3 * (count // 3), dtype=torch.int32, device=self.env.device - ) - torch.testing.assert_close(self.event_man._reset_term_last_triggered_step_id[0], expected_trigger_count) - # -- term 2 - torch.testing.assert_close(self.event_man._reset_term_last_triggered_step_id[1], term_2_trigger_step_id) - - # check the values of dummy1 - torch.testing.assert_close(self.env.dummy1, expected_dummy1_value) - - def test_apply_reset_mode_subset_env_ids(self): - """Test the application of event terms that are in reset mode over a subset of environment ids.""" - cfg = { - "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="reset"), - "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset", min_step_count_between_reset=10), - } - - self.event_man = EventManager(cfg, self.env) - - # since we are applying the event terms over a subset of env ids, we need to keep track of the trigger count - # manually for the sake of testing - term_2_trigger_step_id = torch.zeros((self.env.num_envs,), dtype=torch.int32, device=self.env.device) - term_2_trigger_once = torch.zeros((self.env.num_envs,), dtype=torch.bool, device=self.env.device) - expected_dummy1_value = torch.zeros_like(self.env.dummy1) - - for count in range(50): - # randomly select a subset of environment ids - env_ids = (torch.rand(self.env.num_envs, device=self.env.device) < 0.5).nonzero().flatten() - # apply the event terms for the selected env ids - self.event_man.apply("reset", env_ids=env_ids, global_env_step_count=count) - - # modify the trigger count for term 2 - trigger_ids = (count - term_2_trigger_step_id[env_ids]) >= 10 - trigger_ids |= (term_2_trigger_step_id[env_ids] == 0) & ~term_2_trigger_once[env_ids] - term_2_trigger_step_id[env_ids[trigger_ids]] = count - term_2_trigger_once[env_ids[trigger_ids]] = True - # we increment the dummy1 by 1 every call to reset mode - # every 10th call, we reset the dummy1 to 0 - expected_dummy1_value[env_ids] += 1 # effect of term 1 - expected_dummy1_value[env_ids[trigger_ids]] = 0 # effect of term 2 - - # check the values of trigger count - # -- term 1 - expected_trigger_count = torch.full((len(env_ids),), count, dtype=torch.int32, device=self.env.device) - torch.testing.assert_close( - self.event_man._reset_term_last_triggered_step_id[0][env_ids], expected_trigger_count - ) - # -- term 2 - torch.testing.assert_close(self.event_man._reset_term_last_triggered_step_id[1], term_2_trigger_step_id) - - # check the values of dummy1 - torch.testing.assert_close(self.env.dummy1, expected_dummy1_value) - - -if __name__ == "__main__": - run_tests() +class reset_dummy2_to_zero_class(ManagerTermBase): + def __init__(self, cfg: ManagerTermBaseCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + pass + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + ) -> None: + env.dummy2[env_ids] = 0 + + +class increment_dummy2_by_one_class(ManagerTermBase): + def __init__(self, cfg: ManagerTermBaseCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + pass + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + ) -> None: + env.dummy2[env_ids] += 1 + + +@pytest.fixture +def env(): + num_envs = 32 + device = "cpu" + # create dummy tensors + dummy1 = torch.zeros((num_envs, 2), device=device) + dummy2 = torch.zeros((num_envs, 10), device=device) + # create sim + sim = SimulationContext() + # create dummy environment + return DummyEnv(num_envs, 0.01, device, sim, dummy1, dummy2) + + +def test_str(env): + """Test the string representation of the event manager.""" + cfg = { + "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), + "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), + "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}), + "term_4": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 2}), + } + event_man = EventManager(cfg, env) + + # print the expected string + print() + print(event_man) + + +def test_config_equivalence(env): + """Test the equivalence of event manager created from different config types.""" + # create from dictionary + cfg = { + "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), + "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), + "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}), + } + event_man_from_dict = EventManager(cfg, env) + + # create from config class + @configclass + class MyEventManagerCfg: + """Event manager config with no type annotations.""" + + term_1 = EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)) + term_2 = EventTermCfg(func=reset_dummy1_to_zero, mode="reset") + term_3 = EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}) + + cfg = MyEventManagerCfg() + event_man_from_cfg = EventManager(cfg, env) + + # create from config class + @configclass + class MyEventManagerAnnotatedCfg: + """Event manager config with type annotations.""" + + term_1: EventTermCfg = EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)) + term_2: EventTermCfg = EventTermCfg(func=reset_dummy1_to_zero, mode="reset") + term_3: EventTermCfg = EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}) + + cfg = MyEventManagerAnnotatedCfg() + event_man_from_annotated_cfg = EventManager(cfg, env) + + # check equivalence + # parsed terms + assert event_man_from_dict.active_terms == event_man_from_annotated_cfg.active_terms + assert event_man_from_cfg.active_terms == event_man_from_annotated_cfg.active_terms + assert event_man_from_dict.active_terms == event_man_from_cfg.active_terms + # parsed term configs + assert event_man_from_dict._mode_term_cfgs == event_man_from_annotated_cfg._mode_term_cfgs + assert event_man_from_cfg._mode_term_cfgs == event_man_from_annotated_cfg._mode_term_cfgs + assert event_man_from_dict._mode_term_cfgs == event_man_from_cfg._mode_term_cfgs + + +def test_active_terms(env): + """Test the correct reading of active terms.""" + cfg = { + "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), + "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), + "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 10}), + "term_4": EventTermCfg(func=change_dummy1_by_value, mode="custom", params={"value": 2}), + } + event_man = EventManager(cfg, env) + + assert len(event_man.active_terms) == 3 + assert len(event_man.active_terms["interval"]) == 1 + assert len(event_man.active_terms["reset"]) == 1 + assert len(event_man.active_terms["custom"]) == 2 + + +def test_class_terms(env): + """Test the correct preparation of function and class event terms.""" + cfg = { + "term_1": EventTermCfg(func=reset_dummy2_to_zero, mode="reset"), + "term_2": EventTermCfg(func=increment_dummy2_by_one_class, mode="interval", interval_range_s=(0.1, 0.1)), + "term_3": EventTermCfg(func=reset_dummy2_to_zero_class, mode="reset"), + } + + event_man = EventManager(cfg, env) + assert len(event_man.active_terms) == 2 + assert len(event_man.active_terms["interval"]) == 1 + assert len(event_man.active_terms["reset"]) == 2 + assert len(event_man._mode_class_term_cfgs) == 2 + assert len(event_man._mode_class_term_cfgs["reset"]) == 1 + + +def test_config_empty(env): + """Test the creation of reward manager with empty config.""" + event_man = EventManager(None, env) + assert len(event_man.active_terms) == 0 + + # print the expected string + print() + print(event_man) + + +def test_invalid_event_func_module(env): + """Test the handling of invalid event function's module in string representation.""" + cfg = { + "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), + "term_2": EventTermCfg(func="a:reset_dummy1_to_zero", mode="reset"), + } + with pytest.raises(ValueError): + EventManager(cfg, env) + + +def test_invalid_event_config(env): + """Test the handling of invalid event function's config parameters.""" + cfg = { + "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="interval", interval_range_s=(0.1, 0.1)), + "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset"), + "term_3": EventTermCfg(func=change_dummy1_by_value, mode="custom"), + } + with pytest.raises(ValueError): + EventManager(cfg, env) + + +def test_apply_interval_mode_without_global_time(env): + """Test the application of event terms that are in interval mode without global time. + + During local time, each environment instance has its own time for the interval term. + """ + # make two intervals -- one is fixed and the other is random + term_1_interval_range_s = (10 * env.dt, 10 * env.dt) + term_2_interval_range_s = (2 * env.dt, 10 * env.dt) + + cfg = { + "term_1": EventTermCfg( + func=increment_dummy1_by_one, + mode="interval", + interval_range_s=term_1_interval_range_s, + is_global_time=False, + ), + "term_2": EventTermCfg( + func=increment_dummy2_by_one, + mode="interval", + interval_range_s=term_2_interval_range_s, + is_global_time=False, + ), + } + + event_man = EventManager(cfg, env) + + # obtain the initial time left for the interval terms + term_2_interval_time = event_man._interval_term_time_left[1].clone() + expected_dummy2_value = torch.zeros_like(env.dummy2) + + for count in range(50): + # apply the event terms + event_man.apply("interval", dt=env.dt) + # manually decrement the interval time for term2 since it is randomly sampled + term_2_interval_time -= env.dt + + # check the values + # we increment the dummy1 by 1 every 10 steps. at the 9th count (aka 10th apply), the value should be 1 + torch.testing.assert_close(env.dummy1, (count + 1) // 10 * torch.ones_like(env.dummy1)) + + # we increment the dummy2 by 1 every 2 to 10 steps based on the random interval + expected_dummy2_value += term_2_interval_time.unsqueeze(1) < 1e-6 + torch.testing.assert_close(env.dummy2, expected_dummy2_value) + + # check the time sampled at the end of the interval is valid + # -- fixed interval + if (count + 1) % 10 == 0: + term_1_interval_time_init = event_man._interval_term_time_left[0].clone() + expected_time_interval_init = torch.full_like(term_1_interval_time_init, term_1_interval_range_s[1]) + torch.testing.assert_close(term_1_interval_time_init, expected_time_interval_init) + # -- random interval + env_ids = (term_2_interval_time < 1e-6).nonzero(as_tuple=True)[0] + if len(env_ids) > 0: + term_2_interval_time[env_ids] = event_man._interval_term_time_left[1][env_ids] + + +def test_apply_interval_mode_with_global_time(env): + """Test the application of event terms that are in interval mode with global time. + + During global time, all the environment instances share the same time for the interval term. + """ + # make two intervals -- one is fixed and the other is random + term_1_interval_range_s = (10 * env.dt, 10 * env.dt) + term_2_interval_range_s = (2 * env.dt, 10 * env.dt) + + cfg = { + "term_1": EventTermCfg( + func=increment_dummy1_by_one, + mode="interval", + interval_range_s=term_1_interval_range_s, + is_global_time=True, + ), + "term_2": EventTermCfg( + func=increment_dummy2_by_one, + mode="interval", + interval_range_s=term_2_interval_range_s, + is_global_time=True, + ), + } + + event_man = EventManager(cfg, env) + + # obtain the initial time left for the interval terms + term_2_interval_time = event_man._interval_term_time_left[1].clone() + expected_dummy2_value = torch.zeros_like(env.dummy2) + + for count in range(50): + # apply the event terms + event_man.apply("interval", dt=env.dt) + # manually decrement the interval time for term2 since it is randomly sampled + term_2_interval_time -= env.dt + + # check the values + # we increment the dummy1 by 1 every 10 steps. at the 9th count (aka 10th apply), the value should be 1 + torch.testing.assert_close(env.dummy1, (count + 1) // 10 * torch.ones_like(env.dummy1)) + + # we increment the dummy2 by 1 every 2 to 10 steps based on the random interval + expected_dummy2_value += term_2_interval_time < 1e-6 + torch.testing.assert_close(env.dummy2, expected_dummy2_value) + + # check the time sampled at the end of the interval is valid + # -- fixed interval + if (count + 1) % 10 == 0: + term_1_interval_time_init = event_man._interval_term_time_left[0].clone() + expected_time_interval_init = torch.full_like(term_1_interval_time_init, term_1_interval_range_s[1]) + torch.testing.assert_close(term_1_interval_time_init, expected_time_interval_init) + # -- random interval + if term_2_interval_time < 1e-6: + term_2_interval_time = event_man._interval_term_time_left[1].clone() + + +def test_apply_reset_mode(env): + """Test the application of event terms that are in reset mode.""" + cfg = { + "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="reset"), + "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset", min_step_count_between_reset=10), + } + + event_man = EventManager(cfg, env) + + # manually keep track of the expected values for dummy1 and trigger count + expected_dummy1_value = torch.zeros_like(env.dummy1) + term_2_trigger_step_id = torch.zeros((env.num_envs,), dtype=torch.int32, device=env.device) + + for count in range(50): + # apply the event terms for all the env ids + if count % 3 == 0: + event_man.apply("reset", global_env_step_count=count) + + # we increment the dummy1 by 1 every call to reset mode due to term 1 + expected_dummy1_value[:] += 1 + # manually update the expected value for term 2 + if (count - term_2_trigger_step_id[0]) >= 10 or count == 0: + expected_dummy1_value = torch.zeros_like(env.dummy1) + term_2_trigger_step_id[:] = count + + # check the values of trigger count + # -- term 1 + expected_trigger_count = torch.full((env.num_envs,), 3 * (count // 3), dtype=torch.int32, device=env.device) + torch.testing.assert_close(event_man._reset_term_last_triggered_step_id[0], expected_trigger_count) + # -- term 2 + torch.testing.assert_close(event_man._reset_term_last_triggered_step_id[1], term_2_trigger_step_id) + + # check the values of dummy1 + torch.testing.assert_close(env.dummy1, expected_dummy1_value) + + +def test_apply_reset_mode_subset_env_ids(env): + """Test the application of event terms that are in reset mode over a subset of environment ids.""" + cfg = { + "term_1": EventTermCfg(func=increment_dummy1_by_one, mode="reset"), + "term_2": EventTermCfg(func=reset_dummy1_to_zero, mode="reset", min_step_count_between_reset=10), + } + + event_man = EventManager(cfg, env) + + # since we are applying the event terms over a subset of env ids, we need to keep track of the trigger count + # manually for the sake of testing + term_2_trigger_step_id = torch.zeros((env.num_envs,), dtype=torch.int32, device=env.device) + term_2_trigger_once = torch.zeros((env.num_envs,), dtype=torch.bool, device=env.device) + expected_dummy1_value = torch.zeros_like(env.dummy1) + + for count in range(50): + # randomly select a subset of environment ids + env_ids = (torch.rand(env.num_envs, device=env.device) < 0.5).nonzero().flatten() + # apply the event terms for the selected env ids + event_man.apply("reset", env_ids=env_ids, global_env_step_count=count) + + # modify the trigger count for term 2 + trigger_ids = (count - term_2_trigger_step_id[env_ids]) >= 10 + trigger_ids |= (term_2_trigger_step_id[env_ids] == 0) & ~term_2_trigger_once[env_ids] + term_2_trigger_step_id[env_ids[trigger_ids]] = count + term_2_trigger_once[env_ids[trigger_ids]] = True + # we increment the dummy1 by 1 every call to reset mode + # every 10th call, we reset the dummy1 to 0 + expected_dummy1_value[env_ids] += 1 # effect of term 1 + expected_dummy1_value[env_ids[trigger_ids]] = 0 # effect of term 2 + + # check the values of trigger count + # -- term 1 + expected_trigger_count = torch.full((len(env_ids),), count, dtype=torch.int32, device=env.device) + torch.testing.assert_close(event_man._reset_term_last_triggered_step_id[0][env_ids], expected_trigger_count) + # -- term 2 + torch.testing.assert_close(event_man._reset_term_last_triggered_step_id[1], term_2_trigger_step_id) + + # check the values of dummy1 + torch.testing.assert_close(env.dummy1, expected_dummy1_value) diff --git a/source/isaaclab/test/managers/test_observation_manager.py b/source/isaaclab/test/managers/test_observation_manager.py index 2961c4edd2c7..d738f179da71 100644 --- a/source/isaaclab/test/managers/test_observation_manager.py +++ b/source/isaaclab/test/managers/test_observation_manager.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,21 +8,32 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import torch -import unittest from collections import namedtuple +from typing import TYPE_CHECKING + +import pytest +import torch -from isaaclab.managers import ManagerTermBase, ObservationGroupCfg, ObservationManager, ObservationTermCfg -from isaaclab.sim import SimulationContext +import isaaclab.sim as sim_utils +from isaaclab.managers import ( + ManagerTermBase, + ObservationGroupCfg, + ObservationManager, + ObservationTermCfg, + RewardTermCfg, +) from isaaclab.utils import configclass, modifiers +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + def grilled_chicken(env): return torch.ones(env.num_envs, 4, device=env.device) @@ -77,7 +88,6 @@ def call_me(self, env: object) -> torch.Tensor: class MyDataClass: - def __init__(self, num_envs: int, device: str): self.pos_w = torch.rand((num_envs, 3), device=device) self.lin_vel_w = torch.rand((num_envs, 3), device=device) @@ -91,572 +101,696 @@ def lin_vel_w_data(env) -> torch.Tensor: return env.data.lin_vel_w -class TestObservationManager(unittest.TestCase): - """Test cases for various situations with observation manager.""" - - def setUp(self) -> None: - # set up the environment - self.dt = 0.01 - self.num_envs = 20 - self.device = "cuda:0" - # set up sim - self.sim = SimulationContext() - # create dummy environment - self.env = namedtuple("ManagerBasedEnv", ["num_envs", "device", "data", "dt", "sim"])( - self.num_envs, self.device, MyDataClass(self.num_envs, self.device), self.dt, self.sim - ) - - def test_str(self): - """Test the string representation of the observation manager.""" +@pytest.fixture(autouse=True) +def setup_env(): + dt = 0.01 + num_envs = 20 + device = "cuda:0" + # set up sim + sim_cfg = sim_utils.SimulationCfg(dt=dt, device=device) + sim = sim_utils.SimulationContext(sim_cfg) + # create dummy environment + env = namedtuple("ManagerBasedEnv", ["num_envs", "device", "data", "dt", "sim"])( + num_envs, device, MyDataClass(num_envs, device), dt, sim + ) + # let the simulation play (we need this for observation manager to compute obs dims) + env.sim._app_control_on_stop_handle = None + env.sim.reset() + return env + + +def test_str(setup_env): + env = setup_env + """Test the string representation of the observation manager.""" + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" - - @configclass - class SampleGroupCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - term_1 = ObservationTermCfg(func="__main__:grilled_chicken", scale=10) - term_2 = ObservationTermCfg(func=grilled_chicken, scale=2) - term_3 = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=5, params={"bbq": True}) - term_4 = ObservationTermCfg( - func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} - ) - term_5 = ObservationTermCfg( - func=grilled_chicken_with_yoghurt_and_bbq, scale=1.0, params={"hot": False, "bland": 2.0} - ) - - policy: ObservationGroupCfg = SampleGroupCfg() - - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - self.assertEqual(len(self.obs_man.active_terms["policy"]), 5) - # print the expected string - obs_man_str = str(self.obs_man) - print() - print(obs_man_str) - obs_man_str_split = obs_man_str.split("|") - term_1_str_index = obs_man_str_split.index(" term_1 ") - term_1_str_shape = obs_man_str_split[term_1_str_index + 1].strip() - self.assertEqual(term_1_str_shape, "(4,)") - - def test_str_with_history(self): - """Test the string representation of the observation manager with history terms.""" - - TERM_1_HISTORY = 5 + class SampleGroupCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) + term_2 = ObservationTermCfg(func=grilled_chicken, scale=2) + term_3 = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=5, params={"bbq": True}) + term_4 = ObservationTermCfg( + func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} + ) + term_5 = ObservationTermCfg( + func=grilled_chicken_with_yoghurt_and_bbq, scale=1.0, params={"hot": False, "bland": 2.0} + ) + + policy: ObservationGroupCfg = SampleGroupCfg() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + assert len(obs_man.active_terms["policy"]) == 5 + # print the expected string + obs_man_str = str(obs_man) + print() + print(obs_man_str) + obs_man_str_split = obs_man_str.split("|") + term_1_str_index = obs_man_str_split.index(" term_1 ") + term_1_str_shape = obs_man_str_split[term_1_str_index + 1].strip() + assert term_1_str_shape == "(4,)" + + +def test_str_with_history(setup_env): + env = setup_env + """Test the string representation of the observation manager with history terms.""" + + TERM_1_HISTORY = 5 + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" - - @configclass - class SampleGroupCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - term_1 = ObservationTermCfg(func="__main__:grilled_chicken", scale=10, history_length=TERM_1_HISTORY) - term_2 = ObservationTermCfg(func=grilled_chicken, scale=2) - term_3 = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=5, params={"bbq": True}) - term_4 = ObservationTermCfg( - func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} - ) - term_5 = ObservationTermCfg( - func=grilled_chicken_with_yoghurt_and_bbq, scale=1.0, params={"hot": False, "bland": 2.0} - ) - - policy: ObservationGroupCfg = SampleGroupCfg() - - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - self.assertEqual(len(self.obs_man.active_terms["policy"]), 5) - # print the expected string - obs_man_str = str(self.obs_man) - print() - print(obs_man_str) - obs_man_str_split = obs_man_str.split("|") - term_1_str_index = obs_man_str_split.index(" term_1 ") - term_1_str_shape = obs_man_str_split[term_1_str_index + 1].strip() - self.assertEqual(term_1_str_shape, "(20,)") - - def test_config_equivalence(self): - """Test the equivalence of observation manager created from different config types.""" - - # create from config class + class SampleGroupCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg(func=grilled_chicken, scale=10, history_length=TERM_1_HISTORY) + term_2 = ObservationTermCfg(func=grilled_chicken, scale=2) + term_3 = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=5, params={"bbq": True}) + term_4 = ObservationTermCfg( + func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} + ) + term_5 = ObservationTermCfg( + func=grilled_chicken_with_yoghurt_and_bbq, scale=1.0, params={"hot": False, "bland": 2.0} + ) + + policy: ObservationGroupCfg = SampleGroupCfg() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + assert len(obs_man.active_terms["policy"]) == 5 + # print the expected string + obs_man_str = str(obs_man) + print() + print(obs_man_str) + obs_man_str_split = obs_man_str.split("|") + term_1_str_index = obs_man_str_split.index(" term_1 ") + term_1_str_shape = obs_man_str_split[term_1_str_index + 1].strip() + assert term_1_str_shape == "(20,)" + + +def test_config_equivalence(setup_env): + env = setup_env + """Test the equivalence of observation manager created from different config types.""" + + # create from config class + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" + @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" + class SampleGroupCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + your_term = ObservationTermCfg(func=grilled_chicken, scale=10) + his_term = ObservationTermCfg(func=grilled_chicken, scale=2) + my_term = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=5, params={"bbq": True}) + her_term = ObservationTermCfg( + func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} + ) - @configclass - class SampleGroupCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" + policy = SampleGroupCfg() + critic = SampleGroupCfg(concatenate_terms=False, her_term=None) - your_term = ObservationTermCfg(func="__main__:grilled_chicken", scale=10) - his_term = ObservationTermCfg(func=grilled_chicken, scale=2) - my_term = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=5, params={"bbq": True}) - her_term = ObservationTermCfg( - func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} - ) + cfg = MyObservationManagerCfg() + obs_man_from_cfg = ObservationManager(cfg, env) - policy = SampleGroupCfg() - critic = SampleGroupCfg(concatenate_terms=False, her_term=None) + # create from config class + @configclass + class MyObservationManagerAnnotatedCfg: + """Test config class for observation manager with annotations on terms.""" - cfg = MyObservationManagerCfg() - obs_man_from_cfg = ObservationManager(cfg, self.env) + @configclass + class SampleGroupCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + your_term: ObservationTermCfg = ObservationTermCfg(func=grilled_chicken, scale=10) + his_term: ObservationTermCfg = ObservationTermCfg(func=grilled_chicken, scale=2) + my_term: ObservationTermCfg = ObservationTermCfg( + func=grilled_chicken_with_bbq, scale=5, params={"bbq": True} + ) + her_term: ObservationTermCfg = ObservationTermCfg( + func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} + ) + + policy: ObservationGroupCfg = SampleGroupCfg() + critic: ObservationGroupCfg = SampleGroupCfg(concatenate_terms=False, her_term=None) + + cfg = MyObservationManagerAnnotatedCfg() + obs_man_from_annotated_cfg = ObservationManager(cfg, env) + + # check equivalence + # parsed terms + assert obs_man_from_cfg.active_terms == obs_man_from_annotated_cfg.active_terms + assert obs_man_from_cfg.group_obs_term_dim == obs_man_from_annotated_cfg.group_obs_term_dim + assert obs_man_from_cfg.group_obs_dim == obs_man_from_annotated_cfg.group_obs_dim + # parsed term configs + assert obs_man_from_cfg._group_obs_term_cfgs == obs_man_from_annotated_cfg._group_obs_term_cfgs + assert obs_man_from_cfg._group_obs_concatenate == obs_man_from_annotated_cfg._group_obs_concatenate + + +def test_config_terms(setup_env): + env = setup_env + """Test the number of terms in the observation manager.""" + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" - # create from config class @configclass - class MyObservationManagerAnnotatedCfg: - """Test config class for observation manager with annotations on terms.""" - - @configclass - class SampleGroupCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - your_term: ObservationTermCfg = ObservationTermCfg(func="__main__:grilled_chicken", scale=10) - his_term: ObservationTermCfg = ObservationTermCfg(func=grilled_chicken, scale=2) - my_term: ObservationTermCfg = ObservationTermCfg( - func=grilled_chicken_with_bbq, scale=5, params={"bbq": True} - ) - her_term: ObservationTermCfg = ObservationTermCfg( - func=grilled_chicken_with_yoghurt, scale=1.0, params={"hot": False, "bland": 2.0} - ) - - policy: ObservationGroupCfg = SampleGroupCfg() - critic: ObservationGroupCfg = SampleGroupCfg(concatenate_terms=False, her_term=None) - - cfg = MyObservationManagerAnnotatedCfg() - obs_man_from_annotated_cfg = ObservationManager(cfg, self.env) - - # check equivalence - # parsed terms - self.assertEqual(obs_man_from_cfg.active_terms, obs_man_from_annotated_cfg.active_terms) - self.assertEqual(obs_man_from_cfg.group_obs_term_dim, obs_man_from_annotated_cfg.group_obs_term_dim) - self.assertEqual(obs_man_from_cfg.group_obs_dim, obs_man_from_annotated_cfg.group_obs_dim) - # parsed term configs - self.assertEqual(obs_man_from_cfg._group_obs_term_cfgs, obs_man_from_annotated_cfg._group_obs_term_cfgs) - self.assertEqual(obs_man_from_cfg._group_obs_concatenate, obs_man_from_annotated_cfg._group_obs_concatenate) - - def test_config_terms(self): - """Test the number of terms in the observation manager.""" + class SampleGroupCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) + term_2 = ObservationTermCfg(func=grilled_chicken_with_curry, scale=0.0, params={"hot": False}) @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" + class SampleMixedGroupCfg(ObservationGroupCfg): + """Test config class for policy observation group with a mix of vector and matrix terms.""" + + concatenate_terms = False + term_1 = ObservationTermCfg(func=grilled_chicken, scale=2.0) + term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=1.5, params={"bland": 0.5}) - @configclass - class SampleGroupCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" + @configclass + class SampleImageGroupCfg(ObservationGroupCfg): + term_1 = ObservationTermCfg(func=grilled_chicken_image, scale=1.5, params={"bland": 0.5, "channel": 1}) + term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=0.5, params={"bland": 0.1, "channel": 3}) - term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) - term_2 = ObservationTermCfg(func=grilled_chicken_with_curry, scale=0.0, params={"hot": False}) + policy: ObservationGroupCfg = SampleGroupCfg() + critic: ObservationGroupCfg = SampleGroupCfg(term_2=None) + mixed: ObservationGroupCfg = SampleMixedGroupCfg() + image: ObservationGroupCfg = SampleImageGroupCfg() - @configclass - class SampleMixedGroupCfg(ObservationGroupCfg): - """Test config class for policy observation group with a mix of vector and matrix terms.""" + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) - concatenate_terms = False - term_1 = ObservationTermCfg(func=grilled_chicken, scale=2.0) - term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=1.5, params={"bland": 0.5}) + assert len(obs_man.active_terms["policy"]) == 2 + assert len(obs_man.active_terms["critic"]) == 1 + assert len(obs_man.active_terms["mixed"]) == 2 + assert len(obs_man.active_terms["image"]) == 2 - @configclass - class SampleImageGroupCfg(ObservationGroupCfg): + # create a new obs manager but where mixed group has invalid config + cfg = MyObservationManagerCfg() + cfg.mixed.concatenate_terms = True - term_1 = ObservationTermCfg(func=grilled_chicken_image, scale=1.5, params={"bland": 0.5, "channel": 1}) - term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=0.5, params={"bland": 0.1, "channel": 3}) + with pytest.raises(RuntimeError): + ObservationManager(cfg, env) - policy: ObservationGroupCfg = SampleGroupCfg() - critic: ObservationGroupCfg = SampleGroupCfg(term_2=None) - mixed: ObservationGroupCfg = SampleMixedGroupCfg() - image: ObservationGroupCfg = SampleImageGroupCfg() - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) +def test_compute(setup_env): + env = setup_env + """Test the observation computation.""" - self.assertEqual(len(self.obs_man.active_terms["policy"]), 2) - self.assertEqual(len(self.obs_man.active_terms["critic"]), 1) - self.assertEqual(len(self.obs_man.active_terms["mixed"]), 2) - self.assertEqual(len(self.obs_man.active_terms["image"]), 2) + pos_scale_tuple = (2.0, 3.0, 1.0) - # create a new obs manager but where mixed group has invalid config - cfg = MyObservationManagerCfg() - cfg.mixed.concatenate_terms = True + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" - with self.assertRaises(RuntimeError): - ObservationManager(cfg, self.env) + @configclass + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" - def test_compute(self): - """Test the observation computation.""" + term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) + term_2 = ObservationTermCfg(func=grilled_chicken_with_curry, scale=0.0, params={"hot": False}) + term_3 = ObservationTermCfg(func=pos_w_data, scale=pos_scale_tuple) + term_4 = ObservationTermCfg(func=lin_vel_w_data, scale=1.5) - pos_scale_tuple = (2.0, 3.0, 1.0) + @configclass + class CriticCfg(ObservationGroupCfg): + term_1 = ObservationTermCfg(func=pos_w_data, scale=pos_scale_tuple) + term_2 = ObservationTermCfg(func=lin_vel_w_data, scale=1.5) + term_3 = ObservationTermCfg(func=pos_w_data, scale=pos_scale_tuple) + term_4 = ObservationTermCfg(func=lin_vel_w_data, scale=1.5) @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" - - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) - term_2 = ObservationTermCfg(func=grilled_chicken_with_curry, scale=0.0, params={"hot": False}) - term_3 = ObservationTermCfg(func=pos_w_data, scale=pos_scale_tuple) - term_4 = ObservationTermCfg(func=lin_vel_w_data, scale=1.5) - - @configclass - class CriticCfg(ObservationGroupCfg): - term_1 = ObservationTermCfg(func=pos_w_data, scale=pos_scale_tuple) - term_2 = ObservationTermCfg(func=lin_vel_w_data, scale=1.5) - term_3 = ObservationTermCfg(func=pos_w_data, scale=pos_scale_tuple) - term_4 = ObservationTermCfg(func=lin_vel_w_data, scale=1.5) - - @configclass - class ImageCfg(ObservationGroupCfg): - - term_1 = ObservationTermCfg(func=grilled_chicken_image, scale=1.5, params={"bland": 0.5, "channel": 1}) - term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=0.5, params={"bland": 0.1, "channel": 3}) - - policy: ObservationGroupCfg = PolicyCfg() - critic: ObservationGroupCfg = CriticCfg() - image: ObservationGroupCfg = ImageCfg() - - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - # compute observation using manager - observations = self.obs_man.compute() - - # obtain the group observations - obs_policy: torch.Tensor = observations["policy"] - obs_critic: torch.Tensor = observations["critic"] - obs_image: torch.Tensor = observations["image"] - - # check the observation shape - self.assertEqual((self.env.num_envs, 11), obs_policy.shape) - self.assertEqual((self.env.num_envs, 12), obs_critic.shape) - self.assertEqual((self.env.num_envs, 128, 256, 4), obs_image.shape) - # check that the scales are applied correctly - torch.testing.assert_close( - self.env.data.pos_w * torch.tensor(pos_scale_tuple, device=self.env.device), obs_critic[:, :3] - ) - torch.testing.assert_close(self.env.data.lin_vel_w * 1.5, obs_critic[:, 3:6]) - # make sure that the data are the same for same terms - # -- within group - torch.testing.assert_close(obs_critic[:, 0:3], obs_critic[:, 6:9]) - torch.testing.assert_close(obs_critic[:, 3:6], obs_critic[:, 9:12]) - # -- between groups - torch.testing.assert_close(obs_policy[:, 5:8], obs_critic[:, 0:3]) - torch.testing.assert_close(obs_policy[:, 8:11], obs_critic[:, 3:6]) - - def test_compute_with_history(self): - """Test the observation computation with history buffers.""" - HISTORY_LENGTH = 5 + class ImageCfg(ObservationGroupCfg): + term_1 = ObservationTermCfg(func=grilled_chicken_image, scale=1.5, params={"bland": 0.5, "channel": 1}) + term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=0.5, params={"bland": 0.1, "channel": 3}) + + policy: ObservationGroupCfg = PolicyCfg() + critic: ObservationGroupCfg = CriticCfg() + image: ObservationGroupCfg = ImageCfg() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + # compute observation using manager + observations = obs_man.compute() + + # obtain the group observations + obs_policy: torch.Tensor = observations["policy"] + obs_critic: torch.Tensor = observations["critic"] + obs_image: torch.Tensor = observations["image"] + + # check the observation shape + assert obs_policy.shape == (env.num_envs, 11) + assert obs_critic.shape == (env.num_envs, 12) + assert obs_image.shape == (env.num_envs, 128, 256, 4) + # check that the scales are applied correctly + assert torch.equal(env.data.pos_w * torch.tensor(pos_scale_tuple, device=env.device), obs_critic[:, :3]) + assert torch.equal(env.data.lin_vel_w * 1.5, obs_critic[:, 3:6]) + # make sure that the data are the same for same terms + # -- within group + assert torch.equal(obs_critic[:, 0:3], obs_critic[:, 6:9]) + assert torch.equal(obs_critic[:, 3:6], obs_critic[:, 9:12]) + # -- between groups + assert torch.equal(obs_policy[:, 5:8], obs_critic[:, 0:3]) + assert torch.equal(obs_policy[:, 8:11], obs_critic[:, 3:6]) + + +def test_compute_with_history(setup_env): + env = setup_env + """Test the observation computation with history buffers.""" + HISTORY_LENGTH = 5 + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" - - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - term_1 = ObservationTermCfg(func=grilled_chicken, history_length=HISTORY_LENGTH) - # total observation size: term_dim (4) * history_len (5) = 20 - term_2 = ObservationTermCfg(func=lin_vel_w_data) - # total observation size: term_dim (3) = 3 - - policy: ObservationGroupCfg = PolicyCfg() - - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - # compute observation using manager - observations = self.obs_man.compute() - # obtain the group observations - obs_policy: torch.Tensor = observations["policy"] - # check the observation shape - self.assertEqual((self.env.num_envs, 23), obs_policy.shape) - # check the observation data - expected_obs_term_1_data = torch.ones(self.env.num_envs, 4 * HISTORY_LENGTH, device=self.env.device) - expected_obs_term_2_data = lin_vel_w_data(self.env) - expected_obs_data_t0 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) - print(expected_obs_data_t0, obs_policy) - self.assertTrue(torch.equal(expected_obs_data_t0, obs_policy)) - # test that the history buffer holds previous data - for _ in range(HISTORY_LENGTH): - observations = self.obs_man.compute() - obs_policy = observations["policy"] - expected_obs_term_1_data = torch.ones(self.env.num_envs, 4 * HISTORY_LENGTH, device=self.env.device) - expected_obs_data_t5 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) - self.assertTrue(torch.equal(expected_obs_data_t5, obs_policy)) - # test reset - self.obs_man.reset() - observations = self.obs_man.compute() + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg(func=grilled_chicken, history_length=HISTORY_LENGTH) + # total observation size: term_dim (4) * history_len (5) = 20 + term_2 = ObservationTermCfg(func=lin_vel_w_data) + # total observation size: term_dim (3) = 3 + + policy: ObservationGroupCfg = PolicyCfg() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + # compute observation using manager + observations = obs_man.compute() + # obtain the group observations + obs_policy: torch.Tensor = observations["policy"] + # check the observation shape + assert obs_policy.shape == (env.num_envs, 23) + # check the observation data + expected_obs_term_1_data = torch.ones(env.num_envs, 4 * HISTORY_LENGTH, device=env.device) + expected_obs_term_2_data = lin_vel_w_data(env) + expected_obs_data_t0 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) + torch.testing.assert_close(expected_obs_data_t0, obs_policy) + # test that the history buffer holds previous data + for _ in range(HISTORY_LENGTH): + observations = obs_man.compute() obs_policy = observations["policy"] - self.assertTrue(torch.equal(expected_obs_data_t0, obs_policy)) - # test reset of specific env ids - reset_env_ids = [2, 4, 16] - self.obs_man.reset(reset_env_ids) - self.assertTrue(torch.equal(expected_obs_data_t0[reset_env_ids], obs_policy[reset_env_ids])) + expected_obs_term_1_data = torch.ones(env.num_envs, 4 * HISTORY_LENGTH, device=env.device) + expected_obs_data_t5 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) + assert torch.equal(expected_obs_data_t5, obs_policy) + # test reset + obs_man.reset() + observations = obs_man.compute() + obs_policy = observations["policy"] + torch.testing.assert_close(expected_obs_data_t0, obs_policy) + # test reset of specific env ids + reset_env_ids = [2, 4, 16] + obs_man.reset(reset_env_ids) + torch.testing.assert_close(expected_obs_data_t0[reset_env_ids], obs_policy[reset_env_ids]) + + +def test_compute_with_2d_history(setup_env): + env = setup_env + """Test the observation computation with history buffers for 2D observations.""" + HISTORY_LENGTH = 5 + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" + + @configclass + class FlattenedPolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" - def test_compute_with_2d_history(self): - """Test the observation computation with history buffers for 2D observations.""" - HISTORY_LENGTH = 5 + term_1 = ObservationTermCfg( + func=grilled_chicken_image, params={"bland": 1.0, "channel": 1}, history_length=HISTORY_LENGTH + ) + # total observation size: term_dim (128, 256) * history_len (5) = 163840 @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" - - @configclass - class FlattenedPolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - term_1 = ObservationTermCfg( - func=grilled_chicken_image, params={"bland": 1.0, "channel": 1}, history_length=HISTORY_LENGTH - ) - # total observation size: term_dim (128, 256) * history_len (5) = 163840 - - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - term_1 = ObservationTermCfg( - func=grilled_chicken_image, - params={"bland": 1.0, "channel": 1}, - history_length=HISTORY_LENGTH, - flatten_history_dim=False, - ) - # total observation size: (5, 128, 256, 1) - - flat_obs_policy: ObservationGroupCfg = FlattenedPolicyCfg() - policy: ObservationGroupCfg = PolicyCfg() - - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - # compute observation using manager - observations = self.obs_man.compute() - # obtain the group observations - obs_policy_flat: torch.Tensor = observations["flat_obs_policy"] - obs_policy: torch.Tensor = observations["policy"] - # check the observation shapes - self.assertEqual((self.env.num_envs, 163840), obs_policy_flat.shape) - self.assertEqual((self.env.num_envs, HISTORY_LENGTH, 128, 256, 1), obs_policy.shape) - - def test_compute_with_group_history(self): - """Test the observation computation with group level history buffer configuration.""" - TERM_HISTORY_LENGTH = 5 - GROUP_HISTORY_LENGTH = 10 + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg( + func=grilled_chicken_image, + params={"bland": 1.0, "channel": 1}, + history_length=HISTORY_LENGTH, + flatten_history_dim=False, + ) + # total observation size: (5, 128, 256, 1) + + flat_obs_policy: ObservationGroupCfg = FlattenedPolicyCfg() + policy: ObservationGroupCfg = PolicyCfg() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + # compute observation using manager + observations = obs_man.compute() + # obtain the group observations + obs_policy_flat: torch.Tensor = observations["flat_obs_policy"] + obs_policy: torch.Tensor = observations["policy"] + # check the observation shapes + assert obs_policy_flat.shape == (env.num_envs, 163840) + assert obs_policy.shape == (env.num_envs, HISTORY_LENGTH, 128, 256, 1) + + +def test_compute_with_group_history(setup_env): + env = setup_env + """Test the observation computation with group level history buffer configuration.""" + TERM_HISTORY_LENGTH = 5 + GROUP_HISTORY_LENGTH = 10 + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" - - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - history_length = GROUP_HISTORY_LENGTH - # group level history length will override all terms - term_1 = ObservationTermCfg(func=grilled_chicken, history_length=TERM_HISTORY_LENGTH) - # total observation size: term_dim (4) * history_len (5) = 20 - # with override total obs size: term_dim (4) * history_len (10) = 40 - term_2 = ObservationTermCfg(func=lin_vel_w_data) - # total observation size: term_dim (3) = 3 - # with override total obs size: term_dim (3) * history_len (10) = 30 - - policy: ObservationGroupCfg = PolicyCfg() - - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - # compute observation using manager - observations = self.obs_man.compute() - # obtain the group observations - obs_policy: torch.Tensor = observations["policy"] - # check the total observation shape - self.assertEqual((self.env.num_envs, 70), obs_policy.shape) - # check the observation data is initialized properly - expected_obs_term_1_data = torch.ones(self.env.num_envs, 4 * GROUP_HISTORY_LENGTH, device=self.env.device) - expected_obs_term_2_data = lin_vel_w_data(self.env).repeat(1, GROUP_HISTORY_LENGTH) - expected_obs_data_t0 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) - self.assertTrue(torch.equal(expected_obs_data_t0, obs_policy)) - # test that the history buffer holds previous data - for _ in range(GROUP_HISTORY_LENGTH): - observations = self.obs_man.compute() - obs_policy = observations["policy"] - expected_obs_term_1_data = torch.ones(self.env.num_envs, 4 * GROUP_HISTORY_LENGTH, device=self.env.device) - expected_obs_term_2_data = lin_vel_w_data(self.env).repeat(1, GROUP_HISTORY_LENGTH) - expected_obs_data_t10 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) - self.assertTrue(torch.equal(expected_obs_data_t10, obs_policy)) - # test reset - self.obs_man.reset() - observations = self.obs_man.compute() + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + history_length = GROUP_HISTORY_LENGTH + # group level history length will override all terms + term_1 = ObservationTermCfg(func=grilled_chicken, history_length=TERM_HISTORY_LENGTH) + # total observation size: term_dim (4) * history_len (5) = 20 + # with override total obs size: term_dim (4) * history_len (10) = 40 + term_2 = ObservationTermCfg(func=lin_vel_w_data) + # total observation size: term_dim (3) = 3 + # with override total obs size: term_dim (3) * history_len (10) = 30 + + policy: ObservationGroupCfg = PolicyCfg() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + # compute observation using manager + observations = obs_man.compute() + # obtain the group observations + obs_policy: torch.Tensor = observations["policy"] + # check the total observation shape + assert obs_policy.shape == (env.num_envs, 70) + # check the observation data is initialized properly + expected_obs_term_1_data = torch.ones(env.num_envs, 4 * GROUP_HISTORY_LENGTH, device=env.device) + expected_obs_term_2_data = lin_vel_w_data(env).repeat(1, GROUP_HISTORY_LENGTH) + expected_obs_data_t0 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) + torch.testing.assert_close(expected_obs_data_t0, obs_policy) + # test that the history buffer holds previous data + for _ in range(GROUP_HISTORY_LENGTH): + observations = obs_man.compute() obs_policy = observations["policy"] - self.assertTrue(torch.equal(expected_obs_data_t0, obs_policy)) - # test reset of specific env ids - reset_env_ids = [2, 4, 16] - self.obs_man.reset(reset_env_ids) - self.assertTrue(torch.equal(expected_obs_data_t0[reset_env_ids], obs_policy[reset_env_ids])) - - def test_invalid_observation_config(self): - """Test the invalid observation config.""" + expected_obs_term_1_data = torch.ones(env.num_envs, 4 * GROUP_HISTORY_LENGTH, device=env.device) + expected_obs_term_2_data = lin_vel_w_data(env).repeat(1, GROUP_HISTORY_LENGTH) + expected_obs_data_t10 = torch.concat((expected_obs_term_1_data, expected_obs_term_2_data), dim=-1) + torch.testing.assert_close(expected_obs_data_t10, obs_policy) + # test reset + obs_man.reset() + observations = obs_man.compute() + obs_policy = observations["policy"] + torch.testing.assert_close(expected_obs_data_t0, obs_policy) + # test reset of specific env ids + reset_env_ids = [2, 4, 16] + obs_man.reset(reset_env_ids) + torch.testing.assert_close(expected_obs_data_t0[reset_env_ids], obs_policy[reset_env_ids]) + + +def test_invalid_observation_config(setup_env): + env = setup_env + """Test the invalid observation config.""" + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=0.1, params={"hot": False}) + term_2 = ObservationTermCfg(func=grilled_chicken_with_yoghurt, scale=2.0, params={"hot": False}) - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" + policy: ObservationGroupCfg = PolicyCfg() - term_1 = ObservationTermCfg(func=grilled_chicken_with_bbq, scale=0.1, params={"hot": False}) - term_2 = ObservationTermCfg(func=grilled_chicken_with_yoghurt, scale=2.0, params={"hot": False}) + # create observation manager + cfg = MyObservationManagerCfg() + # check the invalid config + with pytest.raises(ValueError): + ObservationManager(cfg, env) - policy: ObservationGroupCfg = PolicyCfg() - # create observation manager - cfg = MyObservationManagerCfg() - # check the invalid config - with self.assertRaises(ValueError): - self.obs_man = ObservationManager(cfg, self.env) +def test_callable_class_term(setup_env): + env = setup_env + """Test the observation computation with callable class term.""" - def test_callable_class_term(self): - """Test the observation computation with callable class term.""" + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" - - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" - - term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) - term_2 = ObservationTermCfg(func=complex_function_class, scale=0.2, params={"interval": 0.5}) - - policy: ObservationGroupCfg = PolicyCfg() - - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - # compute observation using manager - observations = self.obs_man.compute() - # check the observation - self.assertEqual((self.env.num_envs, 5), observations["policy"].shape) - self.assertAlmostEqual(observations["policy"][0, -1].item(), 0.2 * 0.5) - - # check memory in term - num_exec_count = 10 - for _ in range(num_exec_count): - observations = self.obs_man.compute() - self.assertAlmostEqual(observations["policy"][0, -1].item(), 0.2 * 0.5 * (num_exec_count + 1)) - - # check reset works - self.obs_man.reset(env_ids=[0, 4, 9, 14, 19]) - observations = self.obs_man.compute() - self.assertAlmostEqual(observations["policy"][0, -1].item(), 0.2 * 0.5) - self.assertAlmostEqual(observations["policy"][1, -1].item(), 0.2 * 0.5 * (num_exec_count + 2)) - - def test_non_callable_class_term(self): - """Test the observation computation with non-callable class term.""" + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) + term_2 = ObservationTermCfg(func=complex_function_class, scale=0.2, params={"interval": 0.5}) + + policy: ObservationGroupCfg = PolicyCfg() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + # compute observation using manager + observations = obs_man.compute() + # check the observation + assert observations["policy"].shape == (env.num_envs, 5) + assert observations["policy"][0, -1].item() == pytest.approx(0.2 * 0.5) + + # check memory in term + num_exec_count = 10 + for _ in range(num_exec_count): + observations = obs_man.compute() + assert observations["policy"][0, -1].item() == pytest.approx(0.2 * 0.5 * (num_exec_count + 1)) + + # check reset works + obs_man.reset(env_ids=[0, 4, 9, 14, 19]) + observations = obs_man.compute() + assert observations["policy"][0, -1].item() == pytest.approx(0.2 * 0.5) + assert observations["policy"][1, -1].item() == pytest.approx(0.2 * 0.5 * (num_exec_count + 2)) + + +def test_non_callable_class_term(setup_env): + env = setup_env + """Test the observation computation with non-callable class term.""" + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) + term_2 = ObservationTermCfg(func=non_callable_complex_function_class, scale=0.2) - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" + policy: ObservationGroupCfg = PolicyCfg() - term_1 = ObservationTermCfg(func=grilled_chicken, scale=10) - term_2 = ObservationTermCfg(func=non_callable_complex_function_class, scale=0.2) + # create observation manager config + cfg = MyObservationManagerCfg() + # create observation manager + with pytest.raises(NotImplementedError): + ObservationManager(cfg, env) - policy: ObservationGroupCfg = PolicyCfg() - # create observation manager config - cfg = MyObservationManagerCfg() - # create observation manager - with self.assertRaises(NotImplementedError): - self.obs_man = ObservationManager(cfg, self.env) +def test_modifier_compute(setup_env): + env = setup_env + """Test the observation computation with modifiers.""" - def test_modifier_compute(self): - """Test the observation computation with modifiers.""" + modifier_1 = modifiers.ModifierCfg(func=modifiers.bias, params={"value": 1.0}) + modifier_2 = modifiers.ModifierCfg(func=modifiers.scale, params={"multiplier": 2.0}) + modifier_3 = modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (-0.5, 0.5)}) + modifier_4 = modifiers.IntegratorCfg(dt=env.dt) - modifier_1 = modifiers.ModifierCfg(func=modifiers.bias, params={"value": 1.0}) - modifier_2 = modifiers.ModifierCfg(func=modifiers.scale, params={"multiplier": 2.0}) - modifier_3 = modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (-0.5, 0.5)}) - modifier_4 = modifiers.IntegratorCfg(dt=self.env.dt) + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + concatenate_terms = False + term_1 = ObservationTermCfg(func=pos_w_data, modifiers=[]) + term_2 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1]) + term_3 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1, modifier_4]) + + @configclass + class CriticCfg(ObservationGroupCfg): + """Test config class for critic observation group""" + + concatenate_terms = False + term_1 = ObservationTermCfg(func=pos_w_data, modifiers=[]) + term_2 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1]) + term_3 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1, modifier_2]) + term_4 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1, modifier_2, modifier_3]) + + policy: ObservationGroupCfg = PolicyCfg() + critic: ObservationGroupCfg = CriticCfg() - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + # compute observation using manager + observations = obs_man.compute() - concatenate_terms = False - term_1 = ObservationTermCfg(func=pos_w_data, modifiers=[]) - term_2 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1]) - term_3 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1, modifier_4]) + # obtain the group observations + obs_policy: dict[str, torch.Tensor] = observations["policy"] + obs_critic: dict[str, torch.Tensor] = observations["critic"] - @configclass - class CriticCfg(ObservationGroupCfg): - """Test config class for critic observation group""" + # check correct application of modifications + assert torch.equal(obs_policy["term_1"] + 1.0, obs_policy["term_2"]) + assert torch.equal(obs_critic["term_1"] + 1.0, obs_critic["term_2"]) + assert torch.equal(2.0 * (obs_critic["term_1"] + 1.0), obs_critic["term_3"]) + assert torch.min(obs_critic["term_4"]) >= -0.5 + assert torch.max(obs_critic["term_4"]) <= 0.5 - concatenate_terms = False - term_1 = ObservationTermCfg(func=pos_w_data, modifiers=[]) - term_2 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1]) - term_3 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1, modifier_2]) - term_4 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier_1, modifier_2, modifier_3]) - policy: ObservationGroupCfg = PolicyCfg() - critic: ObservationGroupCfg = CriticCfg() +def test_serialize(setup_env): + """Test serialize call for ManagerTermBase terms.""" + env = setup_env - # create observation manager - cfg = MyObservationManagerCfg() - self.obs_man = ObservationManager(cfg, self.env) - # compute observation using manager - observations = self.obs_man.compute() + serialize_data = {"test": 0} - # obtain the group observations - obs_policy: dict[str, torch.Tensor] = observations["policy"] - obs_critic: dict[str, torch.Tensor] = observations["critic"] + class test_serialize_term(ManagerTermBase): + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) - # check correct application of modifications - torch.testing.assert_close(obs_policy["term_1"] + 1.0, obs_policy["term_2"]) - torch.testing.assert_close(obs_critic["term_1"] + 1.0, obs_critic["term_2"]) - torch.testing.assert_close(2.0 * (obs_critic["term_1"] + 1.0), obs_critic["term_3"]) - self.assertTrue(torch.min(obs_critic["term_4"]) >= -0.5) - self.assertTrue(torch.max(obs_critic["term_4"]) <= 0.5) + def __call__(self, env: ManagerBasedEnv) -> torch.Tensor: + return grilled_chicken(env) - def test_modifier_invalid_config(self): - """Test modifier initialization with invalid config.""" + def serialize(self) -> dict: + return serialize_data - modifier = modifiers.ModifierCfg(func=modifiers.clip, params={"min": -0.5, "max": 0.5}) + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" @configclass - class MyObservationManagerCfg: - """Test config class for observation manager.""" + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" - @configclass - class PolicyCfg(ObservationGroupCfg): - """Test config class for policy observation group.""" + concatenate_terms = False + term_1 = ObservationTermCfg(func=test_serialize_term) - concatenate_terms = False - term_1 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier]) + policy: ObservationGroupCfg = PolicyCfg() - policy: ObservationGroupCfg = PolicyCfg() + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) - # create observation manager - cfg = MyObservationManagerCfg() + # check expected output + assert obs_man.serialize() == {"policy": {"term_1": serialize_data}} + + +def test_modifier_invalid_config(setup_env): + env = setup_env + """Test modifier initialization with invalid config.""" + + modifier = modifiers.ModifierCfg(func=modifiers.clip, params={"min": -0.5, "max": 0.5}) + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" + + @configclass + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" - with self.assertRaises(ValueError): - self.obs_man = ObservationManager(cfg, self.env) + concatenate_terms = False + term_1 = ObservationTermCfg(func=pos_w_data, modifiers=[modifier]) + policy: ObservationGroupCfg = PolicyCfg() -if __name__ == "__main__": - run_tests() + # create observation manager + cfg = MyObservationManagerCfg() + + with pytest.raises(ValueError): + ObservationManager(cfg, env) + + +def test_concatenate_dim(setup_env): + """Test concatenation of observations along different dimensions.""" + env = setup_env + + @configclass + class MyObservationManagerCfg: + """Test config class for observation manager.""" + + @configclass + class PolicyCfg(ObservationGroupCfg): + """Test config class for policy observation group.""" + + concatenate_terms = True + concatenate_dim = 1 # Concatenate along dimension 1 + term_1 = ObservationTermCfg(func=grilled_chicken_image, scale=1.0, params={"bland": 1.0, "channel": 1}) + term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=1.0, params={"bland": 1.0, "channel": 1}) + + @configclass + class CriticCfg(ObservationGroupCfg): + """Test config class for critic observation group.""" + + concatenate_terms = True + concatenate_dim = 2 # Concatenate along dimension 2 + term_1 = ObservationTermCfg(func=grilled_chicken_image, scale=1.0, params={"bland": 1.0, "channel": 1}) + term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=1.0, params={"bland": 1.0, "channel": 1}) + + @configclass + class CriticCfg_neg_dim(ObservationGroupCfg): + """Test config class for critic observation group.""" + + concatenate_terms = True + concatenate_dim = -1 # Concatenate along last dimension + term_1 = ObservationTermCfg(func=grilled_chicken_image, scale=1.0, params={"bland": 1.0, "channel": 1}) + term_2 = ObservationTermCfg(func=grilled_chicken_image, scale=1.0, params={"bland": 1.0, "channel": 1}) + + policy: ObservationGroupCfg = PolicyCfg() + critic: ObservationGroupCfg = CriticCfg() + critic_neg_dim: ObservationGroupCfg = CriticCfg_neg_dim() + + # create observation manager + cfg = MyObservationManagerCfg() + obs_man = ObservationManager(cfg, env) + # compute observation using manager + observations = obs_man.compute() + + # obtain the group observations + obs_policy: torch.Tensor = observations["policy"] + obs_critic: torch.Tensor = observations["critic"] + obs_critic_neg_dim: torch.Tensor = observations["critic_neg_dim"] + + # check the observation shapes + # For policy: concatenated along dim 1, so width should be doubled + assert obs_policy.shape == (env.num_envs, 128, 512, 1) + # For critic: concatenated along last dim, so channels should be doubled + assert obs_critic.shape == (env.num_envs, 128, 256, 2) + # For critic_neg_dim: concatenated along last dim, so channels should be doubled + assert obs_critic_neg_dim.shape == (env.num_envs, 128, 256, 2) + + # verify the data is concatenated correctly + # For policy: check that the second half matches the first half + torch.testing.assert_close(obs_policy[:, :, :256, :], obs_policy[:, :, 256:, :]) + # For critic: check that the second channel matches the first channel + torch.testing.assert_close(obs_critic[:, :, :, 0], obs_critic[:, :, :, 1]) + + # For critic_neg_dim: check that it is the same as critic + torch.testing.assert_close(obs_critic_neg_dim, obs_critic) diff --git a/source/isaaclab/test/managers/test_recorder_manager.py b/source/isaaclab/test/managers/test_recorder_manager.py index e1cfe90ab174..8a8e8c78a9d2 100644 --- a/source/isaaclab/test/managers/test_recorder_manager.py +++ b/source/isaaclab/test/managers/test_recorder_manager.py @@ -1,14 +1,13 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - # needed to import for allowing type-hinting: torch.Tensor | None from __future__ import annotations """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app @@ -18,17 +17,26 @@ import os import shutil import tempfile -import torch -import unittest import uuid from collections import namedtuple from collections.abc import Sequence +from typing import TYPE_CHECKING + +import h5py +import pytest +import torch -from isaaclab.envs import ManagerBasedEnv +import omni.usd + +from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg from isaaclab.managers import DatasetExportMode, RecorderManager, RecorderManagerBaseCfg, RecorderTerm, RecorderTermCfg +from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim import SimulationContext from isaaclab.utils import configclass +if TYPE_CHECKING: + import numpy as np + class DummyResetRecorderTerm(RecorderTerm): """Dummy recorder term that records dummy data.""" @@ -78,6 +86,94 @@ class DummyStepRecorderTermCfg(RecorderTermCfg): dataset_export_mode = DatasetExportMode.EXPORT_ALL +@configclass +class EmptyManagerCfg: + """Empty manager specifications for the environment.""" + + pass + + +@configclass +class EmptySceneCfg(InteractiveSceneCfg): + """Configuration for an empty scene.""" + + pass + + +def get_empty_base_env_cfg(device: str = "cuda", num_envs: int = 1, env_spacing: float = 1.0): + """Generate base environment config based on device""" + + @configclass + class EmptyEnvCfg(ManagerBasedEnvCfg): + """Configuration for the empty test environment.""" + + # Scene settings + scene: EmptySceneCfg = EmptySceneCfg(num_envs=num_envs, env_spacing=env_spacing) + # Basic settings + actions: EmptyManagerCfg = EmptyManagerCfg() + observations: EmptyManagerCfg = EmptyManagerCfg() + recorders: EmptyManagerCfg = EmptyManagerCfg() + + def __post_init__(self): + """Post initialization.""" + # step settings + self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz + # simulation settings + self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.render_interval = self.decimation # render every 4 sim steps + # pass device down from test + self.sim.device = device + + return EmptyEnvCfg() + + +def get_file_contents(file_name: str, num_steps: int) -> dict[str, np.ndarray]: + """Retrieves the contents of the hdf5 file + Args: + file_name: absolute path to the hdf5 file + num_steps: number of steps taken in the environment + Returns: + dict[str, np.ndarray]: dictionary where keys are HDF5 paths and values are the corresponding data arrays. + """ + data = {} + with h5py.File(file_name, "r") as f: + + def get_data(name, obj): + if isinstance(obj, h5py.Dataset): + if "record_post_step" in name: + assert obj[()].shape == (num_steps, 5) + elif "record_pre_step" in name: + assert obj[()].shape == (num_steps, 4) + else: + raise Exception(f"The hdf5 file contains an unexpected data path, {name}") + data[name] = obj[()] + + f.visititems(get_data) + return data + + +@configclass +class DummyEnvCfg: + """Dummy environment configuration.""" + + @configclass + class DummySimCfg: + """Configuration for the dummy sim.""" + + dt = 0.01 + render_interval = 1 + + @configclass + class DummySceneCfg: + """Configuration for the dummy scene.""" + + num_envs = 1 + + decimation = 1 + sim = DummySimCfg() + scene = DummySceneCfg() + + def create_dummy_env(device: str = "cpu") -> ManagerBasedEnv: """Create a dummy environment.""" @@ -86,79 +182,101 @@ class DummyTerminationManager: dummy_termination_manager = DummyTerminationManager() sim = SimulationContext() + dummy_cfg = DummyEnvCfg() + return namedtuple("ManagerBasedEnv", ["num_envs", "device", "sim", "cfg", "termination_manager"])( - 20, device, sim, dict(), dummy_termination_manager + 20, device, sim, dummy_cfg, dummy_termination_manager ) -class TestRecorderManager(unittest.TestCase): - """Test cases for various situations with recorder manager.""" - - def setUp(self) -> None: - self.dataset_dir = tempfile.mkdtemp() - - def tearDown(self): - # delete the temporary directory after the test - shutil.rmtree(self.dataset_dir) - - def create_dummy_recorder_manager_cfg(self) -> DummyRecorderManagerCfg: - """Get the dummy recorder manager configurations.""" - cfg = DummyRecorderManagerCfg() - cfg.dataset_export_dir_path = self.dataset_dir - cfg.dataset_filename = f"{uuid.uuid4()}.hdf5" - return cfg - - def test_str(self): - """Test the string representation of the recorder manager.""" - # create recorder manager - cfg = DummyRecorderManagerCfg() - recorder_manager = RecorderManager(cfg, create_dummy_env()) - self.assertEqual(len(recorder_manager.active_terms), 2) - # print the expected string - print() - print(recorder_manager) - - def test_initialize_dataset_file(self): - """Test the initialization of the dataset file.""" - # create recorder manager - cfg = self.create_dummy_recorder_manager_cfg() - _ = RecorderManager(cfg, create_dummy_env()) - - # check if the dataset is created - self.assertTrue(os.path.exists(os.path.join(cfg.dataset_export_dir_path, cfg.dataset_filename))) - - def test_record(self): - """Test the recording of the data.""" - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - env = create_dummy_env(device) - # create recorder manager - recorder_manager = RecorderManager(self.create_dummy_recorder_manager_cfg(), env) - - # record the step data - recorder_manager.record_pre_step() - recorder_manager.record_post_step() - - recorder_manager.record_pre_step() - recorder_manager.record_post_step() - - # check the recorded data - for env_id in range(env.num_envs): - episode = recorder_manager.get_episode(env_id) - self.assertEqual(episode.data["record_pre_step"].shape, (2, 4)) - self.assertEqual(episode.data["record_post_step"].shape, (2, 5)) - - # Trigger pre-reset callbacks which then export and clean the episode data - recorder_manager.record_pre_reset(env_ids=None) - for env_id in range(env.num_envs): - episode = recorder_manager.get_episode(env_id) - self.assertTrue(episode.is_empty()) - - recorder_manager.record_post_reset(env_ids=None) - for env_id in range(env.num_envs): - episode = recorder_manager.get_episode(env_id) - self.assertEqual(episode.data["record_post_reset"].shape, (1, 3)) - - -if __name__ == "__main__": - run_tests() +@pytest.fixture +def dataset_dir(): + """Create directory to dump results.""" + test_dir = tempfile.mkdtemp() + yield test_dir + # Cleanup + shutil.rmtree(test_dir) + + +def test_str(dataset_dir): + """Test the string representation of the recorder manager.""" + # create recorder manager + cfg = DummyRecorderManagerCfg() + recorder_manager = RecorderManager(cfg, create_dummy_env()) + assert len(recorder_manager.active_terms) == 2 + # print the expected string + print(recorder_manager) + + +def test_initialize_dataset_file(dataset_dir): + """Test the initialization of the dataset file.""" + # create recorder manager + cfg = DummyRecorderManagerCfg() + cfg.dataset_export_dir_path = dataset_dir + cfg.dataset_filename = f"{uuid.uuid4()}.hdf5" + _ = RecorderManager(cfg, create_dummy_env()) + + # check if the dataset is created + assert os.path.exists(os.path.join(cfg.dataset_export_dir_path, cfg.dataset_filename)) + + +@pytest.mark.parametrize("device", ("cpu", "cuda")) +def test_record(device, dataset_dir): + """Test the recording of the data.""" + env = create_dummy_env(device) + # create recorder manager + cfg = DummyRecorderManagerCfg() + cfg.dataset_export_dir_path = dataset_dir + cfg.dataset_filename = f"{uuid.uuid4()}.hdf5" + recorder_manager = RecorderManager(cfg, env) + + # record the step data + recorder_manager.record_pre_step() + recorder_manager.record_post_step() + + recorder_manager.record_pre_step() + recorder_manager.record_post_step() + + # check the recorded data + for env_id in range(env.num_envs): + episode = recorder_manager.get_episode(env_id) + assert torch.stack(episode.data["record_pre_step"]).shape == (2, 4) + assert torch.stack(episode.data["record_post_step"]).shape == (2, 5) + + # Trigger pre-reset callbacks which then export and clean the episode data + recorder_manager.record_pre_reset(env_ids=None) + for env_id in range(env.num_envs): + episode = recorder_manager.get_episode(env_id) + assert episode.is_empty() + + recorder_manager.record_post_reset(env_ids=None) + for env_id in range(env.num_envs): + episode = recorder_manager.get_episode(env_id) + assert torch.stack(episode.data["record_post_reset"]).shape == (1, 3) + + +@pytest.mark.parametrize("device", ("cpu", "cuda")) +def test_close(device, dataset_dir): + """Test whether data is correctly exported in the close function when fully integrated with ManagerBasedEnv and + `export_in_close` is True.""" + # create a new stage + omni.usd.get_context().new_stage() + # create environment + env_cfg = get_empty_base_env_cfg(device=device, num_envs=2) + cfg = DummyRecorderManagerCfg() + cfg.export_in_close = True + cfg.dataset_export_dir_path = dataset_dir + cfg.dataset_filename = f"{uuid.uuid4()}.hdf5" + env_cfg.recorders = cfg + env = ManagerBasedEnv(cfg=env_cfg) + num_steps = 3 + for _ in range(num_steps): + act = torch.randn_like(env.action_manager.action) + obs, ext = env.step(act) + # check contents of hdf5 file + file_name = f"{env_cfg.recorders.dataset_export_dir_path}/{env_cfg.recorders.dataset_filename}" + data_pre_close = get_file_contents(file_name, num_steps) + assert len(data_pre_close) == 0 + env.close() + data_post_close = get_file_contents(file_name, num_steps) + assert len(data_post_close.keys()) == 2 * env_cfg.scene.num_envs diff --git a/source/isaaclab/test/managers/test_reward_manager.py b/source/isaaclab/test/managers/test_reward_manager.py index 6485af09aba2..8301fac5b504 100644 --- a/source/isaaclab/test/managers/test_reward_manager.py +++ b/source/isaaclab/test/managers/test_reward_manager.py @@ -1,21 +1,22 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import torch -import unittest from collections import namedtuple +import pytest +import torch + from isaaclab.managers import RewardManager, RewardTermCfg from isaaclab.sim import SimulationContext from isaaclab.utils import configclass @@ -37,150 +38,152 @@ def grilled_chicken_with_yoghurt(env, hot: bool, bland: float): return 0 -class TestRewardManager(unittest.TestCase): - """Test cases for various situations with reward manager.""" - - def setUp(self) -> None: - sim = SimulationContext() - self.env = namedtuple("ManagerBasedRLEnv", ["num_envs", "dt", "device", "sim"])(20, 0.1, "cpu", sim) - - def test_str(self): - """Test the string representation of the reward manager.""" - cfg = { - "term_1": RewardTermCfg(func=grilled_chicken, weight=10), - "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, weight=5, params={"bbq": True}), - "term_3": RewardTermCfg( - func=grilled_chicken_with_yoghurt, - weight=1.0, - params={"hot": False, "bland": 2.0}, - ), - } - self.rew_man = RewardManager(cfg, self.env) - self.assertEqual(len(self.rew_man.active_terms), 3) - # print the expected string - print() - print(self.rew_man) - - def test_config_equivalence(self): - """Test the equivalence of reward manager created from different config types.""" - # create from dictionary - cfg = { - "my_term": RewardTermCfg(func=grilled_chicken, weight=10), - "your_term": RewardTermCfg(func=grilled_chicken_with_bbq, weight=2.0, params={"bbq": True}), - "his_term": RewardTermCfg( - func=grilled_chicken_with_yoghurt, - weight=1.0, - params={"hot": False, "bland": 2.0}, - ), - } - rew_man_from_dict = RewardManager(cfg, self.env) - - # create from config class - @configclass - class MyRewardManagerCfg: - """Reward manager config with no type annotations.""" - - my_term = RewardTermCfg(func=grilled_chicken, weight=10.0) - your_term = RewardTermCfg(func=grilled_chicken_with_bbq, weight=2.0, params={"bbq": True}) - his_term = RewardTermCfg(func=grilled_chicken_with_yoghurt, weight=1.0, params={"hot": False, "bland": 2.0}) - - cfg = MyRewardManagerCfg() - rew_man_from_cfg = RewardManager(cfg, self.env) - - # create from config class - @configclass - class MyRewardManagerAnnotatedCfg: - """Reward manager config with type annotations.""" - - my_term: RewardTermCfg = RewardTermCfg(func=grilled_chicken, weight=10.0) - your_term: RewardTermCfg = RewardTermCfg(func=grilled_chicken_with_bbq, weight=2.0, params={"bbq": True}) - his_term: RewardTermCfg = RewardTermCfg( - func=grilled_chicken_with_yoghurt, weight=1.0, params={"hot": False, "bland": 2.0} - ) - - cfg = MyRewardManagerAnnotatedCfg() - rew_man_from_annotated_cfg = RewardManager(cfg, self.env) - - # check equivalence - # parsed terms - self.assertEqual(rew_man_from_dict.active_terms, rew_man_from_annotated_cfg.active_terms) - self.assertEqual(rew_man_from_cfg.active_terms, rew_man_from_annotated_cfg.active_terms) - self.assertEqual(rew_man_from_dict.active_terms, rew_man_from_cfg.active_terms) - # parsed term configs - self.assertEqual(rew_man_from_dict._term_cfgs, rew_man_from_annotated_cfg._term_cfgs) - self.assertEqual(rew_man_from_cfg._term_cfgs, rew_man_from_annotated_cfg._term_cfgs) - self.assertEqual(rew_man_from_dict._term_cfgs, rew_man_from_cfg._term_cfgs) - - def test_compute(self): - """Test the computation of reward.""" - cfg = { - "term_1": RewardTermCfg(func=grilled_chicken, weight=10), - "term_2": RewardTermCfg(func=grilled_chicken_with_curry, weight=0.0, params={"hot": False}), - } - self.rew_man = RewardManager(cfg, self.env) - # compute expected reward - expected_reward = cfg["term_1"].weight * self.env.dt - # compute reward using manager - rewards = self.rew_man.compute(dt=self.env.dt) - # check the reward for environment index 0 - self.assertEqual(float(rewards[0]), expected_reward) - self.assertEqual(tuple(rewards.shape), (self.env.num_envs,)) - - def test_config_empty(self): - """Test the creation of reward manager with empty config.""" - self.rew_man = RewardManager(None, self.env) - self.assertEqual(len(self.rew_man.active_terms), 0) - - # print the expected string - print() - print(self.rew_man) - - # compute reward - rewards = self.rew_man.compute(dt=self.env.dt) - - # check all rewards are zero - torch.testing.assert_close(rewards, torch.zeros_like(rewards)) - - def test_active_terms(self): - """Test the correct reading of active terms.""" - cfg = { - "term_1": RewardTermCfg(func=grilled_chicken, weight=10), - "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, weight=5, params={"bbq": True}), - "term_3": RewardTermCfg(func=grilled_chicken_with_curry, weight=0.0, params={"hot": False}), - } - self.rew_man = RewardManager(cfg, self.env) - - self.assertEqual(len(self.rew_man.active_terms), 3) - - def test_missing_weight(self): - """Test the missing of weight in the config.""" - # TODO: The error should be raised during the config parsing, not during the reward manager creation. - cfg = { - "term_1": RewardTermCfg(func=grilled_chicken, weight=10), - "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, params={"bbq": True}), - } - with self.assertRaises(TypeError): - self.rew_man = RewardManager(cfg, self.env) - - def test_invalid_reward_func_module(self): - """Test the handling of invalid reward function's module in string representation.""" - cfg = { - "term_1": RewardTermCfg(func=grilled_chicken, weight=10), - "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, weight=5, params={"bbq": True}), - "term_3": RewardTermCfg(func="a:grilled_chicken_with_no_bbq", weight=0.1, params={"hot": False}), - } - with self.assertRaises(ValueError): - self.rew_man = RewardManager(cfg, self.env) - - def test_invalid_reward_config(self): - """Test the handling of invalid reward function's config parameters.""" - cfg = { - "term_1": RewardTermCfg(func=grilled_chicken_with_bbq, weight=0.1, params={"hot": False}), - "term_2": RewardTermCfg(func=grilled_chicken_with_yoghurt, weight=2.0, params={"hot": False}), - } - with self.assertRaises(ValueError): - self.rew_man = RewardManager(cfg, self.env) - - -if __name__ == "__main__": - run_tests() +@pytest.fixture +def env(): + sim = SimulationContext() + return namedtuple("ManagerBasedRLEnv", ["num_envs", "dt", "device", "sim"])(20, 0.1, "cpu", sim) + + +def test_str(env): + """Test the string representation of the reward manager.""" + cfg = { + "term_1": RewardTermCfg(func=grilled_chicken, weight=10), + "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, weight=5, params={"bbq": True}), + "term_3": RewardTermCfg( + func=grilled_chicken_with_yoghurt, + weight=1.0, + params={"hot": False, "bland": 2.0}, + ), + } + rew_man = RewardManager(cfg, env) + assert len(rew_man.active_terms) == 3 + # print the expected string + print() + print(rew_man) + + +def test_config_equivalence(env): + """Test the equivalence of reward manager created from different config types.""" + # create from dictionary + cfg = { + "my_term": RewardTermCfg(func=grilled_chicken, weight=10), + "your_term": RewardTermCfg(func=grilled_chicken_with_bbq, weight=2.0, params={"bbq": True}), + "his_term": RewardTermCfg( + func=grilled_chicken_with_yoghurt, + weight=1.0, + params={"hot": False, "bland": 2.0}, + ), + } + rew_man_from_dict = RewardManager(cfg, env) + + # create from config class + @configclass + class MyRewardManagerCfg: + """Reward manager config with no type annotations.""" + + my_term = RewardTermCfg(func=grilled_chicken, weight=10.0) + your_term = RewardTermCfg(func=grilled_chicken_with_bbq, weight=2.0, params={"bbq": True}) + his_term = RewardTermCfg(func=grilled_chicken_with_yoghurt, weight=1.0, params={"hot": False, "bland": 2.0}) + + cfg = MyRewardManagerCfg() + rew_man_from_cfg = RewardManager(cfg, env) + + # create from config class + @configclass + class MyRewardManagerAnnotatedCfg: + """Reward manager config with type annotations.""" + + my_term: RewardTermCfg = RewardTermCfg(func=grilled_chicken, weight=10.0) + your_term: RewardTermCfg = RewardTermCfg(func=grilled_chicken_with_bbq, weight=2.0, params={"bbq": True}) + his_term: RewardTermCfg = RewardTermCfg( + func=grilled_chicken_with_yoghurt, weight=1.0, params={"hot": False, "bland": 2.0} + ) + + cfg = MyRewardManagerAnnotatedCfg() + rew_man_from_annotated_cfg = RewardManager(cfg, env) + + # check equivalence + # parsed terms + assert rew_man_from_dict.active_terms == rew_man_from_annotated_cfg.active_terms + assert rew_man_from_cfg.active_terms == rew_man_from_annotated_cfg.active_terms + assert rew_man_from_dict.active_terms == rew_man_from_cfg.active_terms + # parsed term configs + assert rew_man_from_dict._term_cfgs == rew_man_from_annotated_cfg._term_cfgs + assert rew_man_from_cfg._term_cfgs == rew_man_from_annotated_cfg._term_cfgs + assert rew_man_from_dict._term_cfgs == rew_man_from_cfg._term_cfgs + + +def test_compute(env): + """Test the computation of reward.""" + cfg = { + "term_1": RewardTermCfg(func=grilled_chicken, weight=10), + "term_2": RewardTermCfg(func=grilled_chicken_with_curry, weight=0.0, params={"hot": False}), + } + rew_man = RewardManager(cfg, env) + # compute expected reward + expected_reward = cfg["term_1"].weight * env.dt + # compute reward using manager + rewards = rew_man.compute(dt=env.dt) + # check the reward for environment index 0 + assert float(rewards[0]) == expected_reward + assert tuple(rewards.shape) == (env.num_envs,) + + +def test_config_empty(env): + """Test the creation of reward manager with empty config.""" + rew_man = RewardManager(None, env) + assert len(rew_man.active_terms) == 0 + + # print the expected string + print() + print(rew_man) + + # compute reward + rewards = rew_man.compute(dt=env.dt) + + # check all rewards are zero + torch.testing.assert_close(rewards, torch.zeros_like(rewards)) + + +def test_active_terms(env): + """Test the correct reading of active terms.""" + cfg = { + "term_1": RewardTermCfg(func=grilled_chicken, weight=10), + "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, weight=5, params={"bbq": True}), + "term_3": RewardTermCfg(func=grilled_chicken_with_curry, weight=0.0, params={"hot": False}), + } + rew_man = RewardManager(cfg, env) + + assert len(rew_man.active_terms) == 3 + + +def test_missing_weight(env): + """Test the missing of weight in the config.""" + # TODO: The error should be raised during the config parsing, not during the reward manager creation. + cfg = { + "term_1": RewardTermCfg(func=grilled_chicken, weight=10), + "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, params={"bbq": True}), + } + with pytest.raises(TypeError): + RewardManager(cfg, env) + + +def test_invalid_reward_func_module(env): + """Test the handling of invalid reward function's module in string representation.""" + cfg = { + "term_1": RewardTermCfg(func=grilled_chicken, weight=10), + "term_2": RewardTermCfg(func=grilled_chicken_with_bbq, weight=5, params={"bbq": True}), + "term_3": RewardTermCfg(func="a:grilled_chicken_with_no_bbq", weight=0.1, params={"hot": False}), + } + with pytest.raises(ValueError): + RewardManager(cfg, env) + + +def test_invalid_reward_config(env): + """Test the handling of invalid reward function's config parameters.""" + cfg = { + "term_1": RewardTermCfg(func=grilled_chicken_with_bbq, weight=0.1, params={"hot": False}), + "term_2": RewardTermCfg(func=grilled_chicken_with_yoghurt, weight=2.0, params={"hot": False}), + } + with pytest.raises(ValueError): + RewardManager(cfg, env) diff --git a/source/isaaclab/test/managers/test_termination_manager.py b/source/isaaclab/test/managers/test_termination_manager.py new file mode 100644 index 000000000000..db96e93675c3 --- /dev/null +++ b/source/isaaclab/test/managers/test_termination_manager.py @@ -0,0 +1,139 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest +import torch + +from isaaclab.managers import TerminationManager, TerminationTermCfg +from isaaclab.sim import SimulationContext + + +class DummyEnv: + """Minimal mutable env stub for the termination manager tests.""" + + def __init__(self, num_envs: int, device: str, sim: SimulationContext): + self.num_envs = num_envs + self.device = device + self.sim = sim + self.counter = 0 # mutable step counter used by test terms + + +def fail_every_5_steps(env) -> torch.Tensor: + """Returns True for all envs when counter is a positive multiple of 5.""" + cond = env.counter > 0 and (env.counter % 5 == 0) + return torch.full((env.num_envs,), cond, dtype=torch.bool, device=env.device) + + +def fail_every_10_steps(env) -> torch.Tensor: + """Returns True for all envs when counter is a positive multiple of 10.""" + cond = env.counter > 0 and (env.counter % 10 == 0) + return torch.full((env.num_envs,), cond, dtype=torch.bool, device=env.device) + + +def fail_every_3_steps(env) -> torch.Tensor: + """Returns True for all envs when counter is a positive multiple of 3.""" + cond = env.counter > 0 and (env.counter % 3 == 0) + return torch.full((env.num_envs,), cond, dtype=torch.bool, device=env.device) + + +@pytest.fixture +def env(): + sim = SimulationContext() + return DummyEnv(num_envs=20, device="cpu", sim=sim) + + +def test_initial_state_and_shapes(env): + cfg = { + "term_5": TerminationTermCfg(func=fail_every_5_steps), + "term_10": TerminationTermCfg(func=fail_every_10_steps), + } + tm = TerminationManager(cfg, env) + + # Active term names + assert tm.active_terms == ["term_5", "term_10"] + + # Internal buffers have expected shapes and start as all False + assert tm._term_dones.shape == (env.num_envs, 2) + assert tm._last_episode_dones.shape == (env.num_envs, 2) + assert tm.dones.shape == (env.num_envs,) + assert tm.time_outs.shape == (env.num_envs,) + assert tm.terminated.shape == (env.num_envs,) + assert torch.all(~tm._term_dones) and torch.all(~tm._last_episode_dones) + + +def test_term_transitions_and_persistence(env): + """Concise transitions: single fire, persist, switch, both, persist. + + Uses 3-step and 5-step terms and verifies current-step values and last-episode persistence. + """ + cfg = { + "term_3": TerminationTermCfg(func=fail_every_3_steps, time_out=False), + "term_5": TerminationTermCfg(func=fail_every_5_steps, time_out=False), + } + tm = TerminationManager(cfg, env) + + # step 3: only term_3 -> last_episode [True, False] + env.counter = 3 + out = tm.compute() + assert torch.all(tm.get_term("term_3")) and torch.all(~tm.get_term("term_5")) + assert torch.all(out) + assert torch.all(tm._last_episode_dones[:, 0]) and torch.all(~tm._last_episode_dones[:, 1]) + + # step 4: none -> last_episode persists [True, False] + env.counter = 4 + out = tm.compute() + assert torch.all(~out) + assert torch.all(~tm.get_term("term_3")) and torch.all(~tm.get_term("term_5")) + assert torch.all(tm._last_episode_dones[:, 0]) and torch.all(~tm._last_episode_dones[:, 1]) + + # step 5: only term_5 -> last_episode [False, True] + env.counter = 5 + out = tm.compute() + assert torch.all(~tm.get_term("term_3")) and torch.all(tm.get_term("term_5")) + assert torch.all(out) + assert torch.all(~tm._last_episode_dones[:, 0]) and torch.all(tm._last_episode_dones[:, 1]) + + # step 15: both -> last_episode [True, True] + env.counter = 15 + out = tm.compute() + assert torch.all(tm.get_term("term_3")) and torch.all(tm.get_term("term_5")) + assert torch.all(out) + assert torch.all(tm._last_episode_dones[:, 0]) and torch.all(tm._last_episode_dones[:, 1]) + + # step 16: none -> persist [True, True] + env.counter = 16 + out = tm.compute() + assert torch.all(~out) + assert torch.all(~tm.get_term("term_3")) and torch.all(~tm.get_term("term_5")) + assert torch.all(tm._last_episode_dones[:, 0]) and torch.all(tm._last_episode_dones[:, 1]) + + +def test_time_out_vs_terminated_split(env): + cfg = { + "term_5": TerminationTermCfg(func=fail_every_5_steps, time_out=False), # terminated + "term_10": TerminationTermCfg(func=fail_every_10_steps, time_out=True), # timeout + } + tm = TerminationManager(cfg, env) + + # Step 5: terminated fires, not timeout + env.counter = 5 + out = tm.compute() + assert torch.all(out) + assert torch.all(tm.terminated) and torch.all(~tm.time_outs) + + # Step 10: both fire; timeout and terminated both True + env.counter = 10 + out = tm.compute() + assert torch.all(out) + assert torch.all(tm.terminated) and torch.all(tm.time_outs) diff --git a/source/isaaclab/test/markers/check_markers_visibility.py b/source/isaaclab/test/markers/check_markers_visibility.py index 735cd9b9e976..98dbee8ddcd7 100644 --- a/source/isaaclab/test/markers/check_markers_visibility.py +++ b/source/isaaclab/test/markers/check_markers_visibility.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -70,7 +70,7 @@ class SensorsSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Robot/base", update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/defaultGroundPlane"], diff --git a/source/isaaclab/test/markers/test_visualization_markers.py b/source/isaaclab/test/markers/test_visualization_markers.py index 930f5c45663a..ebc183b804b8 100644 --- a/source/isaaclab/test/markers/test_visualization_markers.py +++ b/source/isaaclab/test/markers/test_visualization_markers.py @@ -1,207 +1,203 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -config = {"headless": True} -simulation_app = AppLauncher(config).app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" +import pytest import torch -import unittest - -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext import isaaclab.sim as sim_utils from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg from isaaclab.markers.config import FRAME_MARKER_CFG, POSITION_GOAL_MARKER_CFG +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils.math import random_orientation from isaaclab.utils.timer import Timer -class TestUsdVisualizationMarkers(unittest.TestCase): - """Test fixture for the VisualizationMarker class.""" - - def setUp(self): - """Create a blank new stage for each test.""" - # Simulation time-step - self.dt = 0.01 - # Open a new stage - stage_utils.create_new_stage() - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="torch", device="cuda:0") - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - # close stage - stage_utils.close_stage() - # clear the simulation context - self.sim.clear_instance() - - def test_instantiation(self): - """Test that the class can be initialized properly.""" - config = VisualizationMarkersCfg( - prim_path="/World/Visuals/test", - markers={ - "test": sim_utils.SphereCfg(radius=1.0), - }, - ) - test_marker = VisualizationMarkers(config) - print(test_marker) - # check number of markers - self.assertEqual(test_marker.num_prototypes, 1) - - def test_usd_marker(self): - """Test with marker from a USD.""" - # create a marker - config = FRAME_MARKER_CFG.replace(prim_path="/World/Visuals/test_frames") - test_marker = VisualizationMarkers(config) - - # play the simulation - self.sim.reset() - # create a buffer - num_frames = 0 - # run with randomization of poses - for count in range(1000): - # sample random poses - if count % 50 == 0: - num_frames = torch.randint(10, 1000, (1,)).item() - frame_translations = torch.randn((num_frames, 3)) - frame_rotations = random_orientation(num_frames, device=self.sim.device) - # set the marker - test_marker.visualize(translations=frame_translations, orientations=frame_rotations) - # update the kit - self.sim.step() - # asset that count is correct - self.assertEqual(test_marker.count, num_frames) - - def test_usd_marker_color(self): - """Test with marker from a USD with its color modified.""" - # create a marker - config = FRAME_MARKER_CFG.copy() - config.prim_path = "/World/Visuals/test_frames" - config.markers["frame"].visual_material = sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)) - test_marker = VisualizationMarkers(config) - - # play the simulation - self.sim.reset() - # run with randomization of poses - for count in range(1000): - # sample random poses - if count % 50 == 0: - num_frames = torch.randint(10, 1000, (1,)).item() - frame_translations = torch.randn((num_frames, 3)) - frame_rotations = random_orientation(num_frames, device=self.sim.device) - # set the marker - test_marker.visualize(translations=frame_translations, orientations=frame_rotations) - # update the kit - self.sim.step() - - def test_multiple_prototypes_marker(self): - """Test with multiple prototypes of spheres.""" - # create a marker - config = POSITION_GOAL_MARKER_CFG.replace(prim_path="/World/Visuals/test_protos") - test_marker = VisualizationMarkers(config) - - # play the simulation - self.sim.reset() - # run with randomization of poses - for count in range(1000): - # sample random poses - if count % 50 == 0: - num_frames = torch.randint(100, 1000, (1,)).item() - frame_translations = torch.randn((num_frames, 3)) - # randomly choose a prototype - marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,)) - # set the marker - test_marker.visualize(translations=frame_translations, marker_indices=marker_indices) - # update the kit - self.sim.step() - - def test_visualization_time_based_on_prototypes(self): - """Test with time taken when number of prototypes is increased.""" - - # create a marker - config = POSITION_GOAL_MARKER_CFG.replace(prim_path="/World/Visuals/test_protos") - test_marker = VisualizationMarkers(config) - - # play the simulation - self.sim.reset() - # number of frames - num_frames = 4096 - - # check that visibility is true - self.assertTrue(test_marker.is_visible()) - # run with randomization of poses and indices - frame_translations = torch.randn((num_frames, 3)) - marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,)) - # set the marker - with Timer("Marker visualization with explicit indices") as timer: - test_marker.visualize(translations=frame_translations, marker_indices=marker_indices) - # save the time - time_with_marker_indices = timer.time_elapsed - - with Timer("Marker visualization with no indices") as timer: - test_marker.visualize(translations=frame_translations) - # save the time - time_with_no_marker_indices = timer.time_elapsed - +@pytest.fixture +def sim(): + """Create a blank new stage for each test.""" + # Simulation time-step + dt = 0.01 + # Open a new stage + sim_utils.create_new_stage() + # Load kit helper + sim_context = SimulationContext(SimulationCfg(dt=dt)) + yield sim_context + # Cleanup + sim_context._disable_app_control_on_stop_handle = True # prevent timeout + sim_context.stop() + sim_context.clear_instance() + sim_utils.close_stage() + + +def test_instantiation(sim): + """Test that the class can be initialized properly.""" + config = VisualizationMarkersCfg( + prim_path="/World/Visuals/test", + markers={ + "test": sim_utils.SphereCfg(radius=1.0), + }, + ) + test_marker = VisualizationMarkers(config) + print(test_marker) + # check number of markers + assert test_marker.num_prototypes == 1 + + +def test_usd_marker(sim): + """Test with marker from a USD.""" + # create a marker + config = FRAME_MARKER_CFG.copy() + config.prim_path = "/World/Visuals/test_frames" + test_marker = VisualizationMarkers(config) + + # play the simulation + sim.reset() + # create a buffer + num_frames = 0 + # run with randomization of poses + for count in range(1000): + # sample random poses + if count % 50 == 0: + num_frames = torch.randint(10, 1000, (1,)).item() + frame_translations = torch.randn(num_frames, 3, device=sim.device) + frame_rotations = random_orientation(num_frames, device=sim.device) + # set the marker + test_marker.visualize(translations=frame_translations, orientations=frame_rotations) # update the kit - self.sim.step() - # check that the time is less - self.assertLess(time_with_no_marker_indices, time_with_marker_indices) - - def test_visualization_time_based_on_visibility(self): - """Test with visibility of markers. When invisible, the visualize call should return.""" - - # create a marker - config = POSITION_GOAL_MARKER_CFG.replace(prim_path="/World/Visuals/test_protos") - test_marker = VisualizationMarkers(config) - - # play the simulation - self.sim.reset() - # number of frames - num_frames = 4096 - - # check that visibility is true - self.assertTrue(test_marker.is_visible()) - # run with randomization of poses and indices - frame_translations = torch.randn((num_frames, 3)) - marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,)) - # set the marker - with Timer("Marker visualization") as timer: - test_marker.visualize(translations=frame_translations, marker_indices=marker_indices) - # save the time - time_with_visualization = timer.time_elapsed - + sim.step() + # asset that count is correct + assert test_marker.count == num_frames + + +def test_usd_marker_color(sim): + """Test with marker from a USD with its color modified.""" + # create a marker + config = FRAME_MARKER_CFG.copy() + config.prim_path = "/World/Visuals/test_frames" + config.markers["frame"].visual_material = sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)) + test_marker = VisualizationMarkers(config) + + # play the simulation + sim.reset() + # run with randomization of poses + for count in range(1000): + # sample random poses + if count % 50 == 0: + num_frames = torch.randint(10, 1000, (1,)).item() + frame_translations = torch.randn(num_frames, 3, device=sim.device) + frame_rotations = random_orientation(num_frames, device=sim.device) + # set the marker + test_marker.visualize(translations=frame_translations, orientations=frame_rotations) # update the kit - self.sim.step() - # make invisible - test_marker.set_visibility(False) - - # check that visibility is false - self.assertFalse(test_marker.is_visible()) - # run with randomization of poses and indices - frame_translations = torch.randn((num_frames, 3)) - marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,)) - # set the marker - with Timer("Marker no visualization") as timer: + sim.step() + + +def test_multiple_prototypes_marker(sim): + """Test with multiple prototypes of spheres.""" + # create a marker + config = POSITION_GOAL_MARKER_CFG.copy() + config.prim_path = "/World/Visuals/test_protos" + test_marker = VisualizationMarkers(config) + + # play the simulation + sim.reset() + # run with randomization of poses + for count in range(1000): + # sample random poses + if count % 50 == 0: + num_frames = torch.randint(100, 1000, (1,)).item() + frame_translations = torch.randn(num_frames, 3, device=sim.device) + # randomly choose a prototype + marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,), device=sim.device) + # set the marker test_marker.visualize(translations=frame_translations, marker_indices=marker_indices) - # save the time - time_with_no_visualization = timer.time_elapsed - - # check that the time is less - self.assertLess(time_with_no_visualization, time_with_visualization) - - -if __name__ == "__main__": - run_tests() + # update the kit + sim.step() + + +def test_visualization_time_based_on_prototypes(sim): + """Test with time taken when number of prototypes is increased.""" + # create a marker + config = POSITION_GOAL_MARKER_CFG.copy() + config.prim_path = "/World/Visuals/test_protos" + test_marker = VisualizationMarkers(config) + + # play the simulation + sim.reset() + # number of frames + num_frames = 4096 + + # check that visibility is true + assert test_marker.is_visible() + # run with randomization of poses and indices + frame_translations = torch.randn(num_frames, 3, device=sim.device) + marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,), device=sim.device) + # set the marker + with Timer("Marker visualization with explicit indices") as timer: + test_marker.visualize(translations=frame_translations, marker_indices=marker_indices) + # save the time + time_with_marker_indices = timer.time_elapsed + + with Timer("Marker visualization with no indices") as timer: + test_marker.visualize(translations=frame_translations) + # save the time + time_with_no_marker_indices = timer.time_elapsed + + # update the kit + sim.step() + # check that the time is less + assert time_with_no_marker_indices < time_with_marker_indices + + +def test_visualization_time_based_on_visibility(sim): + """Test with visibility of markers. When invisible, the visualize call should return.""" + # create a marker + config = POSITION_GOAL_MARKER_CFG.copy() + config.prim_path = "/World/Visuals/test_protos" + test_marker = VisualizationMarkers(config) + + # play the simulation + sim.reset() + # number of frames + num_frames = 4096 + + # check that visibility is true + assert test_marker.is_visible() + # run with randomization of poses and indices + frame_translations = torch.randn(num_frames, 3, device=sim.device) + marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,), device=sim.device) + # set the marker + with Timer("Marker visualization") as timer: + test_marker.visualize(translations=frame_translations, marker_indices=marker_indices) + # save the time + time_with_visualization = timer.time_elapsed + + # update the kit + sim.step() + # make invisible + test_marker.set_visibility(False) + + # check that visibility is false + assert not test_marker.is_visible() + # run with randomization of poses and indices + frame_translations = torch.randn(num_frames, 3, device=sim.device) + marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,), device=sim.device) + # set the marker + with Timer("Marker no visualization") as timer: + test_marker.visualize(translations=frame_translations, marker_indices=marker_indices) + # save the time + time_with_no_visualization = timer.time_elapsed + + # check that the time is less + assert time_with_no_visualization < time_with_visualization diff --git a/source/isaaclab/test/performance/test_kit_startup_performance.py b/source/isaaclab/test/performance/test_kit_startup_performance.py index 78ecebaaffa8..f4134d04ae14 100644 --- a/source/isaaclab/test/performance/test_kit_startup_performance.py +++ b/source/isaaclab/test/performance/test_kit_startup_performance.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,24 +9,15 @@ from __future__ import annotations import time -import unittest -from isaaclab.app import run_tests +from isaaclab.app import AppLauncher -class TestKitStartUpPerformance(unittest.TestCase): - """Test kit startup performance.""" - - def test_kit_start_up_time(self): - """Test kit start-up time.""" - from isaaclab.app import AppLauncher - - start_time = time.time() - self.app_launcher = AppLauncher(headless=True).app - end_time = time.time() - elapsed_time = end_time - start_time - self.assertLessEqual(elapsed_time, 10.0) - - -if __name__ == "__main__": - run_tests() +def test_kit_start_up_time(): + """Test kit start-up time.""" + start_time = time.time() + app_launcher = AppLauncher(headless=True).app # noqa: F841 + end_time = time.time() + elapsed_time = end_time - start_time + # we are doing some more imports on the automate side - will investigate using warp instead of numba cuda + assert elapsed_time <= 12.0 diff --git a/source/isaaclab/test/performance/test_robot_load_performance.py b/source/isaaclab/test/performance/test_robot_load_performance.py index 69f4ad76587a..42d5f1c4fffb 100644 --- a/source/isaaclab/test/performance/test_robot_load_performance.py +++ b/source/isaaclab/test/performance/test_robot_load_performance.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,58 +8,47 @@ from __future__ import annotations -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app -import unittest +import pytest import omni from isaacsim.core.cloner import GridCloner -from isaaclab_assets import ANYMAL_D_CFG, CARTPOLE_CFG - from isaaclab.assets import Articulation from isaaclab.sim import build_simulation_context from isaaclab.utils.timer import Timer - -class TestRobotLoadPerformance(unittest.TestCase): - """Test robot load performance.""" - - """ - Tests - """ - - def test_robot_load_performance(self): - """Test robot load time.""" - test_configs = { - "Cartpole": {"robot_cfg": CARTPOLE_CFG, "expected_load_time": 10.0}, - "Anymal_D": {"robot_cfg": ANYMAL_D_CFG, "expected_load_time": 40.0}, - } - for test_config in test_configs.items(): - for device in ("cuda:0", "cpu"): - with self.subTest(test_config=test_config, device=device): - with build_simulation_context(device=device) as sim: - sim._app_control_on_stop_handle = None - cloner = GridCloner(spacing=2) - target_paths = cloner.generate_paths("/World/Robots", 4096) - omni.usd.get_context().get_stage().DefinePrim(target_paths[0], "Xform") - _ = cloner.clone( - source_prim_path=target_paths[0], - prim_paths=target_paths, - replicate_physics=False, - copy_from_source=True, - ) - with Timer(f"{test_config[0]} load time for device {device}") as timer: - robot = Articulation( # noqa: F841 - test_config[1]["robot_cfg"].replace(prim_path="/World/Robots_.*/Robot") - ) - sim.reset() - elapsed_time = timer.time_elapsed - self.assertLessEqual(elapsed_time, test_config[1]["expected_load_time"]) +from isaaclab_assets import ANYMAL_D_CFG, CARTPOLE_CFG -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize( + "test_config,device", + [ + ({"name": "Cartpole", "robot_cfg": CARTPOLE_CFG, "expected_load_time": 10.0}, "cuda:0"), + ({"name": "Cartpole", "robot_cfg": CARTPOLE_CFG, "expected_load_time": 10.0}, "cpu"), + ({"name": "Anymal_D", "robot_cfg": ANYMAL_D_CFG, "expected_load_time": 40.0}, "cuda:0"), + ({"name": "Anymal_D", "robot_cfg": ANYMAL_D_CFG, "expected_load_time": 40.0}, "cpu"), + ], +) +def test_robot_load_performance(test_config, device): + """Test robot load time.""" + with build_simulation_context(device=device) as sim: + sim._app_control_on_stop_handle = None + cloner = GridCloner(spacing=2) + target_paths = cloner.generate_paths("/World/Robots", 4096) + omni.usd.get_context().get_stage().DefinePrim(target_paths[0], "Xform") + _ = cloner.clone( + source_prim_path=target_paths[0], + prim_paths=target_paths, + replicate_physics=False, + copy_from_source=True, + ) + with Timer(f"{test_config['name']} load time for device {device}") as timer: + robot = Articulation(test_config["robot_cfg"].replace(prim_path="/World/Robots_.*/Robot")) # noqa: F841 + sim.reset() + elapsed_time = timer.time_elapsed + assert elapsed_time <= test_config["expected_load_time"] diff --git a/source/isaaclab/test/scene/check_interactive_scene.py b/source/isaaclab/test/scene/check_interactive_scene.py index 858d37bf579c..5b2463b315a9 100644 --- a/source/isaaclab/test/scene/check_interactive_scene.py +++ b/source/isaaclab/test/scene/check_interactive_scene.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -62,7 +62,7 @@ class MySceneCfg(InteractiveSceneCfg): 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, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], diff --git a/source/isaaclab/test/scene/test_interactive_scene.py b/source/isaaclab/test/scene/test_interactive_scene.py index 2eb612bd3426..1a42a340baa1 100644 --- a/source/isaaclab/test/scene/test_interactive_scene.py +++ b/source/isaaclab/test/scene/test_interactive_scene.py @@ -1,18 +1,19 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest +import pytest +import torch import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg @@ -20,7 +21,6 @@ from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors import ContactSensorCfg from isaaclab.sim import build_simulation_context -from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -29,23 +29,19 @@ class MySceneCfg(InteractiveSceneCfg): """Example scene configuration.""" - # terrain - flat terrain plane - terrain = TerrainImporterCfg( - prim_path="/World/ground", - terrain_type="plane", - ) - # articulation robot = ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd"), + prim_path="/World/envs/env_.*/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd" + ), actuators={ "joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=100.0, damping=1.0), }, ) # rigid object rigid_obj = RigidObjectCfg( - prim_path="/World/RigidObj", + prim_path="/World/envs/env_.*/RigidObj", spawn=sim_utils.CuboidCfg( size=(0.5, 0.5, 0.5), rigid_props=sim_utils.RigidBodyPropertiesCfg( @@ -57,56 +53,164 @@ class MySceneCfg(InteractiveSceneCfg): ), ) - # sensor - sensor = ContactSensorCfg( - prim_path="/World/Robot", + +@pytest.fixture +def setup_scene(request): + """Create simulation context with the specified device.""" + device = request.getfixturevalue("device") + with build_simulation_context(device=device, auto_add_lighting=True, add_ground_plane=True) as sim: + sim._app_control_on_stop_handle = None + + def make_scene(num_envs: int, env_spacing: float = 1.0): + scene_cfg = MySceneCfg(num_envs=num_envs, env_spacing=env_spacing) + return scene_cfg + + yield make_scene, sim + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_scene_entity_isolation(device, setup_scene): + """Tests that multiple instances of InteractiveScene do not share any data. + + In this test, two InteractiveScene instances are created in a loop and added to a list. + The scene at index 0 of the list will have all of its entities cleared manually, and + the test compares that the data held in the scene at index 1 remained intact. + """ + make_scene, sim = setup_scene + scene_cfg = make_scene(num_envs=1) + # set additional light to test 'extras' attribute of the scene + setattr( + scene_cfg, + "light", + AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(), + ), ) - # extras - light - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DistantLightCfg(), + # set additional sensor to test 'sensors' attribute of the scene + setattr(scene_cfg, "sensor", ContactSensorCfg(prim_path="/World/envs/env_.*/Robot")) + + scene_list = [] + # create two InteractiveScene instances + for _ in range(2): + with build_simulation_context(device=device, dt=sim.get_physics_dt()) as _: + scene = InteractiveScene(scene_cfg) + scene_list.append(scene) + scene_0 = scene_list[0] + scene_1 = scene_list[1] + # clear entities for scene_0 - this should not affect any data in scene_1 + scene_0.articulations.clear() + scene_0.rigid_objects.clear() + scene_0.sensors.clear() + scene_0.extras.clear() + # check that scene_0 and scene_1 do not share entity data via dictionary comparison + assert scene_0.articulations == dict() + assert scene_0.articulations != scene_1.articulations + assert scene_0.rigid_objects == dict() + assert scene_0.rigid_objects != scene_1.rigid_objects + assert scene_0.sensors == dict() + assert scene_0.sensors != scene_1.sensors + assert scene_0.extras == dict() + assert scene_0.extras != scene_1.extras + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_relative_flag(device, setup_scene): + make_scene, sim = setup_scene + scene_cfg = make_scene(num_envs=4) + scene = InteractiveScene(scene_cfg) + sim.reset() + + # test relative == False produces different result than relative == True + assert_state_different(scene.get_state(is_relative=False), scene.get_state(is_relative=True)) + + # test is relative == False + prev_state = scene.get_state(is_relative=False) + scene["robot"].write_joint_state_to_sim( + position=torch.rand_like(scene["robot"].data.joint_pos), velocity=torch.rand_like(scene["robot"].data.joint_pos) ) + next_state = scene.get_state(is_relative=False) + assert_state_different(prev_state, next_state) + scene.reset_to(prev_state, is_relative=False) + assert_state_equal(prev_state, scene.get_state(is_relative=False)) + + # test is relative == True + prev_state = scene.get_state(is_relative=True) + scene["robot"].write_joint_state_to_sim( + position=torch.rand_like(scene["robot"].data.joint_pos), velocity=torch.rand_like(scene["robot"].data.joint_pos) + ) + next_state = scene.get_state(is_relative=True) + assert_state_different(prev_state, next_state) + scene.reset_to(prev_state, is_relative=True) + assert_state_equal(prev_state, scene.get_state(is_relative=True)) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_reset_to_env_ids_input_types(device, setup_scene): + make_scene, sim = setup_scene + scene_cfg = make_scene(num_envs=4) + scene = InteractiveScene(scene_cfg) + sim.reset() + + # test env_ids = None + prev_state = scene.get_state() + scene["robot"].write_joint_state_to_sim( + position=torch.rand_like(scene["robot"].data.joint_pos), velocity=torch.rand_like(scene["robot"].data.joint_pos) + ) + scene.reset_to(prev_state, env_ids=None) + assert_state_equal(prev_state, scene.get_state()) - -class TestInteractiveScene(unittest.TestCase): - """Test cases for InteractiveScene.""" - - def setUp(self) -> None: - self.devices = ["cuda:0", "cpu"] - self.sim_dt = 0.001 - self.scene_cfg = MySceneCfg(num_envs=1, env_spacing=1) - - def test_scene_entity_isolation(self): - """Tests that multiple instances of InteractiveScene does not share any data. - - In this test, two InteractiveScene instances are created in a loop and added to a list. - The scene at index 0 of the list will have all of its entities cleared manually, and - the test compares that the data held in the scene at index 1 remained intact. - """ - for device in self.devices: - scene_list = [] - # create two InteractiveScene instances - for _ in range(2): - with build_simulation_context(device=device, dt=self.sim_dt) as _: - scene = InteractiveScene(MySceneCfg(num_envs=1, env_spacing=1)) - scene_list.append(scene) - scene_0 = scene_list[0] - scene_1 = scene_list[1] - # clear entities for scene_0 - this should not affect any data in scene_1 - scene_0.articulations.clear() - scene_0.rigid_objects.clear() - scene_0.sensors.clear() - scene_0.extras.clear() - # check that scene_0 and scene_1 do not share entity data via dictionary comparison - self.assertEqual(scene_0.articulations, dict()) - self.assertNotEqual(scene_0.articulations, scene_1.articulations) - self.assertEqual(scene_0.rigid_objects, dict()) - self.assertNotEqual(scene_0.rigid_objects, scene_1.rigid_objects) - self.assertEqual(scene_0.sensors, dict()) - self.assertNotEqual(scene_0.sensors, scene_1.sensors) - self.assertEqual(scene_0.extras, dict()) - self.assertNotEqual(scene_0.extras, scene_1.extras) - - -if __name__ == "__main__": - run_tests() + # test env_ids = torch tensor + scene["robot"].write_joint_state_to_sim( + position=torch.rand_like(scene["robot"].data.joint_pos), velocity=torch.rand_like(scene["robot"].data.joint_pos) + ) + scene.reset_to(prev_state, env_ids=torch.arange(scene.num_envs, device=scene.device)) + assert_state_equal(prev_state, scene.get_state()) + + +def assert_state_equal(s1: dict, s2: dict, path=""): + """ + Recursively assert that s1 and s2 have the same nested keys + and that every tensor leaf is exactly equal. + """ + assert set(s1.keys()) == set(s2.keys()), f"Key mismatch at {path}: {s1.keys()} vs {s2.keys()}" + for k in s1: + v1, v2 = s1[k], s2[k] + subpath = f"{path}.{k}" if path else k + if isinstance(v1, dict): + assert isinstance(v2, dict), f"Type mismatch at {subpath}" + assert_state_equal(v1, v2, path=subpath) + else: + # leaf: should be a torch.Tensor + assert isinstance(v1, torch.Tensor) and isinstance(v2, torch.Tensor), f"Expected tensors at {subpath}" + if not torch.equal(v1, v2): + diff = (v1 - v2).abs().max() + pytest.fail(f"Tensor mismatch at {subpath}, max abs diff = {diff}") + + +def assert_state_different(s1: dict, s2: dict, path=""): + """ + Recursively scan s1 and s2 (which must have identical keys) and + succeed as soon as you find one tensor leaf that differs. + If you reach the end with everything equal, fail the test. + """ + assert set(s1.keys()) == set(s2.keys()), f"Key mismatch at {path}: {s1.keys()} vs {s2.keys()}" + for k in s1: + v1, v2 = s1[k], s2[k] + subpath = f"{path}.{k}" if path else k + if isinstance(v1, dict): + # recurse; if any nested call returns (i.e. finds a diff), we propagate success + try: + assert_state_different(v1, v2, path=subpath) + return + except AssertionError: + continue + else: + assert isinstance(v1, torch.Tensor) and isinstance(v2, torch.Tensor), f"Expected tensors at {subpath}" + if not torch.equal(v1, v2): + return # found a difference โ†’ success + pytest.fail(f"No differing tensor found in nested state at {path}") diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index 50e2dbec76d1..b4fe5f555dc7 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -20,7 +20,7 @@ # add argparse arguments parser = argparse.ArgumentParser(description="Contact Sensor Test Script") -parser.add_argument("--num_robots", type=int, default=64, help="Number of robots to spawn.") +parser.add_argument("--num_robots", type=int, default=128, help="Number of robots to spawn.") # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) @@ -36,15 +36,13 @@ import torch -import isaacsim.core.utils.prims as prim_utils -from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner -from isaacsim.core.utils.carb import set_carb_setting -from isaacsim.core.utils.viewports import set_camera_view import isaaclab.sim as sim_utils from isaaclab.assets import Articulation from isaaclab.sensors.contact_sensor import ContactSensor, ContactSensorCfg +from isaaclab.sim import SimulationCfg, SimulationContext +from isaaclab.utils.timer import Timer ## # Pre-defined configs @@ -63,9 +61,8 @@ def design_scene(): cfg = sim_utils.GroundPlaneCfg() cfg.func("/World/defaultGroundPlane", cfg) # Lights - cfg = sim_utils.SphereLightCfg() - cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) - cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) + cfg = sim_utils.DomeLightCfg(intensity=2000) + cfg.func("/World/Light/DomeLight", cfg, translation=(-4.5, 3.5, 10.0)) """ @@ -77,19 +74,19 @@ def main(): """Spawns the ANYmal robot and clones it using Isaac Sim Cloner API.""" # Load kit helper - sim = SimulationContext(physics_dt=0.005, rendering_dt=0.005, backend="torch", device="cuda:0") + sim = SimulationContext(SimulationCfg(dt=0.005)) # Set main camera - set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) # Enable hydra scene-graph instancing # this is needed to visualize the scene when flatcache is enabled - set_carb_setting(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + sim._settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) # Create interface to clone the scene cloner = GridCloner(spacing=2.0) cloner.define_base_env("/World/envs") # Everything under the namespace "/World/envs/env_0" will be cloned - prim_utils.define_prim("/World/envs/env_0") + sim.stage.DefinePrim("/World/envs/env_0", "Xform") # Clone the scene num_envs = args_cli.num_robots cloner.define_base_env("/World/envs") @@ -103,7 +100,12 @@ def main(): robot = Articulation(cfg=robot_cfg) # Contact sensor contact_sensor_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/Robot/.*_SHANK", track_air_time=True, debug_vis=not args_cli.headless + prim_path="/World/envs/env_.*/Robot/.*_FOOT", + track_air_time=True, + track_contact_points=True, + track_friction_forces=True, + debug_vis=False, # not args_cli.headless, + filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"], ) contact_sensor = ContactSensor(cfg=contact_sensor_cfg) # filter collisions within each environment instance @@ -126,6 +128,7 @@ def main(): sim_dt = decimation * physics_dt sim_time = 0.0 count = 0 + dt = [] # Simulate physics while simulation_app.is_running(): # If simulation is stopped, then exit. @@ -136,14 +139,20 @@ def main(): sim.step(render=False) continue # reset - if count % 1000 == 0: + if count % 1000 == 0 and count != 0: # reset counters sim_time = 0.0 count = 0 + print("=" * 80) + print("avg dt real-time", sum(dt) / len(dt)) + print("=" * 80) + # reset dof state joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel robot.write_joint_state_to_sim(joint_pos, joint_vel) robot.reset() + dt = [] + # perform 4 steps for _ in range(decimation): # apply actions @@ -159,6 +168,10 @@ def main(): count += 1 # update the buffers if sim.is_playing(): + with Timer() as timer: + contact_sensor.update(sim_dt, force_recompute=True) + dt.append(timer.time_elapsed) + contact_sensor.update(sim_dt, force_recompute=True) if count % 100 == 0: print("Sim-time: ", sim_time) diff --git a/source/isaaclab/test/sensors/check_imu_sensor.py b/source/isaaclab/test/sensors/check_imu_sensor.py index 184a2acac4e7..8a8c048ed62d 100644 --- a/source/isaaclab/test/sensors/check_imu_sensor.py +++ b/source/isaaclab/test/sensors/check_imu_sensor.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,12 +35,12 @@ """Rest everything follows.""" -import torch +import logging import traceback -import carb +import torch + import omni -from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.utils.viewports import set_camera_view from pxr import PhysxSchema @@ -49,11 +49,15 @@ import isaaclab.terrains as terrain_gen from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.sensors.imu import Imu, ImuCfg +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG from isaaclab.terrains.terrain_importer import TerrainImporter from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.timer import Timer +# import logger +logger = logging.getLogger(__name__) + def design_scene(sim: SimulationContext, num_envs: int = 2048) -> RigidObject: """Design the scene.""" @@ -100,7 +104,7 @@ def design_scene(sim: SimulationContext, num_envs: int = 2048) -> RigidObject: for prim in stage.Traverse(): if prim.HasAPI(PhysxSchema.PhysxSceneAPI): physics_scene_prim_path = prim.GetPrimPath() - carb.log_info(f"Physics scene prim path: {physics_scene_prim_path}") + logging.info(f"Physics scene prim path: {physics_scene_prim_path}") break # filter collisions within each environment instance cloner.filter_collisions( @@ -115,16 +119,7 @@ def main(): """Main function.""" # Load kit helper - sim_params = { - "use_gpu": True, - "use_gpu_pipeline": True, - "use_flatcache": True, # deprecated from Isaac Sim 2023.1 onwards - "use_fabric": True, # used from Isaac Sim 2023.1 onwards - "enable_scene_query_support": True, - } - sim = SimulationContext( - physics_dt=1.0 / 60.0, rendering_dt=1.0 / 60.0, sim_params=sim_params, backend="torch", device="cuda:0" - ) + sim = SimulationContext(SimulationCfg()) # Set main camera set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5]) @@ -188,8 +183,8 @@ def main(): # Run the main function main() except Exception as err: - carb.log_error(err) - carb.log_error(traceback.format_exc()) + logger.error(err) + logger.error(traceback.format_exc()) raise finally: # close sim app diff --git a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py new file mode 100644 index 000000000000..73750d0de874 --- /dev/null +++ b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py @@ -0,0 +1,206 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +""" +This script shows how to use the multi-mesh ray caster from the Isaac Lab framework. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py --headless + +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Ray Caster Test Script") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to clone.") +parser.add_argument("--num_objects", type=int, default=0, help="Number of additional objects to clone.") +parser.add_argument( + "--terrain_type", + type=str, + default="generator", + help="Type of terrain to import. Can be 'generator' or 'usd' or 'plane'.", +) +# 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 random + +import torch + +from isaacsim.core.cloner import GridCloner + +import isaaclab.sim as sim_utils +import isaaclab.terrains as terrain_gen +from isaaclab.assets import RigidObject, RigidObjectCfg +from isaaclab.sensors.ray_caster import MultiMeshRayCaster, MultiMeshRayCasterCfg, patterns +from isaaclab.sim import SimulationCfg, SimulationContext +from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG +from isaaclab.terrains.terrain_importer import TerrainImporter +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import quat_from_euler_xyz +from isaaclab.utils.timer import Timer + + +def design_scene(sim: SimulationContext, num_envs: int = 2048): + """Design the scene.""" + # Create interface to clone the scene + cloner = GridCloner(spacing=10.0) + cloner.define_base_env("/World/envs") + # Everything under the namespace "/World/envs/env_0" will be cloned + sim.stage.DefinePrim("/World/envs/env_0", "Xform") + # Define the scene + # -- Light + cfg = sim_utils.DistantLightCfg(intensity=2000) + cfg.func("/World/light", cfg) + # -- Balls + cfg = sim_utils.SphereCfg( + radius=0.25, + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ) + cfg.func("/World/envs/env_0/ball", cfg, translation=(0.0, 0.0, 5.0)) + + for i in range(args_cli.num_objects): + object = sim_utils.CuboidCfg( + size=(0.5 + random.random() * 0.5, 0.5 + random.random() * 0.5, 0.1 + random.random() * 0.05), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg( + diffuse_color=(0.0 + i / args_cli.num_objects, 0.0, 1.0 - i / args_cli.num_objects) + ), + ) + object.func( + f"/World/envs/env_0/object_{i}", + object, + translation=(0.0 + random.random(), 0.0 + random.random(), 1.0), + orientation=quat_from_euler_xyz(torch.Tensor(0), torch.Tensor(0), torch.rand(1) * torch.pi).numpy(), + ) + + # Clone the scene + cloner.define_base_env("/World/envs") + envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) + cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) + physics_scene_path = sim.get_physics_context().prim_path + cloner.filter_collisions( + physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] + ) + + +def main(): + """Main function.""" + # Load kit helper + sim = SimulationContext(SimulationCfg()) + # Set main camera + sim.set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5]) + + # Parameters + num_envs = args_cli.num_envs + # Design the scene + design_scene(sim=sim, num_envs=num_envs) + # Handler for terrains importing + terrain_importer_cfg = terrain_gen.TerrainImporterCfg( + prim_path="/World/ground", + terrain_type=args_cli.terrain_type, + terrain_generator=ROUGH_TERRAINS_CFG, + usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd", + max_init_terrain_level=0, + num_envs=1, + ) + _ = TerrainImporter(terrain_importer_cfg) + + mesh_targets: list[MultiMeshRayCasterCfg.RaycastTargetCfg] = [ + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/ground", track_mesh_transforms=False), + ] + if args_cli.num_objects != 0: + mesh_targets.append( + MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_.*/object_.*", track_mesh_transforms=True) + ) + # Create a ray-caster sensor + ray_caster_cfg = MultiMeshRayCasterCfg( + prim_path="/World/envs/env_.*/ball", + mesh_prim_paths=mesh_targets, + pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(1.6, 1.0)), + attach_yaw_only=True, + debug_vis=not args_cli.headless, + ) + ray_caster = MultiMeshRayCaster(cfg=ray_caster_cfg) + # Create a view over all the balls + balls_cfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/ball", + spawn=None, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 5.0)), + ) + balls = RigidObject(cfg=balls_cfg) + + # Play simulator + sim.reset() + + # Initialize the views + # -- balls + print(balls) + # Print the sensor information + print(ray_caster) + + # Get the initial positions of the balls + ball_initial_poses = balls.data.root_pose_w.clone() + ball_initial_velocities = balls.data.root_vel_w.clone() + + # Create a counter for resetting the scene + step_count = 0 + # Simulate physics + while simulation_app.is_running(): + # If simulation is stopped, then exit. + if sim.is_stopped(): + break + # If simulation is paused, then skip. + if not sim.is_playing(): + sim.step(render=False) + continue + # Reset the scene + if step_count % 500 == 0: + # sample random indices to reset + reset_indices = torch.randint(0, num_envs, (num_envs // 2,), device=sim.device) + # reset the balls + balls.write_root_pose_to_sim(ball_initial_poses[reset_indices], env_ids=reset_indices) + balls.write_root_velocity_to_sim(ball_initial_velocities[reset_indices], env_ids=reset_indices) + balls.reset(reset_indices) + # reset the sensor + ray_caster.reset(reset_indices) + # reset the counter + step_count = 0 + # Step simulation + sim.step() + # Update the ray-caster + with Timer(f"Ray-caster update with {num_envs} x {ray_caster.num_rays} rays"): + ray_caster.update(dt=sim.get_physics_dt(), force_recompute=True) + # Update counter + step_count += 1 + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index 95013a7ddbdf..78f314fdebd6 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -41,15 +41,13 @@ import torch -import isaacsim.core.utils.prims as prim_utils -from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner -from isaacsim.core.prims import RigidPrim -from isaacsim.core.utils.viewports import set_camera_view import isaaclab.sim as sim_utils import isaaclab.terrains as terrain_gen +from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.sensors.ray_caster import RayCaster, RayCasterCfg, patterns +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG from isaaclab.terrains.terrain_importer import TerrainImporter from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -62,7 +60,7 @@ def design_scene(sim: SimulationContext, num_envs: int = 2048): cloner = GridCloner(spacing=2.0) cloner.define_base_env("/World/envs") # Everything under the namespace "/World/envs/env_0" will be cloned - prim_utils.define_prim("/World/envs/env_0") + sim.stage.DefinePrim("/World/envs/env_0", "Xform") # Define the scene # -- Light cfg = sim_utils.DistantLightCfg(intensity=2000) @@ -89,19 +87,9 @@ def design_scene(sim: SimulationContext, num_envs: int = 2048): def main(): """Main function.""" - # Load kit helper - sim_params = { - "use_gpu": True, - "use_gpu_pipeline": True, - "use_flatcache": True, # deprecated from Isaac Sim 2023.1 onwards - "use_fabric": True, # used from Isaac Sim 2023.1 onwards - "enable_scene_query_support": True, - } - sim = SimulationContext( - physics_dt=1.0 / 60.0, rendering_dt=1.0 / 60.0, sim_params=sim_params, backend="torch", device="cuda:0" - ) + sim = SimulationContext(SimulationCfg()) # Set main camera - set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5]) + sim.set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5]) # Parameters num_envs = args_cli.num_envs @@ -123,25 +111,30 @@ def main(): prim_path="/World/envs/env_.*/ball", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(1.6, 1.0)), - attach_yaw_only=True, + ray_alignment="yaw", debug_vis=not args_cli.headless, ) ray_caster = RayCaster(cfg=ray_caster_cfg) # Create a view over all the balls - ball_view = RigidPrim("/World/envs/env_.*/ball", reset_xform_properties=False) + balls_cfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/ball", + spawn=None, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 5.0)), + ) + balls = RigidObject(cfg=balls_cfg) # Play simulator sim.reset() # Initialize the views # -- balls - ball_view.initialize() + print(balls) # Print the sensor information print(ray_caster) # Get the initial positions of the balls - ball_initial_positions, ball_initial_orientations = ball_view.get_world_poses() - ball_initial_velocities = ball_view.get_velocities() + ball_initial_poses = balls.data.root_pose_w.clone() + ball_initial_velocities = balls.data.root_vel_w.clone() # Create a counter for resetting the scene step_count = 0 @@ -157,12 +150,11 @@ def main(): # Reset the scene if step_count % 500 == 0: # sample random indices to reset - reset_indices = torch.randint(0, num_envs, (num_envs // 2,)) + reset_indices = torch.randint(0, num_envs, (num_envs // 2,), device=sim.device) # reset the balls - ball_view.set_world_poses( - ball_initial_positions[reset_indices], ball_initial_orientations[reset_indices], indices=reset_indices - ) - ball_view.set_velocities(ball_initial_velocities[reset_indices], indices=reset_indices) + balls.write_root_pose_to_sim(ball_initial_poses[reset_indices], env_ids=reset_indices) + balls.write_root_velocity_to_sim(ball_initial_velocities[reset_indices], env_ids=reset_indices) + balls.reset(reset_indices) # reset the sensor ray_caster.reset(reset_indices) # reset the counter diff --git a/source/isaaclab/test/sensors/test_camera.py b/source/isaaclab/test/sensors/test_camera.py index 9071252a63f1..584394bfd54f 100644 --- a/source/isaaclab/test/sensors/test_camera.py +++ b/source/isaaclab/test/sensors/test_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,24 +8,22 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" import copy -import numpy as np import os import random + +import numpy as np +import pytest import scipy.spatial.transform as tf import torch -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep from isaacsim.core.prims import SingleGeometryPrim, SingleRigidPrim from pxr import Gf, Usd, UsdGeom @@ -37,867 +35,874 @@ from isaaclab.utils.timer import Timer # sample camera poses -POSITION = [2.5, 2.5, 2.5] -QUAT_ROS = [-0.17591989, 0.33985114, 0.82047325, -0.42470819] -QUAT_OPENGL = [0.33985113, 0.17591988, 0.42470818, 0.82047324] -QUAT_WORLD = [-0.3647052, -0.27984815, -0.1159169, 0.88047623] - - -class TestCamera(unittest.TestCase): - """Test for USD Camera sensor.""" - - def setUp(self): - """Create a blank new stage for each test.""" - self.camera_cfg = CameraCfg( - height=128, - width=128, - prim_path="/World/Camera", - update_period=0, - data_types=["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) - ), - ) - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.01 - # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=self.dt) - self.sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg) - # populate scene - self._populate_scene() - # load stage - stage_utils.update_stage() - - def tearDown(self): - """Stops simulator after each test.""" - # close all the opened viewport from before. - rep.vp_manager.destroy_hydra_textures("Replicator") - # stop simulation - # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - self.sim._timeline.stop() - # clear the stage - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Tests - """ - - def test_camera_init(self): - """Test camera initialization.""" - # Create camera - camera = Camera(self.camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[0].GetPath().pathString, self.camera_cfg.prim_path) - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (1, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (1, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (1, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (1, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (1, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - self.assertEqual(camera.data.info, [{self.camera_cfg.data_types[0]: None}]) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for im_data in camera.data.output.values(): - self.assertEqual(im_data.shape, (1, self.camera_cfg.height, self.camera_cfg.width, 1)) - - def test_camera_init_offset(self): - """Test camera initialization with offset using different conventions.""" - # define the same offset in all conventions - # -- ROS convention - cam_cfg_offset_ros = copy.deepcopy(self.camera_cfg) - cam_cfg_offset_ros.offset = CameraCfg.OffsetCfg( - pos=POSITION, - rot=QUAT_ROS, - convention="ros", - ) - cam_cfg_offset_ros.prim_path = "/World/CameraOffsetRos" - camera_ros = Camera(cam_cfg_offset_ros) - # -- OpenGL convention - cam_cfg_offset_opengl = copy.deepcopy(self.camera_cfg) - cam_cfg_offset_opengl.offset = CameraCfg.OffsetCfg( - pos=POSITION, - rot=QUAT_OPENGL, - convention="opengl", - ) - cam_cfg_offset_opengl.prim_path = "/World/CameraOffsetOpengl" - camera_opengl = Camera(cam_cfg_offset_opengl) - # -- World convention - cam_cfg_offset_world = copy.deepcopy(self.camera_cfg) - cam_cfg_offset_world.offset = CameraCfg.OffsetCfg( - pos=POSITION, - rot=QUAT_WORLD, - convention="world", - ) - cam_cfg_offset_world.prim_path = "/World/CameraOffsetWorld" - camera_world = Camera(cam_cfg_offset_world) - - # play sim - self.sim.reset() - - # retrieve camera pose using USD API - prim_tf_ros = camera_ros._sensor_prims[0].ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - prim_tf_opengl = camera_opengl._sensor_prims[0].ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - prim_tf_world = camera_world._sensor_prims[0].ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - # convert them from column-major to row-major - prim_tf_ros = np.transpose(prim_tf_ros) - prim_tf_opengl = np.transpose(prim_tf_opengl) - prim_tf_world = np.transpose(prim_tf_world) - - # check that all transforms are set correctly - np.testing.assert_allclose(prim_tf_ros[0:3, 3], cam_cfg_offset_ros.offset.pos) - np.testing.assert_allclose(prim_tf_opengl[0:3, 3], cam_cfg_offset_opengl.offset.pos) - np.testing.assert_allclose(prim_tf_world[0:3, 3], cam_cfg_offset_world.offset.pos) - np.testing.assert_allclose( - convert_quat(tf.Rotation.from_matrix(prim_tf_ros[:3, :3]).as_quat(), "wxyz"), - cam_cfg_offset_opengl.offset.rot, - rtol=1e-5, - ) - np.testing.assert_allclose( - convert_quat(tf.Rotation.from_matrix(prim_tf_opengl[:3, :3]).as_quat(), "wxyz"), - cam_cfg_offset_opengl.offset.rot, - rtol=1e-5, - ) - np.testing.assert_allclose( - convert_quat(tf.Rotation.from_matrix(prim_tf_world[:3, :3]).as_quat(), "wxyz"), - cam_cfg_offset_opengl.offset.rot, - rtol=1e-5, - ) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # check if transform correctly set in output - np.testing.assert_allclose(camera_ros.data.pos_w[0].cpu().numpy(), cam_cfg_offset_ros.offset.pos, rtol=1e-5) - np.testing.assert_allclose(camera_ros.data.quat_w_ros[0].cpu().numpy(), QUAT_ROS, rtol=1e-5) - np.testing.assert_allclose(camera_ros.data.quat_w_opengl[0].cpu().numpy(), QUAT_OPENGL, rtol=1e-5) - np.testing.assert_allclose(camera_ros.data.quat_w_world[0].cpu().numpy(), QUAT_WORLD, rtol=1e-5) - - def test_multi_camera_init(self): - """Test multi-camera initialization.""" - # create two cameras with different prim paths - # -- camera 1 - cam_cfg_1 = copy.deepcopy(self.camera_cfg) - cam_cfg_1.prim_path = "/World/Camera_1" - cam_1 = Camera(cam_cfg_1) - # -- camera 2 - cam_cfg_2 = copy.deepcopy(self.camera_cfg) - cam_cfg_2.prim_path = "/World/Camera_2" - cam_2 = Camera(cam_cfg_2) - - # play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - cam_1.update(self.dt) - cam_2.update(self.dt) - # check image data - for cam in [cam_1, cam_2]: - for im_data in cam.data.output.values(): - self.assertEqual(im_data.shape, (1, self.camera_cfg.height, self.camera_cfg.width, 1)) - - def test_multi_camera_with_different_resolution(self): - """Test multi-camera initialization with cameras having different image resolutions.""" - # create two cameras with different prim paths - # -- camera 1 - cam_cfg_1 = copy.deepcopy(self.camera_cfg) - cam_cfg_1.prim_path = "/World/Camera_1" - cam_1 = Camera(cam_cfg_1) - # -- camera 2 - cam_cfg_2 = copy.deepcopy(self.camera_cfg) - cam_cfg_2.prim_path = "/World/Camera_2" - cam_cfg_2.height = 240 - cam_cfg_2.width = 320 - cam_2 = Camera(cam_cfg_2) - - # play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() +POSITION = (2.5, 2.5, 2.5) +QUAT_ROS = (-0.17591989, 0.33985114, 0.82047325, -0.42470819) +QUAT_OPENGL = (0.33985113, 0.17591988, 0.42470818, 0.82047324) +QUAT_WORLD = (-0.3647052, -0.27984815, -0.1159169, 0.88047623) + +# NOTE: setup and teardown are own function to allow calling them in the tests + +# resolutions +HEIGHT = 240 +WIDTH = 320 + + +def setup() -> tuple[sim_utils.SimulationContext, CameraCfg, float]: + camera_cfg = CameraCfg( + height=HEIGHT, + width=WIDTH, + prim_path="/World/Camera", + update_period=0, + data_types=["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) + ), + ) + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.01 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt) + sim = sim_utils.SimulationContext(sim_cfg) + # populate scene + _populate_scene() + # load stage + sim_utils.update_stage() + return sim, camera_cfg, dt + + +def teardown(sim: sim_utils.SimulationContext): + # Cleanup + # close all the opened viewport from before. + rep.vp_manager.destroy_hydra_textures("Replicator") + # stop simulation + # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( + sim._timeline.stop() + # clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.fixture +def setup_sim_camera(): + """Create a simulation context.""" + sim, camera_cfg, dt = setup() + yield sim, camera_cfg, dt + teardown(sim) + + +def test_camera_init(setup_sim_camera): + """Test camera initialization.""" + # Create camera configuration + sim, camera_cfg, dt = setup_sim_camera + # Create camera + camera = Camera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[0].GetPath().pathString == camera_cfg.prim_path + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exist and have correct shapes + assert camera.data.pos_w.shape == (1, 3) + assert camera.data.quat_w_ros.shape == (1, 4) + assert camera.data.quat_w_world.shape == (1, 4) + assert camera.data.quat_w_opengl.shape == (1, 4) + assert camera.data.intrinsic_matrices.shape == (1, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + assert camera.data.info == [{camera_cfg.data_types[0]: None}] + + # Simulate physics + for _ in range(10): # perform rendering - self.sim.step() + sim.step() # update camera - cam_1.update(self.dt) - cam_2.update(self.dt) - # check image sizes - self.assertEqual( - cam_1.data.output["distance_to_image_plane"].shape, (1, self.camera_cfg.height, self.camera_cfg.width, 1) - ) - self.assertEqual(cam_2.data.output["distance_to_image_plane"].shape, (1, cam_cfg_2.height, cam_cfg_2.width, 1)) - - def test_camera_init_intrinsic_matrix(self): - """Test camera initialization from intrinsic matrix.""" - # get the first camera - camera_1 = Camera(cfg=self.camera_cfg) - # get intrinsic matrix - self.sim.reset() - intrinsic_matrix = camera_1.data.intrinsic_matrices[0].cpu().flatten().tolist() - self.tearDown() - # reinit the first camera - self.setUp() - camera_1 = Camera(cfg=self.camera_cfg) - # initialize from intrinsic matrix - intrinsic_camera_cfg = CameraCfg( - height=128, - width=128, - prim_path="/World/Camera_2", - update_period=0, - data_types=["distance_to_image_plane"], - spawn=sim_utils.PinholeCameraCfg.from_intrinsic_matrix( - intrinsic_matrix=intrinsic_matrix, - width=128, - height=128, - focal_length=24.0, - focus_distance=400.0, - clipping_range=(0.1, 1.0e5), - ), - ) - camera_2 = Camera(cfg=intrinsic_camera_cfg) + camera.update(sim.cfg.dt) + # check image data + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 1) + + +def test_camera_init_offset(setup_sim_camera): + """Test camera initialization with offset using different conventions.""" + sim, camera_cfg, dt = setup_sim_camera + # define the same offset in all conventions + # -- ROS convention + cam_cfg_offset_ros = copy.deepcopy(camera_cfg) + cam_cfg_offset_ros.update_latest_camera_pose = True + cam_cfg_offset_ros.offset = CameraCfg.OffsetCfg( + pos=POSITION, + rot=QUAT_ROS, + convention="ros", + ) + cam_cfg_offset_ros.prim_path = "/World/CameraOffsetRos" + camera_ros = Camera(cam_cfg_offset_ros) + # -- OpenGL convention + cam_cfg_offset_opengl = copy.deepcopy(camera_cfg) + cam_cfg_offset_opengl.update_latest_camera_pose = True + cam_cfg_offset_opengl.offset = CameraCfg.OffsetCfg( + pos=POSITION, + rot=QUAT_OPENGL, + convention="opengl", + ) + cam_cfg_offset_opengl.prim_path = "/World/CameraOffsetOpengl" + camera_opengl = Camera(cam_cfg_offset_opengl) + # -- World convention + cam_cfg_offset_world = copy.deepcopy(camera_cfg) + cam_cfg_offset_world.update_latest_camera_pose = True + cam_cfg_offset_world.offset = CameraCfg.OffsetCfg( + pos=POSITION, + rot=QUAT_WORLD, + convention="world", + ) + cam_cfg_offset_world.prim_path = "/World/CameraOffsetWorld" + camera_world = Camera(cam_cfg_offset_world) + + # play sim + sim.reset() + + # retrieve camera pose using USD API + prim_tf_ros = camera_ros._sensor_prims[0].ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + prim_tf_opengl = camera_opengl._sensor_prims[0].ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + prim_tf_world = camera_world._sensor_prims[0].ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # convert them from column-major to row-major + prim_tf_ros = np.transpose(prim_tf_ros) + prim_tf_opengl = np.transpose(prim_tf_opengl) + prim_tf_world = np.transpose(prim_tf_world) + + # check that all transforms are set correctly + np.testing.assert_allclose(prim_tf_ros[0:3, 3], cam_cfg_offset_ros.offset.pos) + np.testing.assert_allclose(prim_tf_opengl[0:3, 3], cam_cfg_offset_opengl.offset.pos) + np.testing.assert_allclose(prim_tf_world[0:3, 3], cam_cfg_offset_world.offset.pos) + np.testing.assert_allclose( + convert_quat(tf.Rotation.from_matrix(prim_tf_ros[:3, :3]).as_quat(), "wxyz"), + cam_cfg_offset_opengl.offset.rot, + rtol=1e-5, + ) + np.testing.assert_allclose( + convert_quat(tf.Rotation.from_matrix(prim_tf_opengl[:3, :3]).as_quat(), "wxyz"), + cam_cfg_offset_opengl.offset.rot, + rtol=1e-5, + ) + np.testing.assert_allclose( + convert_quat(tf.Rotation.from_matrix(prim_tf_world[:3, :3]).as_quat(), "wxyz"), + cam_cfg_offset_opengl.offset.rot, + rtol=1e-5, + ) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # check if transform correctly set in output + np.testing.assert_allclose(camera_ros.data.pos_w[0].cpu().numpy(), cam_cfg_offset_ros.offset.pos, rtol=1e-5) + np.testing.assert_allclose(camera_ros.data.quat_w_ros[0].cpu().numpy(), QUAT_ROS, rtol=1e-5) + np.testing.assert_allclose(camera_ros.data.quat_w_opengl[0].cpu().numpy(), QUAT_OPENGL, rtol=1e-5) + np.testing.assert_allclose(camera_ros.data.quat_w_world[0].cpu().numpy(), QUAT_WORLD, rtol=1e-5) + + +def test_multi_camera_init(setup_sim_camera): + """Test multi-camera initialization.""" + sim, camera_cfg, dt = setup_sim_camera + # create two cameras with different prim paths + # -- camera 1 + cam_cfg_1 = copy.deepcopy(camera_cfg) + cam_cfg_1.prim_path = "/World/Camera_1" + cam_1 = Camera(cam_cfg_1) + # -- camera 2 + cam_cfg_2 = copy.deepcopy(camera_cfg) + cam_cfg_2.prim_path = "/World/Camera_2" + cam_2 = Camera(cam_cfg_2) + + # play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + cam_1.update(dt) + cam_2.update(dt) + # check image data + for cam in [cam_1, cam_2]: + for im_data in cam.data.output.values(): + assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 1) + + +def test_multi_camera_with_different_resolution(setup_sim_camera): + """Test multi-camera initialization with cameras having different image resolutions.""" + sim, camera_cfg, dt = setup_sim_camera + # create two cameras with different prim paths + # -- camera 1 + cam_cfg_1 = copy.deepcopy(camera_cfg) + cam_cfg_1.prim_path = "/World/Camera_1" + cam_1 = Camera(cam_cfg_1) + # -- camera 2 + cam_cfg_2 = copy.deepcopy(camera_cfg) + cam_cfg_2.prim_path = "/World/Camera_2" + cam_cfg_2.height = 240 + cam_cfg_2.width = 320 + cam_2 = Camera(cam_cfg_2) + + # play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # perform rendering + sim.step() + # update camera + cam_1.update(dt) + cam_2.update(dt) + # check image sizes + assert cam_1.data.output["distance_to_image_plane"].shape == (1, camera_cfg.height, camera_cfg.width, 1) + assert cam_2.data.output["distance_to_image_plane"].shape == (1, cam_cfg_2.height, cam_cfg_2.width, 1) + + +def test_camera_init_intrinsic_matrix(setup_sim_camera): + """Test camera initialization from intrinsic matrix.""" + sim, camera_cfg, dt = setup_sim_camera + # get the first camera + camera_1 = Camera(cfg=camera_cfg) + # get intrinsic matrix + sim.reset() + intrinsic_matrix = camera_1.data.intrinsic_matrices[0].cpu().flatten().tolist() + teardown(sim) + # reinit the first camera + sim, camera_cfg, dt = setup() + camera_1 = Camera(cfg=camera_cfg) + # initialize from intrinsic matrix + intrinsic_camera_cfg = CameraCfg( + height=HEIGHT, + width=WIDTH, + prim_path="/World/Camera_2", + update_period=0, + data_types=["distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsic_matrix, + width=WIDTH, + height=HEIGHT, + focal_length=24.0, + focus_distance=400.0, + clipping_range=(0.1, 1.0e5), + ), + ) + camera_2 = Camera(cfg=intrinsic_camera_cfg) + + # play sim + sim.reset() + + # update cameras + camera_1.update(dt) + camera_2.update(dt) + + # check image data + torch.testing.assert_close( + camera_1.data.output["distance_to_image_plane"], + camera_2.data.output["distance_to_image_plane"], + rtol=5e-3, + atol=1e-4, + ) + # check that both intrinsic matrices are the same + torch.testing.assert_close( + camera_1.data.intrinsic_matrices[0], + camera_2.data.intrinsic_matrices[0], + rtol=5e-3, + atol=1e-4, + ) + + +def test_camera_set_world_poses(setup_sim_camera): + """Test camera function to set specific world pose.""" + sim, camera_cfg, dt = setup_sim_camera + # enable update latest camera pose + camera_cfg.update_latest_camera_pose = True + # init camera + camera = Camera(camera_cfg) + # play sim + sim.reset() + + # convert to torch tensors + position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + orientation = torch.tensor([QUAT_WORLD], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_world_poses(position.clone(), orientation.clone(), convention="world") + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # check if transform correctly set in output + torch.testing.assert_close(camera.data.pos_w, position) + torch.testing.assert_close(camera.data.quat_w_world, orientation) + + +def test_camera_set_world_poses_from_view(setup_sim_camera): + """Test camera function to set specific world pose from view.""" + sim, camera_cfg, dt = setup_sim_camera + # enable update latest camera pose + camera_cfg.update_latest_camera_pose = True + # init camera + camera = Camera(camera_cfg) + # play sim + sim.reset() + + # convert to torch tensors + eyes = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + quat_ros_gt = torch.tensor([QUAT_ROS], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_world_poses_from_view(eyes.clone(), targets.clone()) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # check if transform correctly set in output + torch.testing.assert_close(camera.data.pos_w, eyes) + torch.testing.assert_close(camera.data.quat_w_ros, quat_ros_gt) + + +def test_intrinsic_matrix(setup_sim_camera): + """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" + sim, camera_cfg, dt = setup_sim_camera + # enable update latest camera pose + camera_cfg.update_latest_camera_pose = True + # init camera + camera = Camera(camera_cfg) + # play sim + sim.reset() + # Desired properties (obtained from realsense camera at 320x240 resolution) + rs_intrinsic_matrix = [229.8, 0.0, 160.0, 0.0, 229.8, 120.0, 0.0, 0.0, 1.0] + rs_intrinsic_matrix = torch.tensor(rs_intrinsic_matrix, device=camera.device).reshape(3, 3).unsqueeze(0) + # Set matrix into simulator + camera.set_intrinsic_matrices(rs_intrinsic_matrix.clone()) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # Check that matrix is correct + torch.testing.assert_close(rs_intrinsic_matrix[0, 0, 0], camera.data.intrinsic_matrices[0, 0, 0]) + torch.testing.assert_close(rs_intrinsic_matrix[0, 1, 1], camera.data.intrinsic_matrices[0, 1, 1]) + torch.testing.assert_close(rs_intrinsic_matrix[0, 0, 2], camera.data.intrinsic_matrices[0, 0, 2]) + torch.testing.assert_close(rs_intrinsic_matrix[0, 1, 2], camera.data.intrinsic_matrices[0, 1, 2]) - # play sim - self.sim.reset() - # update cameras - camera_1.update(self.dt) - camera_2.update(self.dt) +def test_depth_clipping(setup_sim_camera): + """Test depth clipping. - # check image data - torch.testing.assert_close( - camera_1.data.output["distance_to_image_plane"], - camera_2.data.output["distance_to_image_plane"], - rtol=5e-3, - atol=1e-4, - ) - # check that both intrinsic matrices are the same - torch.testing.assert_close( - camera_1.data.intrinsic_matrices[0], - camera_2.data.intrinsic_matrices[0], - rtol=5e-3, - atol=1e-4, - ) + .. note:: - def test_camera_set_world_poses(self): - """Test camera function to set specific world pose.""" - camera = Camera(self.camera_cfg) - # play sim - self.sim.reset() - - # convert to torch tensors - position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) - orientation = torch.tensor([QUAT_WORLD], dtype=torch.float32, device=camera.device) - # set new pose - camera.set_world_poses(position.clone(), orientation.clone(), convention="world") - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # check if transform correctly set in output - torch.testing.assert_close(camera.data.pos_w, position) - torch.testing.assert_close(camera.data.quat_w_world, orientation) - - def test_camera_set_world_poses_from_view(self): - """Test camera function to set specific world pose from view.""" - camera = Camera(self.camera_cfg) - # play sim - self.sim.reset() - - # convert to torch tensors - eyes = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) - targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) - quat_ros_gt = torch.tensor([QUAT_ROS], dtype=torch.float32, device=camera.device) - # set new pose - camera.set_world_poses_from_view(eyes.clone(), targets.clone()) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # check if transform correctly set in output - torch.testing.assert_close(camera.data.pos_w, eyes) - torch.testing.assert_close(camera.data.quat_w_ros, quat_ros_gt) - - def test_intrinsic_matrix(self): - """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.height = 240 - camera_cfg.width = 320 - camera = Camera(camera_cfg) - # play sim - self.sim.reset() - # Desired properties (obtained from realsense camera at 320x240 resolution) - rs_intrinsic_matrix = [229.31640625, 0.0, 164.810546875, 0.0, 229.826171875, 122.1650390625, 0.0, 0.0, 1.0] - rs_intrinsic_matrix = torch.tensor(rs_intrinsic_matrix, device=camera.device).reshape(3, 3).unsqueeze(0) - # Set matrix into simulator - camera.set_intrinsic_matrices(rs_intrinsic_matrix.clone()) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # Check that matrix is correct - # TODO: This is not correctly setting all values in the matrix since the - # vertical aperture and aperture offsets are not being set correctly - # This is a bug in the simulator. - torch.testing.assert_close(rs_intrinsic_matrix[0, 0, 0], camera.data.intrinsic_matrices[0, 0, 0]) - # torch.testing.assert_close(rs_intrinsic_matrix[0, 1, 1], camera.data.intrinsic_matrices[0, 1, 1]) - - def test_depth_clipping(self): - """Test depth clipping. - - .. note:: - - This test is the same for all camera models to enforce the same clipping behavior. - """ - # get camera cfgs - camera_cfg_zero = CameraCfg( - prim_path="/World/CameraZero", - offset=CameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), - spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( - focal_length=38.0, - intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], - height=540, - width=960, - clipping_range=(0.1, 10), - ), + This test is the same for all camera models to enforce the same clipping behavior. + """ + # get camera cfgs + sim, _, dt = setup_sim_camera + camera_cfg_zero = CameraCfg( + prim_path="/World/CameraZero", + offset=CameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), + spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( + focal_length=38.0, + intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], height=540, width=960, - data_types=["distance_to_image_plane", "distance_to_camera"], - depth_clipping_behavior="zero", - ) - camera_zero = Camera(camera_cfg_zero) - - camera_cfg_none = copy.deepcopy(camera_cfg_zero) - camera_cfg_none.prim_path = "/World/CameraNone" - camera_cfg_none.depth_clipping_behavior = "none" - camera_none = Camera(camera_cfg_none) - - camera_cfg_max = copy.deepcopy(camera_cfg_zero) - camera_cfg_max.prim_path = "/World/CameraMax" - camera_cfg_max.depth_clipping_behavior = "max" - camera_max = Camera(camera_cfg_max) - - # Play sim - self.sim.reset() - - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - camera_zero.update(self.dt) - camera_none.update(self.dt) - camera_max.update(self.dt) - - # none clipping should contain inf values - self.assertTrue(torch.isinf(camera_none.data.output["distance_to_camera"]).any()) - self.assertTrue(torch.isinf(camera_none.data.output["distance_to_image_plane"]).any()) - self.assertTrue( - camera_none.data.output["distance_to_camera"][ - ~torch.isinf(camera_none.data.output["distance_to_camera"]) - ].min() - >= camera_cfg_zero.spawn.clipping_range[0] - ) - self.assertTrue( - camera_none.data.output["distance_to_camera"][ - ~torch.isinf(camera_none.data.output["distance_to_camera"]) - ].max() - <= camera_cfg_zero.spawn.clipping_range[1] - ) - self.assertTrue( - camera_none.data.output["distance_to_image_plane"][ - ~torch.isinf(camera_none.data.output["distance_to_image_plane"]) - ].min() - >= camera_cfg_zero.spawn.clipping_range[0] - ) - self.assertTrue( - camera_none.data.output["distance_to_image_plane"][ - ~torch.isinf(camera_none.data.output["distance_to_camera"]) - ].max() - <= camera_cfg_zero.spawn.clipping_range[1] - ) - - # zero clipping should result in zero values - self.assertTrue( - torch.all( - camera_zero.data.output["distance_to_camera"][ - torch.isinf(camera_none.data.output["distance_to_camera"]) - ] - == 0.0 - ) - ) - self.assertTrue( - torch.all( - camera_zero.data.output["distance_to_image_plane"][ - torch.isinf(camera_none.data.output["distance_to_image_plane"]) - ] - == 0.0 - ) - ) - self.assertTrue( - camera_zero.data.output["distance_to_camera"][camera_zero.data.output["distance_to_camera"] != 0.0].min() - >= camera_cfg_zero.spawn.clipping_range[0] - ) - self.assertTrue(camera_zero.data.output["distance_to_camera"].max() <= camera_cfg_zero.spawn.clipping_range[1]) - self.assertTrue( - camera_zero.data.output["distance_to_image_plane"][ - camera_zero.data.output["distance_to_image_plane"] != 0.0 - ].min() - >= camera_cfg_zero.spawn.clipping_range[0] - ) - self.assertTrue( - camera_zero.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.spawn.clipping_range[1] - ) - - # max clipping should result in max values - self.assertTrue( - torch.all( - camera_max.data.output["distance_to_camera"][torch.isinf(camera_none.data.output["distance_to_camera"])] - == camera_cfg_zero.spawn.clipping_range[1] - ) - ) - self.assertTrue( - torch.all( - camera_max.data.output["distance_to_image_plane"][ - torch.isinf(camera_none.data.output["distance_to_image_plane"]) - ] - == camera_cfg_zero.spawn.clipping_range[1] - ) - ) - self.assertTrue(camera_max.data.output["distance_to_camera"].min() >= camera_cfg_zero.spawn.clipping_range[0]) - self.assertTrue(camera_max.data.output["distance_to_camera"].max() <= camera_cfg_zero.spawn.clipping_range[1]) - self.assertTrue( - camera_max.data.output["distance_to_image_plane"].min() >= camera_cfg_zero.spawn.clipping_range[0] - ) - self.assertTrue( - camera_max.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.spawn.clipping_range[1] - ) - - def test_camera_resolution_all_colorize(self): - """Test camera resolution is correctly set for all types with colorization enabled.""" - # Add all types - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ] - camera_cfg.colorize_instance_id_segmentation = True - camera_cfg.colorize_instance_segmentation = True - camera_cfg.colorize_semantic_segmentation = True - # Create camera - camera = Camera(camera_cfg) - - # Play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - camera.update(self.dt) - - # expected sizes - hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) - hw_2c_shape = (1, camera_cfg.height, camera_cfg.width, 2) - hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) - hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) - # access image data and compare shapes - output = camera.data.output - self.assertEqual(output["rgb"].shape, hw_3c_shape) - self.assertEqual(output["rgba"].shape, hw_4c_shape) - self.assertEqual(output["depth"].shape, hw_1c_shape) - self.assertEqual(output["distance_to_camera"].shape, hw_1c_shape) - self.assertEqual(output["distance_to_image_plane"].shape, hw_1c_shape) - self.assertEqual(output["normals"].shape, hw_3c_shape) - self.assertEqual(output["motion_vectors"].shape, hw_2c_shape) - self.assertEqual(output["semantic_segmentation"].shape, hw_4c_shape) - self.assertEqual(output["instance_segmentation_fast"].shape, hw_4c_shape) - self.assertEqual(output["instance_id_segmentation_fast"].shape, hw_4c_shape) - - # access image data and compare dtype - output = camera.data.output - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.uint8) - - def test_camera_resolution_no_colorize(self): - """Test camera resolution is correctly set for all types with no colorization enabled.""" - # Add all types - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ] - camera_cfg.colorize_instance_id_segmentation = False - camera_cfg.colorize_instance_segmentation = False - camera_cfg.colorize_semantic_segmentation = False - # Create camera - camera = Camera(camera_cfg) - - # Play sim - self.sim.reset() - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(12): - self.sim.step() - camera.update(self.dt) - - # expected sizes - hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) - hw_2c_shape = (1, camera_cfg.height, camera_cfg.width, 2) - hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) - hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) - # access image data and compare shapes - output = camera.data.output - self.assertEqual(output["rgb"].shape, hw_3c_shape) - self.assertEqual(output["rgba"].shape, hw_4c_shape) - self.assertEqual(output["depth"].shape, hw_1c_shape) - self.assertEqual(output["distance_to_camera"].shape, hw_1c_shape) - self.assertEqual(output["distance_to_image_plane"].shape, hw_1c_shape) - self.assertEqual(output["normals"].shape, hw_3c_shape) - self.assertEqual(output["motion_vectors"].shape, hw_2c_shape) - self.assertEqual(output["semantic_segmentation"].shape, hw_1c_shape) - self.assertEqual(output["instance_segmentation_fast"].shape, hw_1c_shape) - self.assertEqual(output["instance_id_segmentation_fast"].shape, hw_1c_shape) - - # access image data and compare dtype - output = camera.data.output - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.int32) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.int32) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.int32) - - def test_camera_large_resolution_all_colorize(self): - """Test camera resolution is correctly set for all types with colorization enabled.""" - # Add all types - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", + clipping_range=(0.1, 10), + ), + height=540, + width=960, + data_types=["distance_to_image_plane", "distance_to_camera"], + depth_clipping_behavior="zero", + ) + camera_zero = Camera(camera_cfg_zero) + + camera_cfg_none = copy.deepcopy(camera_cfg_zero) + camera_cfg_none.prim_path = "/World/CameraNone" + camera_cfg_none.depth_clipping_behavior = "none" + camera_none = Camera(camera_cfg_none) + + camera_cfg_max = copy.deepcopy(camera_cfg_zero) + camera_cfg_max.prim_path = "/World/CameraMax" + camera_cfg_max.depth_clipping_behavior = "max" + camera_max = Camera(camera_cfg_max) + + # Play sim + sim.reset() + + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + camera_zero.update(dt) + camera_none.update(dt) + camera_max.update(dt) + + # none clipping should contain inf values + assert torch.isinf(camera_none.data.output["distance_to_camera"]).any() + assert torch.isinf(camera_none.data.output["distance_to_image_plane"]).any() + assert ( + camera_none.data.output["distance_to_camera"][~torch.isinf(camera_none.data.output["distance_to_camera"])].min() + >= camera_cfg_zero.spawn.clipping_range[0] + ) + assert ( + camera_none.data.output["distance_to_camera"][~torch.isinf(camera_none.data.output["distance_to_camera"])].max() + <= camera_cfg_zero.spawn.clipping_range[1] + ) + assert ( + camera_none.data.output["distance_to_image_plane"][ + ~torch.isinf(camera_none.data.output["distance_to_image_plane"]) + ].min() + >= camera_cfg_zero.spawn.clipping_range[0] + ) + assert ( + camera_none.data.output["distance_to_image_plane"][ + ~torch.isinf(camera_none.data.output["distance_to_camera"]) + ].max() + <= camera_cfg_zero.spawn.clipping_range[1] + ) + + # zero clipping should result in zero values + assert torch.all( + camera_zero.data.output["distance_to_camera"][torch.isinf(camera_none.data.output["distance_to_camera"])] == 0.0 + ) + assert torch.all( + camera_zero.data.output["distance_to_image_plane"][ + torch.isinf(camera_none.data.output["distance_to_image_plane"]) ] - camera_cfg.colorize_instance_id_segmentation = True - camera_cfg.colorize_instance_segmentation = True - camera_cfg.colorize_semantic_segmentation = True - camera_cfg.width = 512 - camera_cfg.height = 512 - # Create camera - camera = Camera(camera_cfg) - - # Play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - camera.update(self.dt) - - # expected sizes - hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) - hw_2c_shape = (1, camera_cfg.height, camera_cfg.width, 2) - hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) - hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) - # access image data and compare shapes - output = camera.data.output - self.assertEqual(output["rgb"].shape, hw_3c_shape) - self.assertEqual(output["rgba"].shape, hw_4c_shape) - self.assertEqual(output["depth"].shape, hw_1c_shape) - self.assertEqual(output["distance_to_camera"].shape, hw_1c_shape) - self.assertEqual(output["distance_to_image_plane"].shape, hw_1c_shape) - self.assertEqual(output["normals"].shape, hw_3c_shape) - self.assertEqual(output["motion_vectors"].shape, hw_2c_shape) - self.assertEqual(output["semantic_segmentation"].shape, hw_4c_shape) - self.assertEqual(output["instance_segmentation_fast"].shape, hw_4c_shape) - self.assertEqual(output["instance_id_segmentation_fast"].shape, hw_4c_shape) - - # access image data and compare dtype - output = camera.data.output - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.uint8) - - def test_camera_resolution_rgb_only(self): - """Test camera resolution is correctly set for RGB only.""" - # Add all types - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = [ - "rgb", + == 0.0 + ) + assert ( + camera_zero.data.output["distance_to_camera"][camera_zero.data.output["distance_to_camera"] != 0.0].min() + >= camera_cfg_zero.spawn.clipping_range[0] + ) + assert camera_zero.data.output["distance_to_camera"].max() <= camera_cfg_zero.spawn.clipping_range[1] + assert ( + camera_zero.data.output["distance_to_image_plane"][ + camera_zero.data.output["distance_to_image_plane"] != 0.0 + ].min() + >= camera_cfg_zero.spawn.clipping_range[0] + ) + assert camera_zero.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.spawn.clipping_range[1] + + # max clipping should result in max values + assert torch.all( + camera_max.data.output["distance_to_camera"][torch.isinf(camera_none.data.output["distance_to_camera"])] + == camera_cfg_zero.spawn.clipping_range[1] + ) + assert torch.all( + camera_max.data.output["distance_to_image_plane"][ + torch.isinf(camera_none.data.output["distance_to_image_plane"]) ] - # Create camera - camera = Camera(camera_cfg) - - # Play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - camera.update(self.dt) - - # expected sizes - hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) - # access image data and compare shapes - output = camera.data.output - self.assertEqual(output["rgb"].shape, hw_3c_shape) - # access image data and compare dtype - self.assertEqual(output["rgb"].dtype, torch.uint8) - - def test_camera_resolution_rgba_only(self): - """Test camera resolution is correctly set for RGBA only.""" - # Add all types - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = [ - "rgba", - ] - # Create camera - camera = Camera(camera_cfg) - - # Play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - camera.update(self.dt) - - # expected sizes - hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) - # access image data and compare shapes - output = camera.data.output - self.assertEqual(output["rgba"].shape, hw_4c_shape) - # access image data and compare dtype - self.assertEqual(output["rgba"].dtype, torch.uint8) - - def test_camera_resolution_depth_only(self): - """Test camera resolution is correctly set for depth only.""" - # Add all types - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = [ - "depth", - ] - # Create camera - camera = Camera(camera_cfg) - - # Play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - camera.update(self.dt) - - # expected sizes - hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) - # access image data and compare shapes - output = camera.data.output - self.assertEqual(output["depth"].shape, hw_1c_shape) - # access image data and compare dtype - self.assertEqual(output["depth"].dtype, torch.float) - - def test_throughput(self): - """Checks that the single camera gets created properly with a rig.""" - # Create directory temp dir to dump the results - file_dir = os.path.dirname(os.path.realpath(__file__)) - temp_dir = os.path.join(file_dir, "output", "camera", "throughput") - os.makedirs(temp_dir, exist_ok=True) - # Create replicator writer - rep_writer = rep.BasicWriter(output_dir=temp_dir, frame_padding=3) - # create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.height = 480 - camera_cfg.width = 640 - camera = Camera(camera_cfg) - - # Play simulator - self.sim.reset() - - # Set camera pose - eyes = torch.tensor([[2.5, 2.5, 2.5]], dtype=torch.float32, device=camera.device) - targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) - camera.set_world_poses_from_view(eyes, targets) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - # Simulate physics - for _ in range(5): - # perform rendering - self.sim.step() - # update camera - with Timer(f"Time taken for updating camera with shape {camera.image_shape}"): - camera.update(self.dt) + == camera_cfg_zero.spawn.clipping_range[1] + ) + assert camera_max.data.output["distance_to_camera"].min() >= camera_cfg_zero.spawn.clipping_range[0] + assert camera_max.data.output["distance_to_camera"].max() <= camera_cfg_zero.spawn.clipping_range[1] + assert camera_max.data.output["distance_to_image_plane"].min() >= camera_cfg_zero.spawn.clipping_range[0] + assert camera_max.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.spawn.clipping_range[1] + + +def test_camera_resolution_all_colorize(setup_sim_camera): + """Test camera resolution is correctly set for all types with colorization enabled.""" + # Add all types + sim, camera_cfg, dt = setup_sim_camera + camera_cfg.data_types = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + camera_cfg.colorize_instance_id_segmentation = True + camera_cfg.colorize_instance_segmentation = True + camera_cfg.colorize_semantic_segmentation = True + # Create camera + camera = Camera(camera_cfg) + + # Play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + camera.update(dt) + + # expected sizes + hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) + hw_2c_shape = (1, camera_cfg.height, camera_cfg.width, 2) + hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) + hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) + # access image data and compare shapes + output = camera.data.output + assert output["rgb"].shape == hw_3c_shape + assert output["rgba"].shape == hw_4c_shape + assert output["depth"].shape == hw_1c_shape + assert output["distance_to_camera"].shape == hw_1c_shape + assert output["distance_to_image_plane"].shape == hw_1c_shape + assert output["normals"].shape == hw_3c_shape + assert output["motion_vectors"].shape == hw_2c_shape + assert output["semantic_segmentation"].shape == hw_4c_shape + assert output["instance_segmentation_fast"].shape == hw_4c_shape + assert output["instance_id_segmentation_fast"].shape == hw_4c_shape + + # access image data and compare dtype + output = camera.data.output + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.uint8 + assert output["instance_segmentation_fast"].dtype == torch.uint8 + assert output["instance_id_segmentation_fast"].dtype == torch.uint8 + + +def test_camera_resolution_no_colorize(setup_sim_camera): + """Test camera resolution is correctly set for all types with no colorization enabled.""" + # Add all types + sim, camera_cfg, dt = setup_sim_camera + camera_cfg.data_types = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + camera_cfg.colorize_instance_id_segmentation = False + camera_cfg.colorize_instance_segmentation = False + camera_cfg.colorize_semantic_segmentation = False + # Create camera + camera = Camera(camera_cfg) + + # Play sim + sim.reset() + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(12): + sim.step() + camera.update(dt) + + # expected sizes + hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) + hw_2c_shape = (1, camera_cfg.height, camera_cfg.width, 2) + hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) + hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) + # access image data and compare shapes + output = camera.data.output + assert output["rgb"].shape == hw_3c_shape + assert output["rgba"].shape == hw_4c_shape + assert output["depth"].shape == hw_1c_shape + assert output["distance_to_camera"].shape == hw_1c_shape + assert output["distance_to_image_plane"].shape == hw_1c_shape + assert output["normals"].shape == hw_3c_shape + assert output["motion_vectors"].shape == hw_2c_shape + assert output["semantic_segmentation"].shape == hw_1c_shape + assert output["instance_segmentation_fast"].shape == hw_1c_shape + assert output["instance_id_segmentation_fast"].shape == hw_1c_shape + + # access image data and compare dtype + output = camera.data.output + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.int32 + assert output["instance_segmentation_fast"].dtype == torch.int32 + assert output["instance_id_segmentation_fast"].dtype == torch.int32 + + +def test_camera_large_resolution_all_colorize(setup_sim_camera): + """Test camera resolution is correctly set for all types with colorization enabled.""" + # Add all types + sim, camera_cfg, dt = setup_sim_camera + camera_cfg.data_types = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + camera_cfg.colorize_instance_id_segmentation = True + camera_cfg.colorize_instance_segmentation = True + camera_cfg.colorize_semantic_segmentation = True + camera_cfg.width = 512 + camera_cfg.height = 512 + # Create camera + camera = Camera(camera_cfg) + + # Play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + camera.update(dt) + + # expected sizes + hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) + hw_2c_shape = (1, camera_cfg.height, camera_cfg.width, 2) + hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) + hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) + # access image data and compare shapes + output = camera.data.output + assert output["rgb"].shape == hw_3c_shape + assert output["rgba"].shape == hw_4c_shape + assert output["depth"].shape == hw_1c_shape + assert output["distance_to_camera"].shape == hw_1c_shape + assert output["distance_to_image_plane"].shape == hw_1c_shape + assert output["normals"].shape == hw_3c_shape + assert output["motion_vectors"].shape == hw_2c_shape + assert output["semantic_segmentation"].shape == hw_4c_shape + assert output["instance_segmentation_fast"].shape == hw_4c_shape + assert output["instance_id_segmentation_fast"].shape == hw_4c_shape + + # access image data and compare dtype + output = camera.data.output + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.uint8 + assert output["instance_segmentation_fast"].dtype == torch.uint8 + assert output["instance_id_segmentation_fast"].dtype == torch.uint8 + + +def test_camera_resolution_rgb_only(setup_sim_camera): + """Test camera resolution is correctly set for RGB only.""" + # Add all types + sim, camera_cfg, dt = setup_sim_camera + camera_cfg.data_types = ["rgb"] + # Create camera + camera = Camera(camera_cfg) + + # Play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + camera.update(dt) + + # expected sizes + hw_3c_shape = (1, camera_cfg.height, camera_cfg.width, 3) + # access image data and compare shapes + output = camera.data.output + assert output["rgb"].shape == hw_3c_shape + # access image data and compare dtype + assert output["rgb"].dtype == torch.uint8 + + +def test_camera_resolution_rgba_only(setup_sim_camera): + """Test camera resolution is correctly set for RGBA only.""" + # Add all types + sim, camera_cfg, dt = setup_sim_camera + camera_cfg.data_types = ["rgba"] + # Create camera + camera = Camera(camera_cfg) + + # Play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + camera.update(dt) + + # expected sizes + hw_4c_shape = (1, camera_cfg.height, camera_cfg.width, 4) + # access image data and compare shapes + output = camera.data.output + assert output["rgba"].shape == hw_4c_shape + # access image data and compare dtype + assert output["rgba"].dtype == torch.uint8 + + +def test_camera_resolution_depth_only(setup_sim_camera): + """Test camera resolution is correctly set for depth only.""" + # Add all types + sim, camera_cfg, dt = setup_sim_camera + camera_cfg.data_types = ["depth"] + # Create camera + camera = Camera(camera_cfg) + + # Play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + camera.update(dt) + + # expected sizes + hw_1c_shape = (1, camera_cfg.height, camera_cfg.width, 1) + # access image data and compare shapes + output = camera.data.output + assert output["depth"].shape == hw_1c_shape + # access image data and compare dtype + assert output["depth"].dtype == torch.float + + +def test_throughput(setup_sim_camera): + """Checks that the single camera gets created properly with a rig.""" + # Create directory temp dir to dump the results + file_dir = os.path.dirname(os.path.realpath(__file__)) + temp_dir = os.path.join(file_dir, "output", "camera", "throughput") + os.makedirs(temp_dir, exist_ok=True) + # Create replicator writer + rep_writer = rep.BasicWriter(output_dir=temp_dir, frame_padding=3) + # create camera + sim, camera_cfg, dt = setup_sim_camera + camera_cfg.height = 480 + camera_cfg.width = 640 + camera = Camera(camera_cfg) + + # Play simulator + sim.reset() + + # Set camera pose + eyes = torch.tensor([[2.5, 2.5, 2.5]], dtype=torch.float32, device=camera.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + camera.set_world_poses_from_view(eyes, targets) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # Simulate physics + for _ in range(5): + # perform rendering + sim.step() + # update camera + with Timer(f"Time taken for updating camera with shape {camera.image_shape}"): + camera.update(dt) + # Save images + with Timer(f"Time taken for writing data with shape {camera.image_shape} "): + # Pack data back into replicator format to save them using its writer + rep_output = {"annotators": {}} + camera_data = convert_dict_to_backend({k: v[0] for k, v in camera.data.output.items()}, backend="numpy") + for key, data, info in zip(camera_data.keys(), camera_data.values(), camera.data.info[0].values()): + if info is not None: + rep_output["annotators"][key] = {"render_product": {"data": data, **info}} + else: + rep_output["annotators"][key] = {"render_product": {"data": data}} # Save images - with Timer(f"Time taken for writing data with shape {camera.image_shape} "): - # Pack data back into replicator format to save them using its writer - rep_output = {"annotators": {}} - camera_data = convert_dict_to_backend({k: v[0] for k, v in camera.data.output.items()}, backend="numpy") - for key, data, info in zip(camera_data.keys(), camera_data.values(), camera.data.info[0].values()): - if info is not None: - rep_output["annotators"][key] = {"render_product": {"data": data, **info}} - else: - rep_output["annotators"][key] = {"render_product": {"data": data}} - # Save images - rep_output["trigger_outputs"] = {"on_time": camera.frame[0]} - rep_writer.write(rep_output) - print("----------------------------------------") - # Check image data - for im_data in camera.data.output.values(): - self.assertEqual(im_data.shape, (1, camera_cfg.height, camera_cfg.width, 1)) - - def test_sensor_print(self): - """Test sensor print is working correctly.""" - # Create sensor - sensor = Camera(cfg=self.camera_cfg) - # Play sim - self.sim.reset() - # print info - print(sensor) - - """ - Helper functions. - """ - - @staticmethod - def _populate_scene(): - """Add prims to the scene.""" - # Ground-plane - cfg = sim_utils.GroundPlaneCfg() - cfg.func("/World/defaultGroundPlane", cfg) - # Lights - cfg = sim_utils.SphereLightCfg() - cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) - cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) - # Random objects - random.seed(0) - for i in range(10): - # sample random position - position = np.random.rand(3) - np.asarray([0.05, 0.05, -1.0]) - position *= np.asarray([1.5, 1.5, 0.5]) - # create prim - prim_type = random.choice(["Cube", "Sphere", "Cylinder"]) - prim = prim_utils.create_prim( - f"/World/Objects/Obj_{i:02d}", - prim_type, - translation=position, - scale=(0.25, 0.25, 0.25), - semantic_label=prim_type, - ) - # cast to geom prim - geom_prim = getattr(UsdGeom, prim_type)(prim) - # set random color - color = Gf.Vec3f(random.random(), random.random(), random.random()) - geom_prim.CreateDisplayColorAttr() - geom_prim.GetDisplayColorAttr().Set([color]) - # add rigid properties - SingleGeometryPrim(f"/World/Objects/Obj_{i:02d}", collision=True) - SingleRigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) - - -if __name__ == "__main__": - run_tests() + rep_output["trigger_outputs"] = {"on_time": camera.frame[0]} + rep_writer.write(rep_output) + print("----------------------------------------") + # Check image data + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 1) + + +def test_sensor_print(setup_sim_camera): + """Test sensor print is working correctly.""" + # Create sensor + sim, camera_cfg, dt = setup_sim_camera + sensor = Camera(cfg=camera_cfg) + # Play sim + sim.reset() + # print info + print(sensor) + + +def _populate_scene(): + """Add prims to the scene.""" + # Ground-plane + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/defaultGroundPlane", cfg) + # Lights + cfg = sim_utils.SphereLightCfg() + cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) + cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) + # Random objects + random.seed(0) + for i in range(10): + # sample random position + position = np.random.rand(3) - np.asarray([0.05, 0.05, -1.0]) + position *= np.asarray([1.5, 1.5, 0.5]) + # create prim + prim_type = random.choice(["Cube", "Sphere", "Cylinder"]) + prim = sim_utils.create_prim( + f"/World/Objects/Obj_{i:02d}", + prim_type, + translation=position, + scale=(0.25, 0.25, 0.25), + semantic_label=prim_type, + ) + # cast to geom prim + geom_prim = getattr(UsdGeom, prim_type)(prim) + # set random color + color = Gf.Vec3f(random.random(), random.random(), random.random()) + geom_prim.CreateDisplayColorAttr() + geom_prim.GetDisplayColorAttr().Set([color]) + # add rigid properties + SingleGeometryPrim(f"/World/Objects/Obj_{i:02d}", collision=True) + SingleRigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 5343a5672cb7..ed376f97f2d1 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,26 +7,29 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import torch -import unittest from dataclasses import MISSING from enum import Enum +import pytest +import torch +from flaky import flaky + import carb +from pxr import PhysxSchema import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors import ContactSensor, ContactSensorCfg -from isaaclab.sim import build_simulation_context +from isaaclab.sim import SimulationCfg, SimulationContext, build_simulation_context +from isaaclab.sim.utils.stage import get_current_stage from isaaclab.terrains import HfRandomUniformTerrainCfg, TerrainGeneratorCfg, TerrainImporterCfg from isaaclab.utils import configclass @@ -45,7 +48,7 @@ class ContactTestMode(Enum): @configclass -class TestContactSensorRigidObjectCfg(RigidObjectCfg): +class ContactSensorRigidObjectCfg(RigidObjectCfg): """Configuration for rigid objects used for the contact sensor test. This contains the expected values in the configuration to simplify test fixtures. @@ -64,13 +67,13 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): terrain: TerrainImporterCfg = MISSING """Terrain configuration within the scene.""" - shape: TestContactSensorRigidObjectCfg = MISSING + shape: ContactSensorRigidObjectCfg = MISSING """RigidObject contact prim configuration.""" contact_sensor: ContactSensorCfg = MISSING """Contact sensor configuration.""" - shape_2: TestContactSensorRigidObjectCfg = None + shape_2: ContactSensorRigidObjectCfg = None """RigidObject contact prim configuration. Defaults to None, i.e. not included in the scene. This is a second prim used for testing contact filtering. @@ -88,7 +91,7 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): ## -CUBE_CFG = TestContactSensorRigidObjectCfg( +CUBE_CFG = ContactSensorRigidObjectCfg( prim_path="/World/Objects/Cube", spawn=sim_utils.CuboidCfg( size=(0.5, 0.5, 0.5), @@ -107,7 +110,7 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): ) """Configuration of the cube prim.""" -SPHERE_CFG = TestContactSensorRigidObjectCfg( +SPHERE_CFG = ContactSensorRigidObjectCfg( prim_path="/World/Objects/Sphere", spawn=sim_utils.SphereCfg( radius=0.25, @@ -126,7 +129,7 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): ) """Configuration of the sphere prim.""" -CYLINDER_CFG = TestContactSensorRigidObjectCfg( +CYLINDER_CFG = ContactSensorRigidObjectCfg( prim_path="/World/Objects/Cylinder", spawn=sim_utils.CylinderCfg( radius=0.5, @@ -147,7 +150,7 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): ) """Configuration of the cylinder prim.""" -CAPSULE_CFG = TestContactSensorRigidObjectCfg( +CAPSULE_CFG = ContactSensorRigidObjectCfg( prim_path="/World/Objects/Capsule", spawn=sim_utils.CapsuleCfg( radius=0.25, @@ -168,7 +171,7 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): ) """Configuration of the capsule prim.""" -CONE_CFG = TestContactSensorRigidObjectCfg( +CONE_CFG = ContactSensorRigidObjectCfg( prim_path="/World/Objects/Cone", spawn=sim_utils.ConeCfg( radius=0.5, @@ -211,377 +214,633 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): """Configuration of the generated mesh terrain.""" -class TestContactSensor(unittest.TestCase): - """Unittest class for testing the contact sensor. +@pytest.fixture(scope="module") +def setup_simulation(): + """Fixture to set up simulation parameters.""" + sim_dt = 0.0025 + durations = [sim_dt, sim_dt * 2, sim_dt * 32, sim_dt * 128] + terrains = [FLAT_TERRAIN_CFG, COBBLESTONE_TERRAIN_CFG] + devices = ["cuda:0", "cpu"] + carb_settings_iface = carb.settings.get_settings() + return sim_dt, durations, terrains, devices, carb_settings_iface + + +@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 + # internally, the contact sensor should enable contact processing so it should always work. + sim_dt, durations, terrains, devices, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", disable_contact_processing) + _run_contact_sensor_test(CUBE_CFG, sim_dt, devices, terrains, carb_settings_iface, durations) + + +@pytest.mark.parametrize("disable_contact_processing", [True, False]) +@flaky(max_runs=3, min_passes=1) +def test_sphere_contact_time(setup_simulation, disable_contact_processing): + """Checks contact sensor values for contact time and air time for a sphere collision primitive.""" + # check for both contact processing enabled and disabled + # internally, the contact sensor should enable contact processing so it should always work. + sim_dt, durations, terrains, devices, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", disable_contact_processing) + _run_contact_sensor_test(SPHERE_CFG, sim_dt, devices, terrains, carb_settings_iface, durations) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 6, 24]) +def test_cube_stack_contact_filtering(setup_simulation, device, num_envs): + """Checks contact sensor reporting for filtering stacked cube prims.""" + sim_dt, durations, terrains, devices, carb_settings_iface = setup_simulation + with build_simulation_context(device=device, dt=sim_dt, add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Instance new scene for the current terrain and contact prim. + scene_cfg = ContactSensorSceneCfg(num_envs=num_envs, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground") + # -- cube 1 + scene_cfg.shape = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_1") + scene_cfg.shape.init_state.pos = (0, -1.0, 1.0) + # -- cube 2 (on top of cube 1) + scene_cfg.shape_2 = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_2") + scene_cfg.shape_2.init_state.pos = (0, -1.0, 1.525) + # -- contact sensor 1 + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Cube_1", + track_pose=True, + debug_vis=False, + update_period=0.0, + filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_2"], + ) + # -- contact sensor 2 + scene_cfg.contact_sensor_2 = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Cube_2", + track_pose=True, + debug_vis=False, + update_period=0.0, + filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_1"], + ) + scene = InteractiveScene(scene_cfg) + + # Check that contact processing is enabled + assert not carb_settings_iface.get("/physics/disableContactProcessing") + + # Set variables internally for reference + sim.reset() + + contact_sensor = scene["contact_sensor"] + contact_sensor_2 = scene["contact_sensor_2"] + + # Check that contact processing is enabled + assert contact_sensor.contact_physx_view.filter_count == 1 + assert contact_sensor_2.contact_physx_view.filter_count == 1 + + # Play the simulation + scene.reset() + for _ in range(500): + _perform_sim_step(sim, scene, sim_dt) + + # Check values for cube 2 --> cube 1 is the only collision for cube 2 + torch.testing.assert_close(contact_sensor_2.data.force_matrix_w[:, :, 0], contact_sensor_2.data.net_forces_w) + # Check that forces are opposite and equal + torch.testing.assert_close( + contact_sensor_2.data.force_matrix_w[:, :, 0], -contact_sensor.data.force_matrix_w[:, :, 0] + ) + # Check values are non-zero (contacts are happening and are getting reported) + assert contact_sensor_2.data.net_forces_w.sum().item() > 0.0 + assert contact_sensor.data.net_forces_w.sum().item() > 0.0 + + +def test_no_contact_reporting(setup_simulation): + """Test that forcing the disable of contact processing results in no contact reporting. + + We borrow the test :func:`test_cube_stack_contact_filtering` to test this and force disable contact processing. + """ + # TODO: This test only works on CPU. For GPU, it seems the contact processing is not disabled. + sim_dt, durations, terrains, devices, carb_settings_iface = setup_simulation + with build_simulation_context(device="cpu", dt=sim_dt, add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Instance new scene for the current terrain and contact prim. + scene_cfg = ContactSensorSceneCfg(num_envs=32, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG + # -- cube 1 + scene_cfg.shape = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_1") + scene_cfg.shape.init_state.pos = (0, -1.0, 1.0) + # -- cube 2 (on top of cube 1) + scene_cfg.shape_2 = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_2") + scene_cfg.shape_2.init_state.pos = (0, -1.0, 1.525) + # -- contact sensor 1 + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Cube_1", + track_pose=True, + debug_vis=False, + update_period=0.0, + filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_2"], + ) + # -- contact sensor 2 + scene_cfg.contact_sensor_2 = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Cube_2", + track_pose=True, + debug_vis=False, + update_period=0.0, + filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_1"], + ) + scene = InteractiveScene(scene_cfg) + + # Force disable contact processing + carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + + # Set variables internally for reference + sim.reset() + + # Extract from scene for type hinting + contact_sensor: ContactSensor = scene["contact_sensor"] + contact_sensor_2: ContactSensor = scene["contact_sensor_2"] + + # Check buffers have the right size + assert contact_sensor.contact_physx_view.filter_count == 1 + assert contact_sensor_2.contact_physx_view.filter_count == 1 + + # Reset the contact sensors + scene.reset() + # Let the scene come to a rest + for _ in range(500): + _perform_sim_step(sim, scene, sim_dt) + + # check values are zero (contacts are happening but not reported) + assert contact_sensor.data.net_forces_w.sum().item() == 0.0 + assert contact_sensor.data.force_matrix_w.sum().item() == 0.0 + assert contact_sensor_2.data.net_forces_w.sum().item() == 0.0 + assert contact_sensor_2.data.force_matrix_w.sum().item() == 0.0 + + +@pytest.mark.isaacsim_ci +def test_sensor_print(setup_simulation): + """Test sensor print is working correctly.""" + sim_dt, durations, terrains, devices, carb_settings_iface = setup_simulation + with build_simulation_context(device="cuda:0", dt=sim_dt, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + # Spawn things into stage + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground") + scene_cfg.shape = CUBE_CFG + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + # print info + print(scene.sensors["contact_sensor"]) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_contact_sensor_threshold(setup_simulation, device): + """Test that the contact sensor USD threshold attribute is set to 0.0.""" + sim_dt, durations, terrains, devices, carb_settings_iface = setup_simulation + with build_simulation_context(device=device, dt=sim_dt, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + # Spawn things into stage + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground") + scene_cfg.shape = CUBE_CFG + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + + stage = get_current_stage() + prim_path = scene_cfg.shape.prim_path + prim = stage.GetPrimAtPath(prim_path) + + # Ensure the contact sensor was created properly + contact_sensor = scene["contact_sensor"] + assert contact_sensor is not None, "Contact sensor was not created" + + # Check if the prim has contact report API and verify threshold is close to 0.0 + if prim.HasAPI(PhysxSchema.PhysxContactReportAPI): + cr_api = PhysxSchema.PhysxContactReportAPI.Get(stage, prim.GetPrimPath()) + threshold_attr = cr_api.GetThresholdAttr() + if threshold_attr.IsValid(): + threshold_value = threshold_attr.Get() + assert pytest.approx(threshold_value, abs=1e-6) == 0.0, ( + f"Expected USD threshold to be close to 0.0, but got {threshold_value}" + ) + - This class includes test cases for the available rigid object primitives, and tests that the - the contact sensor is reporting correct results for various contact durations, terrain types, and - evaluation devices. +# minor gravity force in -z to ensure object stays on ground plane +@pytest.mark.parametrize("grav_dir", [(-10.0, 0.0, -0.1), (0.0, -10.0, -0.1)]) +@pytest.mark.isaacsim_ci +def test_friction_reporting(setup_simulation, grav_dir): """ + Test friction force reporting for contact sensors. + + This test places a contact sensor enabled cube onto a ground plane under different gravity directions. + It then compares the normalized friction force dir with the direction of gravity to ensure they are aligned. + """ + sim_dt, _, _, _, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + device = "cuda:0" + sim_cfg = SimulationCfg(dt=sim_dt, device=device, gravity=grav_dir) + with build_simulation_context(sim_cfg=sim_cfg, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG + scene_cfg.shape = CUBE_CFG + + filter_prim_paths_expr = [scene_cfg.terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] + + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + track_friction_forces=True, + filter_prim_paths_expr=filter_prim_paths_expr, + ) + + scene = InteractiveScene(scene_cfg) + + sim.reset() + + scene["contact_sensor"].reset() + scene["shape"].write_root_pose_to_sim( + root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0]) + ) + + # step sim once to compute friction forces + _perform_sim_step(sim, scene, sim_dt) + + # check that forces are being reported match expected friction forces + expected_friction, _, _, _ = scene["contact_sensor"].contact_physx_view.get_friction_data(dt=sim_dt) + reported_friction = scene["contact_sensor"].data.friction_forces_w[0, 0, :] + + torch.testing.assert_close(expected_friction.sum(dim=0), reported_friction[0], atol=1e-6, rtol=1e-5) + + # check that friction force direction opposes gravity direction + grav = torch.tensor(grav_dir, device=device) + norm_reported_friction = reported_friction / reported_friction.norm() + norm_gravity = grav / grav.norm() + dot = torch.dot(norm_reported_friction[0], norm_gravity) + + torch.testing.assert_close(torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-4, rtol=1e-3) + + +@pytest.mark.isaacsim_ci +def test_invalid_prim_paths_config(setup_simulation): + sim_dt, _, _, _, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + device = "cuda:0" + sim_cfg = SimulationCfg(dt=sim_dt, device=device) + with build_simulation_context(sim_cfg=sim_cfg, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG + scene_cfg.shape = CUBE_CFG + + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + track_friction_forces=True, + filter_prim_paths_expr=[], + ) + + try: + _ = InteractiveScene(scene_cfg) + + sim.reset() + + assert False, "Expected ValueError due to invalid contact sensor configuration." + except ValueError: + pass + + +@pytest.mark.isaacsim_ci +def test_invalid_max_contact_points_config(setup_simulation): + sim_dt, _, _, _, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + device = "cuda:0" + sim_cfg = SimulationCfg(dt=sim_dt, device=device) + with build_simulation_context(sim_cfg=sim_cfg, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG + scene_cfg.shape = CUBE_CFG + filter_prim_paths_expr = [scene_cfg.terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] + + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + track_friction_forces=True, + filter_prim_paths_expr=filter_prim_paths_expr, + max_contact_data_count_per_prim=0, + ) + + try: + _ = InteractiveScene(scene_cfg) - @classmethod - def setUpClass(cls): - """Contact sensor test suite init.""" - cls.sim_dt = 0.0025 - cls.durations = [cls.sim_dt, cls.sim_dt * 2, cls.sim_dt * 32, cls.sim_dt * 128] - cls.terrains = [FLAT_TERRAIN_CFG, COBBLESTONE_TERRAIN_CFG] - cls.devices = ["cuda:0", "cpu"] - cls.carb_settings_iface = carb.settings.get_settings() - - def test_cube_contact_time(self): - """Checks contact sensor values for contact time and air time for a cube collision primitive.""" - # check for both contact processing enabled and disabled - # internally, the contact sensor should enable contact processing so it should always work. - for disable_contact_processing in [True, False]: - with self.subTest(disable_contact_processing=disable_contact_processing): - self.carb_settings_iface.set_bool("/physics/disableContactProcessing", disable_contact_processing) - self._run_contact_sensor_test(shape_cfg=CUBE_CFG) - - def test_sphere_contact_time(self): - """Checks contact sensor values for contact time and air time for a sphere collision primitive.""" - # check for both contact processing enabled and disabled - # internally, the contact sensor should enable contact processing so it should always work. - for disable_contact_processing in [True, False]: - with self.subTest(disable_contact_processing=disable_contact_processing): - self.carb_settings_iface.set_bool("/physics/disableContactProcessing", disable_contact_processing) - self._run_contact_sensor_test(shape_cfg=SPHERE_CFG) - - def test_cube_stack_contact_filtering(self): - """Checks contact sensor reporting for filtering stacked cube prims.""" - for device in self.devices: - for num_envs in [1, 6, 24]: - with self.subTest(device=device, num_envs=num_envs): - with build_simulation_context(device=device, dt=self.sim_dt, add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Instance new scene for the current terrain and contact prim. - scene_cfg = ContactSensorSceneCfg(num_envs=num_envs, env_spacing=1.0, lazy_sensor_update=False) - scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground") - # -- cube 1 - scene_cfg.shape = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_1") - scene_cfg.shape.init_state.pos = (0, -1.0, 1.0) - # -- cube 2 (on top of cube 1) - scene_cfg.shape_2 = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_2") - scene_cfg.shape_2.init_state.pos = (0, -1.0, 1.525) - # -- contact sensor 1 - scene_cfg.contact_sensor = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Cube_1", - track_pose=True, - debug_vis=False, - update_period=0.0, - filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_2"], - ) - # -- contact sensor 2 - scene_cfg.contact_sensor_2 = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Cube_2", - track_pose=True, - debug_vis=False, - update_period=0.0, - filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_1"], - ) - scene = InteractiveScene(scene_cfg) - - # Set variables internally for reference - self.sim = sim - self.scene = scene - - # Check that contact processing is enabled - self.assertFalse(self.carb_settings_iface.get("/physics/disableContactProcessing")) - - # Play the simulation - self.sim.reset() - - # Extract from scene for type hinting - contact_sensor: ContactSensor = self.scene["contact_sensor"] - contact_sensor_2: ContactSensor = self.scene["contact_sensor_2"] - # Check buffers have the right size - self.assertEqual(contact_sensor.contact_physx_view.filter_count, 1) - self.assertEqual(contact_sensor_2.contact_physx_view.filter_count, 1) - - # Reset the contact sensors - self.scene.reset() - # Let the scene come to a rest - for _ in range(500): - self._perform_sim_step() - - # Check values for cube 2 --> cube 1 is the only collision for cube 2 - torch.testing.assert_close( - contact_sensor_2.data.force_matrix_w[:, :, 0], contact_sensor_2.data.net_forces_w - ) - # Check that forces are opposite and equal - torch.testing.assert_close( - contact_sensor_2.data.force_matrix_w[:, :, 0], -contact_sensor.data.force_matrix_w[:, :, 0] - ) - # Check values are non-zero (contacts are happening and are getting reported) - self.assertGreater(contact_sensor_2.data.net_forces_w.sum().item(), 0.0) - self.assertGreater(contact_sensor.data.net_forces_w.sum().item(), 0.0) - - def test_no_contact_reporting(self): - """Test that forcing the disable of contact processing results in no contact reporting. - - We borrow the test :func:`test_cube_stack_contact_filtering` to test this and force disable contact processing. - """ - # TODO: This test only works on CPU. For GPU, it seems the contact processing is not disabled. - for device in ["cpu"]: - with self.subTest(device=device): - with build_simulation_context(device=device, dt=self.sim_dt, add_lighting=True) as sim: + sim.reset() + + assert False, "Expected ValueError due to invalid contact sensor configuration." + except ValueError: + pass + + +""" +Internal helpers. +""" + + +def _run_contact_sensor_test( + shape_cfg: ContactSensorRigidObjectCfg, + sim_dt: float, + devices: list[str], + terrains: list[TerrainImporterCfg], + carb_settings_iface, + durations: list[float], +): + """ + Runs a rigid body test for a given contact primitive configuration. + + This method iterates through each device and terrain combination in the simulation environment, + running tests for contact sensors. + """ + for device in devices: + for terrain in terrains: + for track_contact_data in [True, False]: + with build_simulation_context(device=device, dt=sim_dt, add_lighting=True) as sim: sim._app_control_on_stop_handle = None - # Instance new scene for the current terrain and contact prim. - scene_cfg = ContactSensorSceneCfg(num_envs=32, env_spacing=1.0, lazy_sensor_update=False) - scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground") - # -- cube 1 - scene_cfg.shape = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_1") - scene_cfg.shape.init_state.pos = (0, -1.0, 1.0) - # -- cube 2 (on top of cube 1) - scene_cfg.shape_2 = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_2") - scene_cfg.shape_2.init_state.pos = (0, -1.0, 1.525) - # -- contact sensor 1 + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = terrain + scene_cfg.shape = shape_cfg + test_contact_data = False + if (type(shape_cfg.spawn) is sim_utils.SphereCfg) and (terrain.terrain_type == "plane"): + test_contact_data = True + elif track_contact_data: + continue + + if track_contact_data: + if terrain.terrain_type == "plane": + filter_prim_paths_expr = [terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] + elif terrain.terrain_type == "generator": + filter_prim_paths_expr = [terrain.prim_path + "/terrain/mesh"] + else: + filter_prim_paths_expr = [] + scene_cfg.contact_sensor = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Cube_1", - track_pose=True, - debug_vis=False, - update_period=0.0, - filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_2"], - ) - # -- contact sensor 2 - scene_cfg.contact_sensor_2 = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Cube_2", + prim_path=shape_cfg.prim_path, track_pose=True, debug_vis=False, update_period=0.0, - filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_1"], + track_air_time=True, + history_length=3, + track_contact_points=track_contact_data, + track_friction_forces=track_contact_data, + filter_prim_paths_expr=filter_prim_paths_expr, ) scene = InteractiveScene(scene_cfg) - # Force disable contact processing - self.carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + # Play the simulation + sim.reset() + + # Run contact time and air time tests. + _test_sensor_contact( + shape=scene["shape"], + sensor=scene["contact_sensor"], + mode=ContactTestMode.IN_CONTACT, + sim=sim, + scene=scene, + sim_dt=sim_dt, + durations=durations, + test_contact_data=test_contact_data, + ) + _test_sensor_contact( + shape=scene["shape"], + sensor=scene["contact_sensor"], + mode=ContactTestMode.NON_CONTACT, + sim=sim, + scene=scene, + sim_dt=sim_dt, + durations=durations, + test_contact_data=test_contact_data, + ) - # Set variables internally for reference - self.sim = sim - self.scene = scene - # Play the simulation - self.sim.reset() - - # Extract from scene for type hinting - contact_sensor: ContactSensor = self.scene["contact_sensor"] - contact_sensor_2: ContactSensor = self.scene["contact_sensor_2"] - # Check buffers have the right size - self.assertEqual(contact_sensor.contact_physx_view.filter_count, 1) - self.assertEqual(contact_sensor_2.contact_physx_view.filter_count, 1) - - # Reset the contact sensors - self.scene.reset() - # Let the scene come to a rest - for _ in range(500): - self._perform_sim_step() - - # check values are zero (contacts are happening but not reported) - self.assertEqual(contact_sensor.data.net_forces_w.sum().item(), 0.0) - self.assertEqual(contact_sensor.data.force_matrix_w.sum().item(), 0.0) - self.assertEqual(contact_sensor_2.data.net_forces_w.sum().item(), 0.0) - self.assertEqual(contact_sensor_2.data.force_matrix_w.sum().item(), 0.0) - - def test_sensor_print(self): - """Test sensor print is working correctly.""" - with build_simulation_context(device="cuda:0", dt=self.sim_dt, add_lighting=False) as sim: - sim._app_control_on_stop_handle = None - # Spawn things into stage - scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) - scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground") - scene_cfg.shape = CUBE_CFG - scene_cfg.contact_sensor = ContactSensorCfg( - prim_path=scene_cfg.shape.prim_path, - track_pose=True, - debug_vis=False, - update_period=0.0, - track_air_time=True, - history_length=3, +def _test_sensor_contact( + shape: RigidObject, + sensor: ContactSensor, + mode: ContactTestMode, + sim: SimulationContext, + scene: InteractiveScene, + sim_dt: float, + durations: list[float], + test_contact_data: bool = False, +): + """Test for the contact sensor. + + This test sets the contact prim to a pose either in contact or out of contact with the ground plane for + a known duration. Once the contact duration has elapsed, the data stored inside the contact sensor + associated with the contact prim is checked against the expected values. + + This process is repeated for all elements in :attr:`TestContactSensor.durations`, where each successive + contact timing test is punctuated by setting the contact prim to the complement of the desired contact mode + for 1 sim time-step. + + Args: + shape: The contact prim used for the contact sensor test. + sensor: The sensor reporting data to be verified by the contact sensor test. + mode: The contact test mode: either contact with ground plane or air time. + """ + # reset the test state + sensor.reset() + expected_last_test_contact_time = 0 + expected_last_reset_contact_time = 0 + + # set poses for shape for a given contact sensor test mode. + # desired contact mode to set for a given duration. + test_pose = None + # complement of the desired contact mode used to reset the contact sensor. + reset_pose = None + if mode == ContactTestMode.IN_CONTACT: + test_pose = shape.cfg.contact_pose + reset_pose = shape.cfg.non_contact_pose + elif mode == ContactTestMode.NON_CONTACT: + test_pose = shape.cfg.non_contact_pose + reset_pose = shape.cfg.contact_pose + else: + raise ValueError("Received incompatible contact sensor test mode") + + for idx in range(len(durations)): + current_test_time = 0 + duration = durations[idx] + while current_test_time < duration: + # set object states to contact the ground plane + shape.write_root_pose_to_sim(root_pose=test_pose) + # perform simulation step + _perform_sim_step(sim, scene, sim_dt) + # increment contact time + current_test_time += sim_dt + # set last contact time to the previous desired contact duration plus the extra dt allowance. + expected_last_test_contact_time = durations[idx - 1] + sim_dt if idx > 0 else 0 + # Check the data inside the contact sensor + if mode == ContactTestMode.IN_CONTACT: + _check_prim_contact_state_times( + sensor=sensor, + expected_air_time=0.0, + expected_contact_time=durations[idx], + expected_last_contact_time=expected_last_test_contact_time, + expected_last_air_time=expected_last_reset_contact_time, + dt=duration + sim_dt, + ) + elif mode == ContactTestMode.NON_CONTACT: + _check_prim_contact_state_times( + sensor=sensor, + expected_air_time=durations[idx], + expected_contact_time=0.0, + expected_last_contact_time=expected_last_reset_contact_time, + expected_last_air_time=expected_last_test_contact_time, + dt=duration + sim_dt, ) - scene = InteractiveScene(scene_cfg) - # Play the simulator - sim.reset() - # print info - print(scene.sensors["contact_sensor"]) + if test_contact_data: + _test_contact_position(shape, sensor, mode) + _test_friction_forces(shape, sensor, mode) + + # switch the contact mode for 1 dt step before the next contact test begins. + shape.write_root_pose_to_sim(root_pose=reset_pose) + # perform simulation step + _perform_sim_step(sim, scene, sim_dt) + # set the last air time to 2 sim_dt steps, because last_air_time and last_contact_time + # adds an additional sim_dt to the total time spent in the previous contact mode for uncertainty in + # when the contact switch happened in between a dt step. + expected_last_reset_contact_time = 2 * sim_dt + + +def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: + if not sensor.cfg.track_friction_forces: + assert sensor._data.friction_forces_w is None + return + + # check shape of the contact_pos_w tensor + num_bodies = sensor.num_bodies + assert sensor._data.friction_forces_w.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) + # compare friction forces + if mode == ContactTestMode.IN_CONTACT: + assert torch.any(torch.abs(sensor._data.friction_forces_w) > 1e-5).item() + friction_forces, _, buffer_count, buffer_start_indices = sensor.contact_physx_view.get_friction_data( + dt=sensor._sim_physics_dt + ) + for i in range(sensor.num_instances * num_bodies): + for j in range(sensor.contact_physx_view.filter_count): + start_index_ij = buffer_start_indices[i, j] + count_ij = buffer_count[i, j] + force = torch.sum(friction_forces[start_index_ij : (start_index_ij + count_ij), :], dim=0) + env_idx = i // num_bodies + body_idx = i % num_bodies + assert torch.allclose(force, sensor._data.friction_forces_w[env_idx, body_idx, j, :], atol=1e-5) + + elif mode == ContactTestMode.NON_CONTACT: + assert torch.all(sensor._data.friction_forces_w == 0.0).item() + + +def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: + """Test for the contact positions (only implemented for sphere and flat terrain) + checks that the contact position is radius distance away from the root of the object + Args: + shape: The contact prim used for the contact sensor test. + sensor: The sensor reporting data to be verified by the contact sensor test. + mode: The contact test mode: either contact with ground plane or air time. """ - Internal helpers. + if not sensor.cfg.track_contact_points: + assert sensor._data.contact_pos_w is None + return + + # check shape of the contact_pos_w tensor + num_bodies = sensor.num_bodies + assert sensor._data.contact_pos_w.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) + # check contact positions + if mode == ContactTestMode.IN_CONTACT: + contact_position = sensor._data.pos_w + torch.tensor( + [[0.0, 0.0, -shape.cfg.spawn.radius]], device=sensor._data.pos_w.device + ) + assert torch.all( + torch.abs(torch.norm(sensor._data.contact_pos_w - contact_position.unsqueeze(1), p=2, dim=-1)) < 1e-2 + ).item() + elif mode == ContactTestMode.NON_CONTACT: + assert torch.all(torch.isnan(sensor._data.contact_pos_w)).item() + + +def _check_prim_contact_state_times( + sensor: ContactSensor, + expected_air_time: float, + expected_contact_time: float, + expected_last_air_time: float, + expected_last_contact_time: float, + dt: float, +): + """Checks contact sensor data matches expected values. + + Args: + sensor: Instance of ContactSensor containing data to be tested. + expected_air_time: Air time ground truth. + expected_contact_time: Contact time ground truth. + expected_last_air_time: Last air time ground truth. + expected_last_contact_time: Last contact time ground truth. + dt: Time since previous contact mode switch. If the contact prim left contact 0.1 seconds ago, + dt should be 0.1 + simulation dt seconds. """ - - def _run_contact_sensor_test(self, shape_cfg: TestContactSensorRigidObjectCfg): - """Runs a rigid body test for a given contact primitive configuration. - - This method iterates through each device and terrain combination in the simulation environment, - running tests for contact sensors. - - Args: - shape_cfg: The configuration parameters for the shape to be tested. - """ - for device in self.devices: - for terrain in self.terrains: - with self.subTest(device=device, terrain=terrain): - with build_simulation_context(device=device, dt=self.sim_dt, add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Instance new scene for the current terrain and contact prim. - scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) - scene_cfg.terrain = terrain - scene_cfg.shape = shape_cfg - scene_cfg.contact_sensor = ContactSensorCfg( - prim_path=shape_cfg.prim_path, - track_pose=True, - debug_vis=False, - update_period=0.0, - track_air_time=True, - history_length=3, - ) - scene = InteractiveScene(scene_cfg) - - # Set variables internally for reference - self.sim = sim - self.scene = scene - - # Check that contact processing is enabled - self.assertFalse(self.carb_settings_iface.get("/physics/disableContactProcessing")) - - # Play the simulation - self.sim.reset() - - # Run contact time and air time tests. - self._test_sensor_contact( - shape=self.scene["shape"], - sensor=self.scene["contact_sensor"], - mode=ContactTestMode.IN_CONTACT, - ) - self._test_sensor_contact( - shape=self.scene["shape"], - sensor=self.scene["contact_sensor"], - mode=ContactTestMode.NON_CONTACT, - ) - - def _test_sensor_contact(self, shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode): - """Test for the contact sensor. - - This test sets the contact prim to a pose either in contact or out of contact with the ground plane for - a known duration. Once the contact duration has elapsed, the data stored inside the contact sensor - associated with the contact prim is checked against the expected values. - - This process is repeated for all elements in :attr:`TestContactSensor.durations`, where each successive - contact timing test is punctuated by setting the contact prim to the complement of the desired contact mode - for 1 sim time-step. - - Args: - shape: The contact prim used for the contact sensor test. - sensor: The sensor reporting data to be verified by the contact sensor test. - mode: The contact test mode: either contact with ground plane or air time. - """ - # reset the test state - sensor.reset() - expected_last_test_contact_time = 0 - expected_last_reset_contact_time = 0 - - # set poses for shape for a given contact sensor test mode. - # desired contact mode to set for a given duration. - test_pose = None - # complement of the desired contact mode used to reset the contact sensor. - reset_pose = None - if mode == ContactTestMode.IN_CONTACT: - test_pose = shape.cfg.contact_pose - reset_pose = shape.cfg.non_contact_pose - elif mode == ContactTestMode.NON_CONTACT: - test_pose = shape.cfg.non_contact_pose - reset_pose = shape.cfg.contact_pose - else: - raise ValueError("Received incompatible contact sensor test mode") - - for idx in range(len(self.durations)): - current_test_time = 0 - duration = self.durations[idx] - while current_test_time < duration: - # set object states to contact the ground plane - shape.write_root_pose_to_sim(root_pose=test_pose) - # perform simulation step - self._perform_sim_step() - # increment contact time - current_test_time += self.sim_dt - # set last contact time to the previous desired contact duration plus the extra dt allowance. - expected_last_test_contact_time = self.durations[idx - 1] + self.sim_dt if idx > 0 else 0 - # Check the data inside the contact sensor - if mode == ContactTestMode.IN_CONTACT: - self._check_prim_contact_state_times( - sensor=sensor, - expected_air_time=0.0, - expected_contact_time=self.durations[idx], - expected_last_contact_time=expected_last_test_contact_time, - expected_last_air_time=expected_last_reset_contact_time, - dt=duration + self.sim_dt, - ) - elif mode == ContactTestMode.NON_CONTACT: - self._check_prim_contact_state_times( - sensor=sensor, - expected_air_time=self.durations[idx], - expected_contact_time=0.0, - expected_last_contact_time=expected_last_reset_contact_time, - expected_last_air_time=expected_last_test_contact_time, - dt=duration + self.sim_dt, - ) - # switch the contact mode for 1 dt step before the next contact test begins. - shape.write_root_pose_to_sim(root_pose=reset_pose) - # perform simulation step - self._perform_sim_step() - # set the last air time to 2 sim_dt steps, because last_air_time and last_contact_time - # adds an additional sim_dt to the total time spent in the previous contact mode for uncertainty in - # when the contact switch happened in between a dt step. - expected_last_reset_contact_time = 2 * self.sim_dt - - def _check_prim_contact_state_times( - self, - sensor: ContactSensor, - expected_air_time: float, - expected_contact_time: float, - expected_last_air_time: float, - expected_last_contact_time: float, - dt: float, - ) -> None: - """Checks contact sensor data matches expected values. - - Args: - sensor: Instance of ContactSensor containing data to be tested. - expected_air_time: Air time ground truth. - expected_contact_time: Contact time ground truth. - expected_last_air_time: Last air time ground truth. - expected_last_contact_time: Last contact time ground truth. - dt: Time since previous contact mode switch. If the contact prim left contact 0.1 seconds ago, - dt should be 0.1 + simulation dt seconds. - """ - # store current state of the contact prim - in_air = False - in_contact = False - if expected_air_time > 0.0: - in_air = True - if expected_contact_time > 0.0: - in_contact = True - measured_contact_time = sensor.data.current_contact_time - measured_air_time = sensor.data.current_air_time - measured_last_contact_time = sensor.data.last_contact_time - measured_last_air_time = sensor.data.last_air_time - # check current contact state - self.assertAlmostEqual(measured_contact_time.item(), expected_contact_time, places=2) - self.assertAlmostEqual(measured_air_time.item(), expected_air_time, places=2) - # check last contact state - self.assertAlmostEqual(measured_last_contact_time.item(), expected_last_contact_time, places=2) - self.assertAlmostEqual(measured_last_air_time.item(), expected_last_air_time, places=2) - # check current contact mode - self.assertEqual(sensor.compute_first_contact(dt=dt).item(), in_contact) - self.assertEqual(sensor.compute_first_air(dt=dt).item(), in_air) - - def _perform_sim_step(self) -> None: - """Updates sensors and steps the contact sensor test scene.""" - # write data to simulation - self.scene.write_data_to_sim() - # simulate - self.sim.step(render=False) - # update buffers at sim dt - self.scene.update(dt=self.sim_dt) - - -if __name__ == "__main__": - run_tests() + # store current state of the contact prim + in_air = False + in_contact = False + if expected_air_time > 0.0: + in_air = True + if expected_contact_time > 0.0: + in_contact = True + measured_contact_time = sensor.data.current_contact_time + measured_air_time = sensor.data.current_air_time + measured_last_contact_time = sensor.data.last_contact_time + measured_last_air_time = sensor.data.last_air_time + # check current contact state + assert pytest.approx(measured_contact_time.item(), 0.01) == expected_contact_time + assert pytest.approx(measured_air_time.item(), 0.01) == expected_air_time + # check last contact state + assert pytest.approx(measured_last_contact_time.item(), 0.01) == expected_last_contact_time + assert pytest.approx(measured_last_air_time.item(), 0.01) == expected_last_air_time + # check current contact mode + assert sensor.compute_first_contact(dt=dt).item() == in_contact + assert sensor.compute_first_air(dt=dt).item() == in_air + + +def _perform_sim_step(sim, scene, sim_dt): + """Updates sensors and steps the contact sensor test scene.""" + # write data to simulation + scene.write_data_to_sim() + # simulate + sim.step(render=False) + # update buffers at sim dt + scene.update(dt=sim_dt) diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 4b7859a596c4..5e0ccf8e1f35 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -1,24 +1,22 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import math + +import pytest import scipy.spatial.transform as tf import torch -import unittest - -import isaacsim.core.utils.stage as stage_utils import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils @@ -73,536 +71,726 @@ class MySceneCfg(InteractiveSceneCfg): ) -class TestFrameTransformer(unittest.TestCase): - """Test for frame transformer sensor.""" +@pytest.fixture +def sim(): + """Create a simulation context.""" + # Create a new stage + sim_utils.create_new_stage() + # Load kit helper + sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.005, device="cpu")) + # Set main camera + sim.set_camera_view(eye=(5.0, 5.0, 5.0), target=(0.0, 0.0, 0.0)) + yield sim + # Cleanup + sim.clear_all_callbacks() + sim.clear_instance() - def setUp(self): - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Load kit helper - self.sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.005, device="cpu")) - # Set main camera - self.sim.set_camera_view(eye=[5, 5, 5], target=[0.0, 0.0, 0.0]) - def tearDown(self): - """Stops simulator after each test.""" - # stop simulation - # self.sim.stop() - # clear the stage - self.sim.clear_all_callbacks() - self.sim.clear_instance() +def test_frame_transformer_feet_wrt_base(sim): + """Test feet transformations w.r.t. base source frame. + In this test, the source frame is the robot base. """ - Tests - """ - - def test_frame_transformer_feet_wrt_base(self): - """Test feet transformations w.r.t. base source frame. - - In this test, the source frame is the robot base. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=32, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="LF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=32, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="LF_FOOT_USER", + prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, -math.pi / 2), ), - FrameTransformerCfg.FrameCfg( - name="RF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), + ), + FrameTransformerCfg.FrameCfg( + name="RF_FOOT_USER", + prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, math.pi / 2), ), - FrameTransformerCfg.FrameCfg( - name="LH_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LH_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(-0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), + ), + FrameTransformerCfg.FrameCfg( + name="LH_FOOT_USER", + prim_path="{ENV_REGEX_NS}/Robot/LH_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(-0.08795, 0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, -math.pi / 2), + ), + ), + FrameTransformerCfg.FrameCfg( + name="RH_FOOT_USER", + prim_path="{ENV_REGEX_NS}/Robot/RH_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(-0.08795, -0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, math.pi / 2), + ), + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + # Acquire the index of ground truth bodies + feet_indices, feet_names = scene.articulations["robot"].find_bodies(["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"]) + + target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names + + # Reorder the feet indices to match the order of the target frames with _USER suffix removed + target_frame_names = [name.split("_USER")[0] for name in target_frame_names] + + # Find the indices of the feet in the order of the target frames + reordering_indices = [feet_names.index(name) for name in target_frame_names] + feet_indices = [feet_indices[i] for i in reordering_indices] + + # default joint targets + default_actions = scene.articulations["robot"].data.default_joint_pos.clone() + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene.articulations["robot"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + joint_pos = scene.articulations["robot"].data.default_joint_pos + joint_vel = scene.articulations["robot"].data.default_joint_vel + # -- set root state + # -- robot + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + # reset buffers + scene.reset() + + # set joint targets + robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) + scene.articulations["robot"].set_joint_position_target(robot_actions) + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + root_pose_w = scene.articulations["robot"].data.root_pose_w + feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w[:, feet_indices] + feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w[:, feet_indices] + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w + feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w + + # check if they are same + torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) + torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) + torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) + torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) + + # check if relative transforms are same + feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source + feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source + for index in range(len(feet_indices)): + # ground-truth + foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( + root_pose_w[:, :3], root_pose_w[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] + ) + # check if they are same + torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) + torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) + + +def test_frame_transformer_feet_wrt_thigh(sim): + """Test feet transformation w.r.t. thigh source frame.""" + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=32, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/LF_THIGH", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="LF_FOOT_USER", + prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, -math.pi / 2), ), - FrameTransformerCfg.FrameCfg( - name="RH_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RH_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(-0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), + ), + FrameTransformerCfg.FrameCfg( + name="RF_FOOT_USER", + prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, math.pi / 2), ), - ], + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + # Acquire the index of ground truth bodies + source_frame_index = scene.articulations["robot"].find_bodies("LF_THIGH")[0][0] + feet_indices, feet_names = scene.articulations["robot"].find_bodies(["LF_FOOT", "RF_FOOT"]) + # Check names are parsed the same order + user_feet_names = [f"{name}_USER" for name in feet_names] + assert scene.sensors["frame_transformer"].data.target_frame_names == user_feet_names + + # default joint targets + default_actions = scene.articulations["robot"].data.default_joint_pos.clone() + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene.articulations["robot"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + joint_pos = scene.articulations["robot"].data.default_joint_pos + joint_vel = scene.articulations["robot"].data.default_joint_vel + # -- set root state + # -- robot + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + # reset buffers + scene.reset() + + # set joint targets + robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) + scene.articulations["robot"].set_joint_position_target(robot_actions) + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + source_pose_w_gt = scene.articulations["robot"].data.body_state_w[:, source_frame_index, :7] + feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w[:, feet_indices] + feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w[:, feet_indices] + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w + feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w + # check if they are same + torch.testing.assert_close(source_pose_w_gt[:, :3], source_pos_w_tf) + torch.testing.assert_close(source_pose_w_gt[:, 3:], source_quat_w_tf) + torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) + torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) + + # check if relative transforms are same + feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source + feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source + for index in range(len(feet_indices)): + # ground-truth + foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( + source_pose_w_gt[:, :3], source_pose_w_gt[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] + ) + # check if they are same + torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) + torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) + + +def test_frame_transformer_robot_body_to_external_cube(sim): + """Test transformation from robot body to a cube in the scene.""" + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="CUBE_USER", + prim_path="{ENV_REGEX_NS}/cube", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + # default joint targets + default_actions = scene.articulations["robot"].data.default_joint_pos.clone() + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene.articulations["robot"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + joint_pos = scene.articulations["robot"].data.default_joint_pos + joint_vel = scene.articulations["robot"].data.default_joint_vel + # -- set root state + # -- robot + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + # reset buffers + scene.reset() + + # set joint targets + robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) + scene.articulations["robot"].set_joint_position_target(robot_actions) + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + root_pose_w = scene.articulations["robot"].data.root_pose_w + cube_pos_w_gt = scene.rigid_objects["cube"].data.root_pos_w + cube_quat_w_gt = scene.rigid_objects["cube"].data.root_quat_w + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + cube_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.squeeze() + cube_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.squeeze() + + # check if they are same + torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) + torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) + torch.testing.assert_close(cube_pos_w_gt, cube_pos_w_tf) + torch.testing.assert_close(cube_quat_w_gt, cube_quat_w_tf) + + # check if relative transforms are same + cube_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source + cube_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source + # ground-truth + cube_pos_b, cube_quat_b = math_utils.subtract_frame_transforms( + root_pose_w[:, :3], root_pose_w[:, 3:], cube_pos_w_tf, cube_quat_w_tf ) - scene = InteractiveScene(scene_cfg) + # check if they are same + torch.testing.assert_close(cube_pos_source_tf[:, 0], cube_pos_b) + torch.testing.assert_close(cube_quat_source_tf[:, 0], cube_quat_b) - # Play the simulator - self.sim.reset() - # Acquire the index of ground truth bodies - feet_indices, feet_names = scene.articulations["robot"].find_bodies( - ["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"] - ) +@pytest.mark.isaacsim_ci +def test_frame_transformer_offset_frames(sim): + """Test body transformation w.r.t. base source frame. + In this test, the source frame is the cube frame. + """ + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/cube", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="CUBE_CENTER", + prim_path="{ENV_REGEX_NS}/cube", + ), + FrameTransformerCfg.FrameCfg( + name="CUBE_TOP", + prim_path="{ENV_REGEX_NS}/cube", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.1), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ), + FrameTransformerCfg.FrameCfg( + name="CUBE_BOTTOM", + prim_path="{ENV_REGEX_NS}/cube", + offset=OffsetCfg( + pos=(0.0, 0.0, -0.1), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene["cube"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + # -- set root state + # -- cube + scene["cube"].write_root_pose_to_sim(root_state[:, :7]) + scene["cube"].write_root_velocity_to_sim(root_state[:, 7:]) + # reset buffers + scene.reset() + + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + cube_pos_w_gt = scene["cube"].data.root_pos_w + cube_quat_w_gt = scene["cube"].data.root_quat_w + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + target_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.squeeze() + target_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.squeeze() target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - # Reorder the feet indices to match the order of the target frames with _USER suffix removed - target_frame_names = [name.split("_USER")[0] for name in target_frame_names] - - # Find the indices of the feet in the order of the target frames - reordering_indices = [feet_names.index(name) for name in target_frame_names] - feet_indices = [feet_indices[i] for i in reordering_indices] - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.clone() - # Define simulation stepping - sim_dt = self.sim.get_physics_dt() - # Simulate physics - for count in range(100): - # # reset - if count % 25 == 0: - # reset root state - root_state = scene.articulations["robot"].data.default_root_state.clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos - joint_vel = scene.articulations["robot"].data.default_joint_vel - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] - feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w[:, feet_indices] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w - feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w - feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w + cube_center_idx = target_frame_names.index("CUBE_CENTER") + cube_bottom_idx = target_frame_names.index("CUBE_BOTTOM") + cube_top_idx = target_frame_names.index("CUBE_TOP") - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) - - # check if relative transforms are same - feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source - feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source - for index in range(len(feet_indices)): - # ground-truth - foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] - ) - # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) - - def test_frame_transformer_feet_wrt_thigh(self): - """Test feet transformation w.r.t. thigh source frame. - - In this test, the source frame is the LF leg's thigh frame. This frame is not at index 0, - when the frame bodies are sorted in the order of the regex matching in the frame transformer. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=32, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/LF_THIGH", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="LF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), - ), - FrameTransformerCfg.FrameCfg( - name="RF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), - ), - ], - ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - self.sim.reset() - - # Acquire the index of ground truth bodies - source_frame_index = scene.articulations["robot"].find_bodies("LF_THIGH")[0][0] - feet_indices, feet_names = scene.articulations["robot"].find_bodies(["LF_FOOT", "RF_FOOT"]) - # Check names are parsed the same order - user_feet_names = [f"{name}_USER" for name in feet_names] - self.assertListEqual(scene.sensors["frame_transformer"].data.target_frame_names, user_feet_names) - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.clone() - # Define simulation stepping - sim_dt = self.sim.get_physics_dt() - # Simulate physics - for count in range(100): - # # reset - if count % 25 == 0: - # reset root state - root_state = scene.articulations["robot"].data.default_root_state.clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos - joint_vel = scene.articulations["robot"].data.default_joint_vel - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - source_pose_w_gt = scene.articulations["robot"].data.body_state_w[:, source_frame_index, :7] - feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w[:, feet_indices] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w - feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w - feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w - # check if they are same - torch.testing.assert_close(source_pose_w_gt[:, :3], source_pos_w_tf) - torch.testing.assert_close(source_pose_w_gt[:, 3:], source_quat_w_tf) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) - - # check if relative transforms are same - feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source - feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source - for index in range(len(feet_indices)): - # ground-truth - foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( - source_pose_w_gt[:, :3], source_pose_w_gt[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] - ) - # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) - - def test_frame_transformer_robot_body_to_external_cube(self): - """Test transformation from robot body to a cube in the scene. - - In this test, the source frame is the robot base. - - The target_frame is a cube in the scene, external to the robot. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="CUBE_USER", - prim_path="{ENV_REGEX_NS}/cube", - ), - ], - ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - self.sim.reset() - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.clone() - # Define simulation stepping - sim_dt = self.sim.get_physics_dt() - # Simulate physics - for count in range(100): - # # reset - if count % 25 == 0: - # reset root state - root_state = scene.articulations["robot"].data.default_root_state.clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos - joint_vel = scene.articulations["robot"].data.default_joint_vel - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] - cube_pos_w_gt = scene.rigid_objects["cube"].data.root_state_w[:, :3] - cube_quat_w_gt = scene.rigid_objects["cube"].data.root_state_w[:, 3:7] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w - cube_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.squeeze() - cube_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.squeeze() + # check if they are same + torch.testing.assert_close(cube_pos_w_gt, source_pos_w_tf) + torch.testing.assert_close(cube_quat_w_gt, source_quat_w_tf) + torch.testing.assert_close(cube_pos_w_gt, target_pos_w_tf[:, cube_center_idx]) + torch.testing.assert_close(cube_quat_w_gt, target_quat_w_tf[:, cube_center_idx]) - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(cube_pos_w_gt, cube_pos_w_tf) - torch.testing.assert_close(cube_quat_w_gt, cube_quat_w_tf) - - # check if relative transforms are same - cube_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source - cube_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source - # ground-truth - cube_pos_b, cube_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], cube_pos_w_tf, cube_quat_w_tf + # test offsets are applied correctly + # -- cube top + cube_pos_top = target_pos_w_tf[:, cube_top_idx] + cube_quat_top = target_quat_w_tf[:, cube_top_idx] + torch.testing.assert_close(cube_pos_top, cube_pos_w_gt + torch.tensor([0.0, 0.0, 0.1])) + torch.testing.assert_close(cube_quat_top, cube_quat_w_gt) + + # -- cube bottom + cube_pos_bottom = target_pos_w_tf[:, cube_bottom_idx] + cube_quat_bottom = target_quat_w_tf[:, cube_bottom_idx] + torch.testing.assert_close(cube_pos_bottom, cube_pos_w_gt + torch.tensor([0.0, 0.0, -0.1])) + torch.testing.assert_close(cube_quat_bottom, cube_quat_w_gt) + + +@pytest.mark.isaacsim_ci +def test_frame_transformer_all_bodies(sim): + """Test transformation of all bodies w.r.t. base source frame. + + In this test, the source frame is the robot base. + + The target_frames are all bodies in the robot, implemented using .* pattern. + """ + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/.*", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names + articulation_body_names = scene.articulations["robot"].data.body_names + + reordering_indices = [target_frame_names.index(name) for name in articulation_body_names] + + # default joint targets + default_actions = scene.articulations["robot"].data.default_joint_pos.clone() + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene.articulations["robot"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + joint_pos = scene.articulations["robot"].data.default_joint_pos + joint_vel = scene.articulations["robot"].data.default_joint_vel + # -- set root state + # -- robot + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + # reset buffers + scene.reset() + + # set joint targets + robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) + scene.articulations["robot"].set_joint_position_target(robot_actions) + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + root_pose_w = scene.articulations["robot"].data.root_pose_w + bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w + bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w + + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + bodies_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w + bodies_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w + + # check if they are same + torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) + torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) + torch.testing.assert_close(bodies_pos_w_gt, bodies_pos_w_tf[:, reordering_indices]) + torch.testing.assert_close(bodies_quat_w_gt, bodies_quat_w_tf[:, reordering_indices]) + + bodies_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source + bodies_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source + + # Go through each body and check if relative transforms are same + for index in range(len(articulation_body_names)): + body_pos_b, body_quat_b = math_utils.subtract_frame_transforms( + root_pose_w[:, :3], root_pose_w[:, 3:], bodies_pos_w_tf[:, index], bodies_quat_w_tf[:, index] ) - # check if they are same - torch.testing.assert_close(cube_pos_source_tf[:, 0], cube_pos_b) - torch.testing.assert_close(cube_quat_source_tf[:, 0], cube_quat_b) - - def test_frame_transformer_offset_frames(self): - """Test body transformation w.r.t. base source frame. - - In this test, the source frame is the cube frame. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/cube", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="CUBE_CENTER", - prim_path="{ENV_REGEX_NS}/cube", - ), - FrameTransformerCfg.FrameCfg( - name="CUBE_TOP", - prim_path="{ENV_REGEX_NS}/cube", - offset=OffsetCfg( - pos=(0.0, 0.0, 0.1), - rot=(1.0, 0.0, 0.0, 0.0), - ), - ), - FrameTransformerCfg.FrameCfg( - name="CUBE_BOTTOM", - prim_path="{ENV_REGEX_NS}/cube", - offset=OffsetCfg( - pos=(0.0, 0.0, -0.1), - rot=(1.0, 0.0, 0.0, 0.0), - ), - ), - ], - ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - self.sim.reset() - - # Define simulation stepping - sim_dt = self.sim.get_physics_dt() - # Simulate physics - for count in range(100): - # # reset - if count % 25 == 0: - # reset root state - root_state = scene["cube"].data.default_root_state.clone() - root_state[:, :3] += scene.env_origins - # -- set root state - # -- cube - scene["cube"].write_root_pose_to_sim(root_state[:, :7]) - scene["cube"].write_root_velocity_to_sim(root_state[:, 7:]) - # reset buffers - scene.reset() - - # write data to sim - scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - cube_pos_w_gt = scene["cube"].data.root_state_w[:, :3] - cube_quat_w_gt = scene["cube"].data.root_state_w[:, 3:7] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w - target_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.squeeze() - target_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.squeeze() - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - - cube_center_idx = target_frame_names.index("CUBE_CENTER") - cube_bottom_idx = target_frame_names.index("CUBE_BOTTOM") - cube_top_idx = target_frame_names.index("CUBE_TOP") - # check if they are same - torch.testing.assert_close(cube_pos_w_gt, source_pos_w_tf) - torch.testing.assert_close(cube_quat_w_gt, source_quat_w_tf) - torch.testing.assert_close(cube_pos_w_gt, target_pos_w_tf[:, cube_center_idx]) - torch.testing.assert_close(cube_quat_w_gt, target_quat_w_tf[:, cube_center_idx]) - - # test offsets are applied correctly - # -- cube top - cube_pos_top = target_pos_w_tf[:, cube_top_idx] - cube_quat_top = target_quat_w_tf[:, cube_top_idx] - torch.testing.assert_close(cube_pos_top, cube_pos_w_gt + torch.tensor([0.0, 0.0, 0.1])) - torch.testing.assert_close(cube_quat_top, cube_quat_w_gt) - - # -- cube bottom - cube_pos_bottom = target_pos_w_tf[:, cube_bottom_idx] - cube_quat_bottom = target_quat_w_tf[:, cube_bottom_idx] - torch.testing.assert_close(cube_pos_bottom, cube_pos_w_gt + torch.tensor([0.0, 0.0, -0.1])) - torch.testing.assert_close(cube_quat_bottom, cube_quat_w_gt) - - def test_frame_transformer_all_bodies(self): - """Test transformation of all bodies w.r.t. base source frame. - - In this test, the source frame is the robot base. - - The target_frames are all bodies in the robot, implemented using .* pattern. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/.*", - ), - ], - ) - scene = InteractiveScene(scene_cfg) + torch.testing.assert_close(bodies_pos_source_tf[:, index], body_pos_b) + torch.testing.assert_close(bodies_quat_source_tf[:, index], body_quat_b) + + +@pytest.mark.isaacsim_ci +def test_sensor_print(sim): + """Test sensor print is working correctly.""" + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/.*", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + # print info + print(scene.sensors["frame_transformer"]) + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("source_robot", ["Robot", "Robot_1"]) +@pytest.mark.parametrize("path_prefix", ["{ENV_REGEX_NS}", "/World"]) +def test_frame_transformer_duplicate_body_names(sim, source_robot, path_prefix): + """Test tracking bodies with same leaf name at different hierarchy levels. + + This test verifies that bodies with the same leaf name but different paths + (e.g., Robot/LF_SHANK vs Robot_1/LF_SHANK, or arm/link vs leg/link) are tracked + separately using their full relative paths internally. + + The test uses 4 target frames to cover both scenarios: + + Explicit frame names (recommended when bodies share the same leaf name): + User provides unique names like "Robot_LF_SHANK" and "Robot_1_LF_SHANK" to + distinguish between bodies at different hierarchy levels. This makes it + easy to identify which transform belongs to which body. + + Implicit frame names (backward compatibility): + When no name is provided, it defaults to the leaf body name (e.g., "RF_SHANK"). + This preserves backward compatibility for users who may have existing code like + `idx = target_frame_names.index("RF_SHANK")`. However, when multiple bodies share + the same leaf name, this results in duplicate frame names. The transforms are + still distinct because internal body tracking uses full relative paths. + + Args: + source_robot: The robot to use as the source frame ("Robot" or "Robot_1"). + This tests that both source frames work correctly when there are + duplicate body names. + path_prefix: The path prefix to use ("{ENV_REGEX_NS}" for env patterns or "/World" for direct paths). + """ - # Play the simulator - self.sim.reset() + # Create a custom scene config with two robots + @configclass + class MultiRobotSceneCfg(InteractiveSceneCfg): + """Scene with two robots having bodies with same names.""" - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - articulation_body_names = scene.articulations["robot"].data.body_names - - reordering_indices = [target_frame_names.index(name) for name in articulation_body_names] - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.clone() - # Define simulation stepping - sim_dt = self.sim.get_physics_dt() - # Simulate physics - for count in range(100): - # # reset - if count % 25 == 0: - # reset root state - root_state = scene.articulations["robot"].data.default_root_state.clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos - joint_vel = scene.articulations["robot"].data.default_joint_vel - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] - bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w - bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w - - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w - bodies_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w - bodies_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(bodies_pos_w_gt, bodies_pos_w_tf[:, reordering_indices]) - torch.testing.assert_close(bodies_quat_w_gt, bodies_quat_w_tf[:, reordering_indices]) - - bodies_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source - bodies_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source - - # Go through each body and check if relative transforms are same - for index in range(len(articulation_body_names)): - body_pos_b, body_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], bodies_pos_w_tf[:, index], bodies_quat_w_tf[:, index] - ) - - torch.testing.assert_close(bodies_pos_source_tf[:, index], body_pos_b) - torch.testing.assert_close(bodies_quat_source_tf[:, index], body_quat_b) - - def test_sensor_print(self): - """Test sensor print is working correctly.""" - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/.*", - ), - ], + # Frame transformer will be set after config creation (needs source_robot parameter) + frame_transformer: FrameTransformerCfg = None # type: ignore + + # Use multiple envs for env patterns, single env for direct paths + num_envs = 2 if path_prefix == "{ENV_REGEX_NS}" else 1 + env_spacing = 10.0 if path_prefix == "{ENV_REGEX_NS}" else 0.0 + + # Create scene config with appropriate prim paths + scene_cfg = MultiRobotSceneCfg(num_envs=num_envs, env_spacing=env_spacing, lazy_sensor_update=False) + scene_cfg.robot = ANYMAL_C_CFG.replace(prim_path=f"{path_prefix}/Robot") + scene_cfg.robot_1 = ANYMAL_C_CFG.replace( + prim_path=f"{path_prefix}/Robot_1", + init_state=ANYMAL_C_CFG.init_state.replace(pos=(2.0, 0.0, 0.6)), + ) + + # Frame transformer tracking same-named bodies from both robots + # Source frame is parametrized to test both Robot/base and Robot_1/base + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path=f"{path_prefix}/{source_robot}/base", + target_frames=[ + # Explicit frame names (recommended when bodies share the same leaf name) + FrameTransformerCfg.FrameCfg( + name="Robot_LF_SHANK", + prim_path=f"{path_prefix}/Robot/LF_SHANK", + ), + FrameTransformerCfg.FrameCfg( + name="Robot_1_LF_SHANK", + prim_path=f"{path_prefix}/Robot_1/LF_SHANK", + ), + # Implicit frame names (backward compatibility) + FrameTransformerCfg.FrameCfg( + prim_path=f"{path_prefix}/Robot/RF_SHANK", + ), + FrameTransformerCfg.FrameCfg( + prim_path=f"{path_prefix}/Robot_1/RF_SHANK", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + # Get target frame names + target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names + + # Verify explicit frame names are present + assert "Robot_LF_SHANK" in target_frame_names, f"Expected 'Robot_LF_SHANK', got {target_frame_names}" + assert "Robot_1_LF_SHANK" in target_frame_names, f"Expected 'Robot_1_LF_SHANK', got {target_frame_names}" + + # Without explicit names, both RF_SHANK frames default to same name "RF_SHANK" + # This results in duplicate frame names (expected behavior for backwards compatibility) + rf_shank_count = target_frame_names.count("RF_SHANK") + assert rf_shank_count == 2, f"Expected 2 'RF_SHANK' entries (name collision), got {rf_shank_count}" + + # Get indices for explicit named frames + robot_lf_idx = target_frame_names.index("Robot_LF_SHANK") + robot_1_lf_idx = target_frame_names.index("Robot_1_LF_SHANK") + + # Get indices for implicit named frames (both named "RF_SHANK") + rf_shank_indices = [i for i, name in enumerate(target_frame_names) if name == "RF_SHANK"] + assert len(rf_shank_indices) == 2, f"Expected 2 RF_SHANK indices, got {rf_shank_indices}" + + # Acquire ground truth body indices + robot_base_body_idx = scene.articulations["robot"].find_bodies("base")[0][0] + robot_1_base_body_idx = scene.articulations["robot_1"].find_bodies("base")[0][0] + robot_lf_shank_body_idx = scene.articulations["robot"].find_bodies("LF_SHANK")[0][0] + robot_1_lf_shank_body_idx = scene.articulations["robot_1"].find_bodies("LF_SHANK")[0][0] + robot_rf_shank_body_idx = scene.articulations["robot"].find_bodies("RF_SHANK")[0][0] + robot_1_rf_shank_body_idx = scene.articulations["robot_1"].find_bodies("RF_SHANK")[0][0] + + # Determine expected source frame based on parameter + expected_source_robot = "robot" if source_robot == "Robot" else "robot_1" + expected_source_base_body_idx = robot_base_body_idx if source_robot == "Robot" else robot_1_base_body_idx + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + + # Simulate physics + for count in range(20): + # Reset periodically + if count % 10 == 0: + # Reset robot + root_state = scene.articulations["robot"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_joint_state_to_sim( + scene.articulations["robot"].data.default_joint_pos, + scene.articulations["robot"].data.default_joint_vel, + ) + # Reset robot_1 + root_state_1 = scene.articulations["robot_1"].data.default_root_state.clone() + root_state_1[:, :3] += scene.env_origins + scene.articulations["robot_1"].write_root_pose_to_sim(root_state_1[:, :7]) + scene.articulations["robot_1"].write_root_velocity_to_sim(root_state_1[:, 7:]) + scene.articulations["robot_1"].write_joint_state_to_sim( + scene.articulations["robot_1"].data.default_joint_pos, + scene.articulations["robot_1"].data.default_joint_vel, + ) + scene.reset() + + # Write data to sim + scene.write_data_to_sim() + # Perform step + sim.step() + # Read data from sim + scene.update(sim_dt) + + # Get frame transformer data + frame_transformer_data = scene.sensors["frame_transformer"].data + source_pos_w = frame_transformer_data.source_pos_w + source_quat_w = frame_transformer_data.source_quat_w + target_pos_w = frame_transformer_data.target_pos_w + + # Get ground truth positions and orientations (after scene.update() so they're current) + robot_lf_pos_w = scene.articulations["robot"].data.body_pos_w[:, robot_lf_shank_body_idx] + robot_1_lf_pos_w = scene.articulations["robot_1"].data.body_pos_w[:, robot_1_lf_shank_body_idx] + robot_rf_pos_w = scene.articulations["robot"].data.body_pos_w[:, robot_rf_shank_body_idx] + robot_1_rf_pos_w = scene.articulations["robot_1"].data.body_pos_w[:, robot_1_rf_shank_body_idx] + + # Get expected source frame positions and orientations (after scene.update() so they're current) + expected_source_base_pos_w = scene.articulations[expected_source_robot].data.body_pos_w[ + :, expected_source_base_body_idx + ] + expected_source_base_quat_w = scene.articulations[expected_source_robot].data.body_quat_w[ + :, expected_source_base_body_idx + ] + + # TEST 1: Verify source frame is correctly resolved + # The source_pos_w should match the expected source robot's base world position + torch.testing.assert_close(source_pos_w, expected_source_base_pos_w, rtol=1e-5, atol=1e-5) + torch.testing.assert_close(source_quat_w, expected_source_base_quat_w, rtol=1e-5, atol=1e-5) + + # TEST 2: Explicit named frames (LF_SHANK) should have DIFFERENT world positions + lf_pos_difference = torch.norm(target_pos_w[:, robot_lf_idx] - target_pos_w[:, robot_1_lf_idx], dim=-1) + assert torch.all(lf_pos_difference > 1.0), ( + f"Robot_LF_SHANK and Robot_1_LF_SHANK should have different positions (got diff={lf_pos_difference}). " + "This indicates body name collision bug." ) - scene = InteractiveScene(scene_cfg) - # Play the simulator - self.sim.reset() - # print info - print(scene.sensors["frame_transformer"]) + # Verify explicit named frames match correct robot bodies + torch.testing.assert_close(target_pos_w[:, robot_lf_idx], robot_lf_pos_w) + torch.testing.assert_close(target_pos_w[:, robot_1_lf_idx], robot_1_lf_pos_w) + # TEST 3: Implicit named frames (RF_SHANK) should also have DIFFERENT world positions + # Even though they have the same frame name, internal body tracking uses full paths + rf_pos_difference = torch.norm( + target_pos_w[:, rf_shank_indices[0]] - target_pos_w[:, rf_shank_indices[1]], dim=-1 + ) + assert torch.all(rf_pos_difference > 1.0), ( + f"The two RF_SHANK frames should have different positions (got diff={rf_pos_difference}). " + "This indicates body name collision bug in internal body tracking." + ) + + # Verify implicit named frames match correct robot bodies + # Note: Order depends on internal processing, so we check both match one of the robots + rf_positions = [target_pos_w[:, rf_shank_indices[0]], target_pos_w[:, rf_shank_indices[1]]] -if __name__ == "__main__": - run_tests() + # Each tracked position should match one of the ground truth positions + for rf_pos in rf_positions: + matches_robot = torch.allclose(rf_pos, robot_rf_pos_w, atol=1e-5) + matches_robot_1 = torch.allclose(rf_pos, robot_1_rf_pos_w, atol=1e-5) + assert matches_robot or matches_robot_1, ( + f"RF_SHANK position {rf_pos} doesn't match either robot's RF_SHANK position" + ) diff --git a/source/isaaclab/test/sensors/test_imu.py b/source/isaaclab/test/sensors/test_imu.py index 71b740bef171..92c97f0c6d70 100644 --- a/source/isaaclab/test/sensors/test_imu.py +++ b/source/isaaclab/test/sensors/test_imu.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -14,10 +14,9 @@ """Rest everything follows.""" import pathlib -import torch -import unittest -import isaacsim.core.utils.stage as stage_utils +import pytest +import torch import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils @@ -25,7 +24,7 @@ from isaaclab.assets import ArticulationCfg, RigidObjectCfg from isaaclab.markers.config import GREEN_ARROW_X_MARKER_CFG, RED_ARROW_X_MARKER_CFG from isaaclab.scene import InteractiveScene, InteractiveSceneCfg -from isaaclab.sensors.imu import ImuCfg +from isaaclab.sensors.imu import Imu, ImuCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass @@ -33,7 +32,7 @@ # Pre-defined configs ## from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort: skip -from isaaclab.utils.assets import NUCLEUS_ASSET_ROOT_DIR # isort: skip +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # isort: skip # offset of imu_link from base_link on anymal_c POS_OFFSET = (0.2488, 0.00835, 0.04628) @@ -82,6 +81,7 @@ class MySceneCfg(InteractiveSceneCfg): # articulations - robot robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/robot") + # pendulum1 pendulum = ArticulationCfg( prim_path="{ENV_REGEX_NS}/pendulum", spawn=sim_utils.UrdfFileCfg( @@ -101,6 +101,27 @@ class MySceneCfg(InteractiveSceneCfg): "joint_1_act": ImplicitActuatorCfg(joint_names_expr=["joint_.*"], stiffness=0.0, damping=0.3), }, ) + # pendulum2 + pendulum2 = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/pendulum2", + spawn=sim_utils.UrdfFileCfg( + fix_base=True, + merge_fixed_joints=True, + make_instanceable=False, + asset_path=f"{pathlib.Path(__file__).parent.resolve()}/urdfs/simple_2_link.urdf", + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( + gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=None, damping=None) + ), + ), + init_state=ArticulationCfg.InitialStateCfg(), + actuators={ + "joint_1_act": ImplicitActuatorCfg(joint_names_expr=["joint_.*"], stiffness=0.0, damping=0.3), + }, + ) + # sensors - imu (filled inside unit test) imu_ball: ImuCfg = ImuCfg( prim_path="{ENV_REGEX_NS}/ball", @@ -122,7 +143,30 @@ class MySceneCfg(InteractiveSceneCfg): ), gravity_bias=(0.0, 0.0, 0.0), ) - + imu_robot_norb: ImuCfg = ImuCfg( + prim_path="{ENV_REGEX_NS}/robot/LF_HIP/LF_hip_fixed", + offset=ImuCfg.OffsetCfg( + pos=POS_OFFSET, + rot=ROT_OFFSET, + ), + gravity_bias=(0.0, 0.0, 0.0), + ) + imu_indirect_pendulum_link: ImuCfg = ImuCfg( + prim_path="{ENV_REGEX_NS}/pendulum2/link_1/imu_link", + debug_vis=not app_launcher._headless, + visualizer_cfg=RED_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Acceleration/imu_link"), + gravity_bias=(0.0, 0.0, 9.81), + ) + imu_indirect_pendulum_base: ImuCfg = ImuCfg( + prim_path="{ENV_REGEX_NS}/pendulum2/link_1", + offset=ImuCfg.OffsetCfg( + pos=PEND_POS_OFFSET, + rot=PEND_ROT_OFFSET, + ), + debug_vis=not app_launcher._headless, + visualizer_cfg=GREEN_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Acceleration/base"), + gravity_bias=(0.0, 0.0, 9.81), + ) imu_pendulum_imu_link: ImuCfg = ImuCfg( prim_path="{ENV_REGEX_NS}/pendulum/imu_link", debug_vis=not app_launcher._headless, @@ -144,398 +188,570 @@ def __post_init__(self): """Post initialization.""" # change position of the robot self.robot.init_state.pos = (0.0, 2.0, 1.0) - self.pendulum.init_state.pos = (-1.0, 1.0, 0.5) + self.pendulum.init_state.pos = (-2.0, 1.0, 0.5) + self.pendulum2.init_state.pos = (2.0, 1.0, 0.5) # change asset - self.robot.spawn.usd_path = f"{NUCLEUS_ASSET_ROOT_DIR}/Isaac/Robots/ANYbotics/anymal_c.usd" + self.robot.spawn.usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_c/anymal_c.usd" # change iterations self.robot.spawn.articulation_props.solver_position_iteration_count = 32 self.robot.spawn.articulation_props.solver_velocity_iteration_count = 32 -class TestImu(unittest.TestCase): - """Test for Imu sensor.""" - - def setUp(self): - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Load simulation context - sim_cfg = sim_utils.SimulationCfg(dt=0.001) - sim_cfg.physx.solver_type = 0 # 0: PGS, 1: TGS --> use PGS for more accurate results - self.sim = sim_utils.SimulationContext(sim_cfg) - # construct scene - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - self.scene = InteractiveScene(scene_cfg) - # Play the simulator - self.sim.reset() - - def tearDown(self): - """Stops simulator after each test.""" - # clear the stage - self.sim.clear_all_callbacks() - self.sim.clear_instance() - +@pytest.fixture +def setup_sim(): + """Create a simulation context and scene.""" + # Create a new stage + sim_utils.create_new_stage() + # Load simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.001) + sim_cfg.physx.solver_type = 0 # 0: PGS, 1: TGS --> use PGS for more accurate results + sim = sim_utils.SimulationContext(sim_cfg) + # construct scene + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + yield sim, scene + # Cleanup + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_constant_velocity(setup_sim): + """Test the Imu sensor with a constant velocity. + + Expected behavior is that the linear and angular are approx the same at every time step as in each step we set + the same velocity and therefore reset the physx buffers. """ - Tests - """ - - def test_constant_velocity(self): - """Test the Imu sensor with a constant velocity. - - Expected behavior is that the linear and angular are approx the same at every time step as in each step we set - the same velocity and therefore reset the physx buffers.""" - prev_lin_acc_ball = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device) - prev_ang_acc_ball = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device) - prev_lin_acc_cube = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device) - prev_ang_acc_cube = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device) - - for idx in range(200): - # set velocity - self.scene.rigid_objects["balls"].write_root_velocity_to_sim( - torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( - self.scene.num_envs, 1 - ) + sim, scene = setup_sim + prev_lin_acc_ball = torch.zeros((scene.num_envs, 3), dtype=torch.float32, device=scene.device) + prev_ang_acc_ball = torch.zeros((scene.num_envs, 3), dtype=torch.float32, device=scene.device) + prev_lin_acc_cube = torch.zeros((scene.num_envs, 3), dtype=torch.float32, device=scene.device) + prev_ang_acc_cube = torch.zeros((scene.num_envs, 3), dtype=torch.float32, device=scene.device) + + for idx in range(200): + # set velocity + scene.rigid_objects["balls"].write_root_velocity_to_sim( + torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=scene.device).repeat( + scene.num_envs, 1 ) - self.scene.rigid_objects["cube"].write_root_velocity_to_sim( - torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( - self.scene.num_envs, 1 - ) + ) + scene.rigid_objects["cube"].write_root_velocity_to_sim( + torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=scene.device).repeat( + scene.num_envs, 1 ) - # write data to sim - self.scene.write_data_to_sim() - - # perform step - self.sim.step() - # read data from sim - self.scene.update(self.sim.get_physics_dt()) - - if idx > 1: - # check the imu accelerations - torch.testing.assert_close( - self.scene.sensors["imu_ball"].data.lin_acc_b, - prev_lin_acc_ball, - rtol=1e-3, - atol=1e-3, - ) - torch.testing.assert_close( - self.scene.sensors["imu_ball"].data.ang_acc_b, - prev_ang_acc_ball, - rtol=1e-3, - atol=1e-3, - ) - - torch.testing.assert_close( - self.scene.sensors["imu_cube"].data.lin_acc_b, - prev_lin_acc_cube, - rtol=1e-3, - atol=1e-3, - ) - torch.testing.assert_close( - self.scene.sensors["imu_cube"].data.ang_acc_b, - prev_ang_acc_cube, - rtol=1e-3, - atol=1e-3, - ) - - # check the imu velocities - # NOTE: the expected lin_vel_b is the same as the set velocity, as write_root_velocity_to_sim is - # setting v_0 (initial velocity) and then a calculation step of v_i = v_0 + a*dt. Consequently, - # the data.lin_vel_b is returning approx. v_i. - torch.testing.assert_close( - self.scene.sensors["imu_ball"].data.lin_vel_b, - torch.tensor( - [[1.0, 0.0, -self.scene.physics_dt * 9.81]], dtype=torch.float32, device=self.scene.device - ).repeat(self.scene.num_envs, 1), - rtol=1e-4, - atol=1e-4, - ) - torch.testing.assert_close( - self.scene.sensors["imu_cube"].data.lin_vel_b, - torch.tensor( - [[1.0, 0.0, -self.scene.physics_dt * 9.81]], dtype=torch.float32, device=self.scene.device - ).repeat(self.scene.num_envs, 1), - rtol=1e-4, - atol=1e-4, - ) - - # update previous values - prev_lin_acc_ball = self.scene.sensors["imu_ball"].data.lin_acc_b.clone() - prev_ang_acc_ball = self.scene.sensors["imu_ball"].data.ang_acc_b.clone() - prev_lin_acc_cube = self.scene.sensors["imu_cube"].data.lin_acc_b.clone() - prev_ang_acc_cube = self.scene.sensors["imu_cube"].data.ang_acc_b.clone() - - def test_constant_acceleration(self): - """Test the Imu sensor with a constant acceleration.""" - for idx in range(100): - # set acceleration - self.scene.rigid_objects["balls"].write_root_velocity_to_sim( - torch.tensor([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( - self.scene.num_envs, 1 - ) - * (idx + 1) - ) - # write data to sim - self.scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - self.scene.update(self.sim.get_physics_dt()) - - # skip first step where initial velocity is zero - if idx < 1: - continue - - # check the imu data - torch.testing.assert_close( - self.scene.sensors["imu_ball"].data.lin_acc_b, - math_utils.quat_rotate_inverse( - self.scene.rigid_objects["balls"].data.root_quat_w, - torch.tensor([[0.1, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( - self.scene.num_envs, 1 - ) - / self.sim.get_physics_dt(), - ), - rtol=1e-4, - atol=1e-4, - ) - - # check the angular velocity - torch.testing.assert_close( - self.scene.sensors["imu_ball"].data.ang_vel_b, - self.scene.rigid_objects["balls"].data.root_ang_vel_b, - rtol=1e-4, - atol=1e-4, - ) - - def test_single_dof_pendulum(self): - """Test imu against analytical pendulum problem.""" - - # pendulum length - pend_length = PEND_POS_OFFSET[0] - - # should achieve same results between the two imu sensors on the robot - for idx in range(500): - - # write data to sim - self.scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - self.scene.update(self.sim.get_physics_dt()) - - # get pendulum joint state - joint_pos = self.scene.articulations["pendulum"].data.joint_pos - joint_vel = self.scene.articulations["pendulum"].data.joint_vel - joint_acc = self.scene.articulations["pendulum"].data.joint_acc - - # IMU and base data - imu_data = self.scene.sensors["imu_pendulum_imu_link"].data - base_data = self.scene.sensors["imu_pendulum_base"].data - - # extract imu_link imu_sensor dynamics - lin_vel_w_imu_link = math_utils.quat_rotate(imu_data.quat_w, imu_data.lin_vel_b) - lin_acc_w_imu_link = math_utils.quat_rotate(imu_data.quat_w, imu_data.lin_acc_b) - - # calculate the joint dynamics from the imu_sensor (y axis of imu_link is parallel to joint axis of pendulum) - joint_vel_imu = math_utils.quat_rotate(imu_data.quat_w, imu_data.ang_vel_b)[..., 1].unsqueeze(-1) - joint_acc_imu = math_utils.quat_rotate(imu_data.quat_w, imu_data.ang_acc_b)[..., 1].unsqueeze(-1) - - # calculate analytical solution - vx = -joint_vel * pend_length * torch.sin(joint_pos) - vy = torch.zeros(2, 1, device=self.scene.device) - vz = -joint_vel * pend_length * torch.cos(joint_pos) - gt_linear_vel_w = torch.cat([vx, vy, vz], dim=-1) - - ax = -joint_acc * pend_length * torch.sin(joint_pos) - joint_vel**2 * pend_length * torch.cos(joint_pos) - ay = torch.zeros(2, 1, device=self.scene.device) - az = ( - -joint_acc * pend_length * torch.cos(joint_pos) - + joint_vel**2 * pend_length * torch.sin(joint_pos) - + 9.81 - ) - gt_linear_acc_w = torch.cat([ax, ay, az], dim=-1) + ) + # write data to sim + scene.write_data_to_sim() - # skip first step where initial velocity is zero - if idx < 2: - continue + # perform step + sim.step() + # read data from sim + scene.update(sim.get_physics_dt()) - # compare imu angular velocity with joint velocity + if idx > 1: + # check the imu accelerations torch.testing.assert_close( - joint_vel, - joint_vel_imu, - rtol=1e-1, + scene.sensors["imu_ball"].data.lin_acc_b, + prev_lin_acc_ball, + rtol=1e-3, atol=1e-3, ) - # compare imu angular acceleration with joint acceleration torch.testing.assert_close( - joint_acc, - joint_acc_imu, - rtol=1e-1, + scene.sensors["imu_ball"].data.ang_acc_b, + prev_ang_acc_ball, + rtol=1e-3, atol=1e-3, ) - # compare imu linear velocituy with simple pendulum calculation - torch.testing.assert_close( - gt_linear_vel_w, - lin_vel_w_imu_link, - rtol=1e-1, - atol=1e-3, - ) - # compare imu linear acceleration with simple pendulum calculation - torch.testing.assert_close( - gt_linear_acc_w, - lin_acc_w_imu_link, - rtol=1e-1, - atol=1e0, - ) - - # check the position between offset and imu definition - torch.testing.assert_close( - base_data.pos_w, - imu_data.pos_w, - rtol=1e-5, - atol=1e-5, - ) - # check the orientation between offset and imu definition - torch.testing.assert_close( - base_data.quat_w, - imu_data.quat_w, - rtol=1e-4, - atol=1e-4, - ) - - # check the angular velocities of the imus between offset and imu definition - torch.testing.assert_close( - base_data.ang_vel_b, - imu_data.ang_vel_b, - rtol=1e-4, - atol=1e-4, - ) - # check the angular acceleration of the imus between offset and imu definition torch.testing.assert_close( - base_data.ang_acc_b, - imu_data.ang_acc_b, - rtol=1e-4, - atol=1e-4, - ) - - # check the linear velocity of the imus between offset and imu definition - torch.testing.assert_close( - base_data.lin_vel_b, - imu_data.lin_vel_b, - rtol=1e-2, - atol=5e-3, - ) - - # check the linear acceleration of the imus between offset and imu definition - torch.testing.assert_close( - base_data.lin_acc_b, - imu_data.lin_acc_b, - rtol=1e-1, - atol=1e-1, - ) - - def test_offset_calculation(self): - """Test offset configuration argument.""" - # should achieve same results between the two imu sensors on the robot - for idx in range(500): - # set acceleration - self.scene.articulations["robot"].write_root_velocity_to_sim( - torch.tensor([[0.05, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( - self.scene.num_envs, 1 - ) - * (idx + 1) - ) - # write data to sim - self.scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - self.scene.update(self.sim.get_physics_dt()) - - # skip first step where initial velocity is zero - if idx < 1: - continue - - # check the accelerations - torch.testing.assert_close( - self.scene.sensors["imu_robot_base"].data.lin_acc_b, - self.scene.sensors["imu_robot_imu_link"].data.lin_acc_b, - rtol=1e-4, - atol=1e-4, + scene.sensors["imu_cube"].data.lin_acc_b, + prev_lin_acc_cube, + rtol=1e-3, + atol=1e-3, ) torch.testing.assert_close( - self.scene.sensors["imu_robot_base"].data.ang_acc_b, - self.scene.sensors["imu_robot_imu_link"].data.ang_acc_b, - rtol=1e-4, - atol=1e-4, + scene.sensors["imu_cube"].data.ang_acc_b, + prev_ang_acc_cube, + rtol=1e-3, + atol=1e-3, ) - # check the velocities + # check the imu velocities + # NOTE: the expected lin_vel_b is the same as the set velocity, as write_root_velocity_to_sim is + # setting v_0 (initial velocity) and then a calculation step of v_i = v_0 + a*dt. Consequently, + # the data.lin_vel_b is returning approx. v_i. torch.testing.assert_close( - self.scene.sensors["imu_robot_base"].data.ang_vel_b, - self.scene.sensors["imu_robot_imu_link"].data.ang_vel_b, + scene.sensors["imu_ball"].data.lin_vel_b, + torch.tensor([[1.0, 0.0, -scene.physics_dt * 9.81]], dtype=torch.float32, device=scene.device).repeat( + scene.num_envs, 1 + ), rtol=1e-4, atol=1e-4, ) torch.testing.assert_close( - self.scene.sensors["imu_robot_base"].data.lin_vel_b, - self.scene.sensors["imu_robot_imu_link"].data.lin_vel_b, + scene.sensors["imu_cube"].data.lin_vel_b, + torch.tensor([[1.0, 0.0, -scene.physics_dt * 9.81]], dtype=torch.float32, device=scene.device).repeat( + scene.num_envs, 1 + ), rtol=1e-4, atol=1e-4, ) - # check the orientation - torch.testing.assert_close( - self.scene.sensors["imu_robot_base"].data.quat_w, - self.scene.sensors["imu_robot_imu_link"].data.quat_w, - rtol=1e-4, - atol=1e-4, + # update previous values + prev_lin_acc_ball = scene.sensors["imu_ball"].data.lin_acc_b.clone() + prev_ang_acc_ball = scene.sensors["imu_ball"].data.ang_acc_b.clone() + prev_lin_acc_cube = scene.sensors["imu_cube"].data.lin_acc_b.clone() + prev_ang_acc_cube = scene.sensors["imu_cube"].data.ang_acc_b.clone() + + +@pytest.mark.isaacsim_ci +def test_constant_acceleration(setup_sim): + """Test the Imu sensor with a constant acceleration.""" + sim, scene = setup_sim + for idx in range(100): + # set acceleration + scene.rigid_objects["balls"].write_root_velocity_to_sim( + torch.tensor([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=scene.device).repeat( + scene.num_envs, 1 ) - # check the position - torch.testing.assert_close( - self.scene.sensors["imu_robot_base"].data.pos_w, - self.scene.sensors["imu_robot_imu_link"].data.pos_w, - rtol=1e-4, - atol=1e-4, - ) - - def test_env_ids_propogation(self): - """Test that env_ids argument propagates through update and reset methods""" - self.scene.reset() - - for idx in range(10): - # set acceleration - self.scene.articulations["robot"].write_root_velocity_to_sim( - torch.tensor([[0.5, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( - self.scene.num_envs, 1 - ) - * (idx + 1) + * (idx + 1) + ) + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim.get_physics_dt()) + + # skip first step where initial velocity is zero + if idx < 1: + continue + + # check the imu data + torch.testing.assert_close( + scene.sensors["imu_ball"].data.lin_acc_b, + math_utils.quat_apply_inverse( + scene.rigid_objects["balls"].data.root_quat_w, + torch.tensor([[0.1, 0.0, 0.0]], dtype=torch.float32, device=scene.device).repeat(scene.num_envs, 1) + / sim.get_physics_dt(), + ), + rtol=1e-4, + atol=1e-4, + ) + + # check the angular velocity + torch.testing.assert_close( + scene.sensors["imu_ball"].data.ang_vel_b, + scene.rigid_objects["balls"].data.root_ang_vel_b, + rtol=1e-4, + atol=1e-4, + ) + + +@pytest.mark.isaacsim_ci +def test_single_dof_pendulum(setup_sim): + """Test imu against analytical pendulum problem.""" + sim, scene = setup_sim + # pendulum length + pend_length = PEND_POS_OFFSET[0] + + # should achieve same results between the two imu sensors on the robot + for idx in range(500): + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim.get_physics_dt()) + + # get pendulum joint state + joint_pos = scene.articulations["pendulum"].data.joint_pos + joint_vel = scene.articulations["pendulum"].data.joint_vel + joint_acc = scene.articulations["pendulum"].data.joint_acc + + # IMU and base data + imu_data = scene.sensors["imu_pendulum_imu_link"].data + base_data = scene.sensors["imu_pendulum_base"].data + + # extract imu_link imu_sensor dynamics + lin_vel_w_imu_link = math_utils.quat_apply(imu_data.quat_w, imu_data.lin_vel_b) + lin_acc_w_imu_link = math_utils.quat_apply(imu_data.quat_w, imu_data.lin_acc_b) + + # calculate the joint dynamics from the imu_sensor (y axis of imu_link is parallel to joint axis of pendulum) + joint_vel_imu = math_utils.quat_apply(imu_data.quat_w, imu_data.ang_vel_b)[..., 1].unsqueeze(-1) + joint_acc_imu = math_utils.quat_apply(imu_data.quat_w, imu_data.ang_acc_b)[..., 1].unsqueeze(-1) + + # calculate analytical solution + vx = -joint_vel * pend_length * torch.sin(joint_pos) + vy = torch.zeros(2, 1, device=scene.device) + vz = -joint_vel * pend_length * torch.cos(joint_pos) + gt_linear_vel_w = torch.cat([vx, vy, vz], dim=-1) + + ax = -joint_acc * pend_length * torch.sin(joint_pos) - joint_vel**2 * pend_length * torch.cos(joint_pos) + ay = torch.zeros(2, 1, device=scene.device) + az = -joint_acc * pend_length * torch.cos(joint_pos) + joint_vel**2 * pend_length * torch.sin(joint_pos) + 9.81 + gt_linear_acc_w = torch.cat([ax, ay, az], dim=-1) + + # skip first step where initial velocity is zero + if idx < 2: + continue + + # compare imu projected gravity + gravity_dir_w = torch.tensor((0.0, 0.0, -1.0), device=scene.device).repeat(2, 1) + gravity_dir_b = math_utils.quat_apply_inverse(imu_data.quat_w, gravity_dir_w) + torch.testing.assert_close( + imu_data.projected_gravity_b, + gravity_dir_b, + ) + + # compare imu angular velocity with joint velocity + torch.testing.assert_close( + joint_vel, + joint_vel_imu, + rtol=1e-1, + atol=1e-3, + ) + # compare imu angular acceleration with joint acceleration + torch.testing.assert_close( + joint_acc, + joint_acc_imu, + rtol=1e-1, + atol=1e-3, + ) + # compare imu linear velocity with simple pendulum calculation + torch.testing.assert_close( + gt_linear_vel_w, + lin_vel_w_imu_link, + rtol=1e-1, + atol=1e-3, + ) + # compare imu linear acceleration with simple pendulum calculation + torch.testing.assert_close( + gt_linear_acc_w, + lin_acc_w_imu_link, + rtol=1e-1, + atol=1e0, + ) + + # check the position between offset and imu definition + torch.testing.assert_close( + base_data.pos_w, + imu_data.pos_w, + rtol=1e-5, + atol=1e-5, + ) + + # check the orientation between offset and imu definition + torch.testing.assert_close( + base_data.quat_w, + imu_data.quat_w, + rtol=1e-4, + atol=1e-4, + ) + + # check the angular velocities of the imus between offset and imu definition + torch.testing.assert_close( + base_data.ang_vel_b, + imu_data.ang_vel_b, + rtol=1e-4, + atol=1e-4, + ) + # check the angular acceleration of the imus between offset and imu definition + torch.testing.assert_close( + base_data.ang_acc_b, + imu_data.ang_acc_b, + rtol=1e-4, + atol=1e-4, + ) + + # check the linear velocity of the imus between offset and imu definition + torch.testing.assert_close( + base_data.lin_vel_b, + imu_data.lin_vel_b, + rtol=1e-2, + atol=5e-3, + ) + + # check the linear acceleration of the imus between offset and imu definition + torch.testing.assert_close( + base_data.lin_acc_b, + imu_data.lin_acc_b, + rtol=1e-1, + atol=1e-1, + ) + + +@pytest.mark.isaacsim_ci +def test_indirect_attachment(setup_sim): + """Test attaching the imu through an xForm primitive configuration argument.""" + sim, scene = setup_sim + # pendulum length + pend_length = PEND_POS_OFFSET[0] + + # should achieve same results between the two imu sensors on the robot + for idx in range(500): + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim.get_physics_dt()) + + # get pendulum joint state + joint_pos = scene.articulations["pendulum2"].data.joint_pos + joint_vel = scene.articulations["pendulum2"].data.joint_vel + joint_acc = scene.articulations["pendulum2"].data.joint_acc + + imu = scene.sensors["imu_indirect_pendulum_link"] + imu_base = scene.sensors["imu_indirect_pendulum_base"] + + torch.testing.assert_close( + imu._offset_pos_b, + imu_base._offset_pos_b, + ) + torch.testing.assert_close(imu._offset_quat_b, imu_base._offset_quat_b, rtol=1e-4, atol=1e-4) + + # IMU and base data + imu_data = scene.sensors["imu_indirect_pendulum_link"].data + base_data = scene.sensors["imu_indirect_pendulum_base"].data + # extract imu_link imu_sensor dynamics + lin_vel_w_imu_link = math_utils.quat_apply(imu_data.quat_w, imu_data.lin_vel_b) + lin_acc_w_imu_link = math_utils.quat_apply(imu_data.quat_w, imu_data.lin_acc_b) + + # calculate the joint dynamics from the imu_sensor (y axis of imu_link is parallel to joint axis of pendulum) + joint_vel_imu = math_utils.quat_apply(imu_data.quat_w, imu_data.ang_vel_b)[..., 1].unsqueeze(-1) + joint_acc_imu = math_utils.quat_apply(imu_data.quat_w, imu_data.ang_acc_b)[..., 1].unsqueeze(-1) + + # calculate analytical solution + vx = -joint_vel * pend_length * torch.sin(joint_pos) + vy = torch.zeros(2, 1, device=scene.device) + vz = -joint_vel * pend_length * torch.cos(joint_pos) + gt_linear_vel_w = torch.cat([vx, vy, vz], dim=-1) + + ax = -joint_acc * pend_length * torch.sin(joint_pos) - joint_vel**2 * pend_length * torch.cos(joint_pos) + ay = torch.zeros(2, 1, device=scene.device) + az = -joint_acc * pend_length * torch.cos(joint_pos) + joint_vel**2 * pend_length * torch.sin(joint_pos) + 9.81 + gt_linear_acc_w = torch.cat([ax, ay, az], dim=-1) + + # skip first step where initial velocity is zero + if idx < 2: + continue + + # compare imu projected gravity + gravity_dir_w = torch.tensor((0.0, 0.0, -1.0), device=scene.device).repeat(2, 1) + gravity_dir_b = math_utils.quat_apply_inverse(imu_data.quat_w, gravity_dir_w) + torch.testing.assert_close( + imu_data.projected_gravity_b, + gravity_dir_b, + ) + + # compare imu angular velocity with joint velocity + torch.testing.assert_close( + joint_vel, + joint_vel_imu, + rtol=1e-1, + atol=1e-3, + ) + # compare imu angular acceleration with joint acceleration + torch.testing.assert_close( + joint_acc, + joint_acc_imu, + rtol=1e-1, + atol=1e-3, + ) + # compare imu linear velocity with simple pendulum calculation + torch.testing.assert_close( + gt_linear_vel_w, + lin_vel_w_imu_link, + rtol=1e-1, + atol=1e-3, + ) + # compare imu linear acceleration with simple pendulum calculation + torch.testing.assert_close( + gt_linear_acc_w, + lin_acc_w_imu_link, + rtol=1e-1, + atol=1e0, + ) + + # check the position between offset and imu definition + torch.testing.assert_close( + base_data.pos_w, + imu_data.pos_w, + rtol=1e-5, + atol=1e-5, + ) + + # check the orientation between offset and imu definition + torch.testing.assert_close( + base_data.quat_w, + imu_data.quat_w, + rtol=1e-4, + atol=1e-4, + ) + + # check the angular velocities of the imus between offset and imu definition + torch.testing.assert_close( + base_data.ang_vel_b, + imu_data.ang_vel_b, + rtol=1e-4, + atol=1e-4, + ) + # check the angular acceleration of the imus between offset and imu definition + torch.testing.assert_close( + base_data.ang_acc_b, + imu_data.ang_acc_b, + rtol=1e-4, + atol=1e-4, + ) + + # check the linear velocity of the imus between offset and imu definition + torch.testing.assert_close( + base_data.lin_vel_b, + imu_data.lin_vel_b, + rtol=1e-2, + atol=5e-3, + ) + + # check the linear acceleration of the imus between offset and imu definition + torch.testing.assert_close( + base_data.lin_acc_b, + imu_data.lin_acc_b, + rtol=1e-1, + atol=1e-1, + ) + + +@pytest.mark.isaacsim_ci +def test_offset_calculation(setup_sim): + """Test offset configuration argument.""" + sim, scene = setup_sim + + # should achieve same results between the two imu sensors on the robot + for idx in range(500): + # set acceleration + scene.articulations["robot"].write_root_velocity_to_sim( + torch.tensor([[0.05, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=scene.device).repeat( + scene.num_envs, 1 ) - # write data to sim - self.scene.write_data_to_sim() - # perform step - self.sim.step() - # read data from sim - self.scene.update(self.sim.get_physics_dt()) - - # reset scene for env 1 - self.scene.reset(env_ids=[1]) + * (idx + 1) + ) + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() # read data from sim - self.scene.update(self.sim.get_physics_dt()) + scene.update(sim.get_physics_dt()) + + # skip first step where initial velocity is zero + if idx < 1: + continue + + # check the accelerations + torch.testing.assert_close( + scene.sensors["imu_robot_base"].data.lin_acc_b, + scene.sensors["imu_robot_imu_link"].data.lin_acc_b, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + scene.sensors["imu_robot_base"].data.ang_acc_b, + scene.sensors["imu_robot_imu_link"].data.ang_acc_b, + rtol=1e-4, + atol=1e-4, + ) + + # check the velocities + torch.testing.assert_close( + scene.sensors["imu_robot_base"].data.ang_vel_b, + scene.sensors["imu_robot_imu_link"].data.ang_vel_b, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + scene.sensors["imu_robot_base"].data.lin_vel_b, + scene.sensors["imu_robot_imu_link"].data.lin_vel_b, + rtol=1e-4, + atol=1e-4, + ) + + # check the orientation + torch.testing.assert_close( + scene.sensors["imu_robot_base"].data.quat_w, + scene.sensors["imu_robot_imu_link"].data.quat_w, + rtol=1e-4, + atol=1e-4, + ) + # check the position + torch.testing.assert_close( + scene.sensors["imu_robot_base"].data.pos_w, + scene.sensors["imu_robot_imu_link"].data.pos_w, + rtol=1e-4, + atol=1e-4, + ) + # check the projected gravity + torch.testing.assert_close( + scene.sensors["imu_robot_base"].data.projected_gravity_b, + scene.sensors["imu_robot_imu_link"].data.projected_gravity_b, + rtol=1e-4, + atol=1e-4, + ) + + +@pytest.mark.isaacsim_ci +def test_attachment_validity(setup_sim): + """Test invalid imu attachment. An imu cannot be attached directly to the world. It must be somehow attached to + something implementing physics.""" + sim, scene = setup_sim + imu_world_cfg = ImuCfg( + prim_path="/World/envs/env_0", + gravity_bias=(0.0, 0.0, 0.0), + ) + with pytest.raises(RuntimeError) as exc_info: + imu_world = Imu(imu_world_cfg) + imu_world._initialize_impl() + assert exc_info.type is RuntimeError and "find a rigid body ancestor prim" in str(exc_info.value) + + +@pytest.mark.isaacsim_ci +def test_env_ids_propagation(setup_sim): + """Test that env_ids argument propagates through update and reset methods""" + sim, scene = setup_sim + scene.reset() + + for idx in range(10): + # set acceleration + scene.articulations["robot"].write_root_velocity_to_sim( + torch.tensor([[0.5, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=scene.device).repeat( + scene.num_envs, 1 + ) + * (idx + 1) + ) + # write data to sim + scene.write_data_to_sim() # perform step - self.sim.step() + sim.step() # read data from sim - self.scene.update(self.sim.get_physics_dt()) - - def test_sensor_print(self): - """Test sensor print is working correctly.""" - # Create sensor - sensor = self.scene.sensors["imu_ball"] - # print info - print(sensor) - - -if __name__ == "__main__": - run_tests() + scene.update(sim.get_physics_dt()) + + # reset scene for env 1 + scene.reset(env_ids=[1]) + # read data from sim + scene.update(sim.get_physics_dt()) + # perform step + sim.step() + # read data from sim + scene.update(sim.get_physics_dt()) + + +@pytest.mark.isaacsim_ci +def test_sensor_print(setup_sim): + """Test sensor print is working correctly.""" + sim, scene = setup_sim + # Create sensor + sensor = scene.sensors["imu_ball"] + # print info + print(sensor) diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster.py new file mode 100644 index 000000000000..c27b25b53b79 --- /dev/null +++ b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster.py @@ -0,0 +1,250 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +from isaaclab.app import AppLauncher + +# launch omniverse app. Used for warp. +app_launcher = AppLauncher(headless=True) + +import numpy as np +import pytest +import torch +import trimesh +import warp as wp + +from isaaclab.utils.math import matrix_from_quat, quat_from_euler_xyz, random_orientation +from isaaclab.utils.warp.ops import convert_to_warp_mesh, raycast_dynamic_meshes, raycast_single_mesh + + +@pytest.fixture(scope="module") +def device(): + return "cuda" if torch.cuda.is_available() else "cpu" + + +@pytest.fixture +def rays(device): + ray_starts = torch.tensor([[0, -0.35, -5], [0.25, 0.35, -5]], dtype=torch.float32, device=device).unsqueeze(0) + ray_directions = torch.tensor([[0, 0, 1], [0, 0, 1]], dtype=torch.float32, device=device).unsqueeze(0) + expected_ray_hits = torch.tensor( + [[0, -0.35, -0.5], [0.25, 0.35, -0.5]], dtype=torch.float32, device=device + ).unsqueeze(0) + return ray_starts, ray_directions, expected_ray_hits + + +@pytest.fixture +def trimesh_box(): + return trimesh.creation.box([2, 2, 1]) + + +@pytest.fixture +def single_mesh(trimesh_box, device): + wp_mesh = convert_to_warp_mesh(trimesh_box.vertices, trimesh_box.faces, device) + return wp_mesh, wp_mesh.id + + +def test_raycast_multi_cubes(device, trimesh_box, rays): + """Test raycasting against two cubes.""" + ray_starts, ray_directions, _ = rays + + trimesh_1 = trimesh_box.copy() + wp_mesh_1 = convert_to_warp_mesh(trimesh_1.vertices, trimesh_1.faces, device) + + translation = np.eye(4) + translation[:3, 3] = [0, 2, 0] + trimesh_2 = trimesh_box.copy().apply_transform(translation) + wp_mesh_2 = convert_to_warp_mesh(trimesh_2.vertices, trimesh_2.faces, device) + + # get mesh id array + mesh_ids_wp = wp.array2d([[wp_mesh_1.id, wp_mesh_2.id]], dtype=wp.uint64, device=device) + + # Static positions (no transforms passed) + ray_start = torch.tensor([[0, 0, -5], [0, 2.5, -5]], dtype=torch.float32, device=device).unsqueeze(0) + ray_hits, ray_distance, ray_normal, ray_face_id, mesh_ids = raycast_dynamic_meshes( + ray_start, + ray_directions, + mesh_ids_wp, + return_distance=True, + return_normal=True, + return_face_id=True, + return_mesh_id=True, + ) + + torch.testing.assert_close( + ray_hits, torch.tensor([[[0, 0, -0.5], [0, 2.5, -0.5]]], dtype=torch.float32, device=device) + ) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], dtype=torch.float32, device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], dtype=torch.float32, device=device)) + assert torch.equal(mesh_ids, torch.tensor([[0, 1]], dtype=torch.int32, device=device)) + + # Dynamic positions/orientations + ray_start = torch.tensor([[0, 0, -5], [0, 4.5, -5]], dtype=torch.float32, device=device).unsqueeze(0) + ray_hits, ray_distance, ray_normal, ray_face_id, mesh_ids = raycast_dynamic_meshes( + ray_start, + ray_directions, + mesh_ids_wp, + return_distance=True, + return_normal=True, + return_face_id=True, + mesh_positions_w=torch.tensor([[[0, 0, 0], [0, 2, 0]]], dtype=torch.float32, device=device), + mesh_orientations_w=torch.tensor([[[1, 0, 0, 0], [1, 0, 0, 0]]], dtype=torch.float32, device=device), + return_mesh_id=True, + ) + + torch.testing.assert_close( + ray_hits, torch.tensor([[[0, 0, -0.5], [0, 4.5, -0.5]]], dtype=torch.float32, device=device) + ) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], dtype=torch.float32, device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], dtype=torch.float32, device=device)) + assert torch.equal(mesh_ids, torch.tensor([[0, 1]], dtype=torch.int32, device=device)) + + +def test_raycast_single_cube(device, single_mesh, rays): + """Test raycasting against a single cube.""" + ray_starts, ray_directions, expected_ray_hits = rays + _, single_mesh_id = single_mesh + + ray_hits, ray_distance, ray_normal, ray_face_id = raycast_single_mesh( + ray_starts, + ray_directions, + single_mesh_id, + return_distance=True, + return_normal=True, + return_face_id=True, + ) + torch.testing.assert_close(ray_hits, expected_ray_hits) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], dtype=torch.float32, device=device)) + torch.testing.assert_close( + ray_normal, + torch.tensor([[[0, 0, -1], [0, 0, -1]]], dtype=torch.float32, device=device), + ) + torch.testing.assert_close(ray_face_id, torch.tensor([[3, 8]], dtype=torch.int32, device=device)) + + # check multiple meshes implementation + ray_hits, ray_distance, ray_normal, ray_face_id, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + ) + torch.testing.assert_close(ray_hits, expected_ray_hits) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], dtype=torch.float32, device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], dtype=torch.float32, device=device)) + torch.testing.assert_close(ray_face_id, torch.tensor([[3, 8]], dtype=torch.int32, device=device)) + + +@pytest.mark.parametrize("num_samples", [10]) +def test_raycast_moving_cube(device, single_mesh, rays, num_samples): + r"""Test raycasting against a single cube with different distances. + |-------------| + |\ | + | \ | + | \ 8 | + | \ | + | \ x_1 | + | \ | + | \ | + | \ | + | \ | + | \ | + | 3 x_2 \ | + | \ | + | \| + |-------------| + + """ + ray_starts, ray_directions, expected_ray_hits = rays + _, single_mesh_id = single_mesh + + # move the cube along the z axis + for distance in torch.linspace(0, 1, num_samples, device=device): + ray_hits, ray_distance, ray_normal, ray_face_id, mesh_id = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + return_mesh_id=True, + mesh_positions_w=torch.tensor([[0, 0, distance]], dtype=torch.float32, device=device), + ) + torch.testing.assert_close( + ray_hits, + expected_ray_hits + + torch.tensor([[0, 0, distance], [0, 0, distance]], dtype=torch.float32, device=device).unsqueeze(0), + ) + torch.testing.assert_close( + ray_distance, distance + torch.tensor([[4.5, 4.5]], dtype=torch.float32, device=device) + ) + torch.testing.assert_close( + ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], dtype=torch.float32, device=device) + ) + torch.testing.assert_close(ray_face_id, torch.tensor([[3, 8]], dtype=torch.int32, device=device)) + + +def test_raycast_rotated_cube(device, single_mesh, rays): + """Test raycasting against a single cube with different 90deg. orientations.""" + ray_starts, ray_directions, expected_ray_hits = rays + _, single_mesh_id = single_mesh + + cube_rotation = quat_from_euler_xyz(torch.tensor([0.0]), torch.tensor([0.0]), torch.tensor([np.pi])).to(device) + ray_hits, ray_distance, ray_normal, ray_face_id, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + mesh_orientations_w=cube_rotation.unsqueeze(0), + ) + torch.testing.assert_close(ray_hits, expected_ray_hits) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], dtype=torch.float32, device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], dtype=torch.float32, device=device)) + # Make sure the face ids are correct. The cube is rotated by 90deg. so the face ids are different. + torch.testing.assert_close(ray_face_id, torch.tensor([[8, 3]], dtype=torch.int32, device=device)) + + +@pytest.mark.parametrize("num_random", [10]) +def test_raycast_random_cube(device, trimesh_box, single_mesh, rays, num_random): + """Test raycasting against a single cube with random poses.""" + ray_starts, ray_directions, _ = rays + _, single_mesh_id = single_mesh + + for orientation in random_orientation(num_random, device): + pos = torch.tensor([[0, 0, torch.rand(1)]], dtype=torch.float32, device=device) + tf_hom = np.eye(4) + tf_hom[:3, :3] = matrix_from_quat(orientation).cpu().numpy() + tf_hom[:3, 3] = pos.cpu().numpy() + tf_mesh = trimesh_box.copy().apply_transform(tf_hom) + + # get raycast for transformed, static mesh + wp_mesh = convert_to_warp_mesh(tf_mesh.vertices, tf_mesh.faces, device) + ray_hits, ray_distance, ray_normal, ray_face_id, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[wp_mesh.id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + ) + # get raycast for modified mesh + ray_hits_m, ray_distance_m, ray_normal_m, ray_face_id_m, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + mesh_positions_w=pos, + mesh_orientations_w=orientation.view(1, 1, -1), + ) + torch.testing.assert_close(ray_hits, ray_hits_m) + torch.testing.assert_close(ray_distance, ray_distance_m) + torch.testing.assert_close(ray_normal, ray_normal_m) + torch.testing.assert_close(ray_face_id, ray_face_id_m) diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py new file mode 100644 index 000000000000..6e30a5fcdc98 --- /dev/null +++ b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py @@ -0,0 +1,862 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +"""Rest everything follows.""" + +import copy +import os + +import numpy as np +import pytest +import torch + +import omni.replicator.core as rep +from pxr import Gf + +import isaaclab.sim as sim_utils +from isaaclab.sensors.camera import Camera, CameraCfg +from isaaclab.sensors.ray_caster import MultiMeshRayCasterCamera, MultiMeshRayCasterCameraCfg, patterns +from isaaclab.sim import PinholeCameraCfg +from isaaclab.terrains.trimesh.utils import make_plane +from isaaclab.terrains.utils import create_prim_from_mesh +from isaaclab.utils import convert_dict_to_backend +from isaaclab.utils.timer import Timer + +# sample camera poses +POSITION = [2.5, 2.5, 2.5] +QUAT_ROS = [-0.17591989, 0.33985114, 0.82047325, -0.42470819] +QUAT_OPENGL = [0.33985113, 0.17591988, 0.42470818, 0.82047324] +QUAT_WORLD = [-0.3647052, -0.27984815, -0.1159169, 0.88047623] + + +@pytest.fixture(scope="function") +def setup_simulation(): + """Fixture to set up and tear down the simulation environment.""" + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.01 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt) + sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg) + # Ground-plane + mesh = make_plane(size=(100, 100), height=0.0, center_zero=True) + create_prim_from_mesh("/World/defaultGroundPlane", mesh) + # load stage + sim_utils.update_stage() + + camera_cfg = MultiMeshRayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), + debug_vis=False, + pattern_cfg=patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=480, + width=640, + ), + data_types=["distance_to_image_plane"], + ) + + # create xform because placement of camera directly under world is not supported + sim_utils.create_prim("/World/Camera", "Xform") + + yield sim, dt, camera_cfg + + # Cleanup + # close all the opened viewport from before. + rep.vp_manager.destroy_hydra_textures("Replicator") + # stop simulation + # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( + sim._timeline.stop() + # clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.parametrize( + "convention,quat", + [ + ("ros", QUAT_ROS), + ("opengl", QUAT_OPENGL), + ("world", QUAT_WORLD), + ], +) +@pytest.mark.isaacsim_ci +def test_camera_init_offset(setup_simulation, convention, quat): + """Test camera initialization with offset using different conventions.""" + sim, dt, camera_cfg = setup_simulation + + # Create camera config with specific convention + cam_cfg_offset = copy.deepcopy(camera_cfg) + cam_cfg_offset.offset = MultiMeshRayCasterCameraCfg.OffsetCfg( + pos=POSITION, + rot=quat, + convention=convention, + ) + sim_utils.create_prim(f"/World/CameraOffset{convention.capitalize()}", "Xform") + cam_cfg_offset.prim_path = f"/World/CameraOffset{convention.capitalize()}" + + camera = MultiMeshRayCasterCamera(cam_cfg_offset) + + # play sim + sim.reset() + + # update camera + camera.update(dt) + + # check that transform is set correctly + np.testing.assert_allclose(camera.data.pos_w[0].cpu().numpy(), cam_cfg_offset.offset.pos) + + del camera + + +@pytest.mark.isaacsim_ci +def test_camera_init(setup_simulation): + """Test camera initialization.""" + sim, dt, camera_cfg = setup_simulation + + # Create camera + camera = MultiMeshRayCasterCamera(cfg=camera_cfg) + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (1, 3) + assert camera.data.quat_w_ros.shape == (1, 4) + assert camera.data.quat_w_world.shape == (1, 4) + assert camera.data.quat_w_opengl.shape == (1, 4) + assert camera.data.intrinsic_matrices.shape == (1, 3, 3) + assert camera.data.image_shape == (camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width) + assert camera.data.info == [{camera_cfg.data_types[0]: None}] + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1) + + del camera + + +@pytest.mark.isaacsim_ci +def test_camera_resolution(setup_simulation): + """Test camera resolution is correctly set.""" + sim, dt, camera_cfg = setup_simulation + + # Create camera + camera = MultiMeshRayCasterCamera(cfg=camera_cfg) + # Play sim + sim.reset() + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + camera.update(dt) + # access image data and compare shapes + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1) + + del camera + + +@pytest.mark.isaacsim_ci +def test_camera_init_intrinsic_matrix(setup_simulation): + """Test camera initialization from intrinsic matrix.""" + sim, dt, camera_cfg = setup_simulation + + # get the first camera + camera_1 = MultiMeshRayCasterCamera(cfg=camera_cfg) + # get intrinsic matrix + sim.reset() + intrinsic_matrix = camera_1.data.intrinsic_matrices[0].cpu().flatten().tolist() + + # initialize from intrinsic matrix + intrinsic_camera_cfg = MultiMeshRayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), + debug_vis=False, + pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsic_matrix, + height=camera_cfg.pattern_cfg.height, + width=camera_cfg.pattern_cfg.width, + focal_length=camera_cfg.pattern_cfg.focal_length, + ), + data_types=["distance_to_image_plane"], + ) + camera_2 = MultiMeshRayCasterCamera(cfg=intrinsic_camera_cfg) + + # play sim + sim.reset() + sim.play() + + # update cameras + camera_1.update(dt) + camera_2.update(dt) + + # check image data + torch.testing.assert_close( + camera_1.data.output["distance_to_image_plane"], + camera_2.data.output["distance_to_image_plane"], + ) + # check that both intrinsic matrices are the same + torch.testing.assert_close( + camera_1.data.intrinsic_matrices[0], + camera_2.data.intrinsic_matrices[0], + ) + + del camera_1, camera_2 + + +@pytest.mark.isaacsim_ci +def test_multi_camera_init(setup_simulation): + """Test multi-camera initialization.""" + sim, dt, camera_cfg = setup_simulation + + # -- camera 1 + cam_cfg_1 = copy.deepcopy(camera_cfg) + cam_cfg_1.prim_path = "/World/Camera_0" + sim_utils.create_prim("/World/Camera_0", "Xform") + # Create camera + cam_1 = MultiMeshRayCasterCamera(cam_cfg_1) + + # -- camera 2 + cam_cfg_2 = copy.deepcopy(camera_cfg) + cam_cfg_2.prim_path = "/World/Camera_1" + sim_utils.create_prim("/World/Camera_1", "Xform") + # Create camera + cam_2 = MultiMeshRayCasterCamera(cam_cfg_2) + + # play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + cam_1.update(dt) + cam_2.update(dt) + # check image data + for cam in [cam_1, cam_2]: + for im_data in cam.data.output.values(): + assert im_data.shape == (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1) + + del cam_1, cam_2 + + +@pytest.mark.isaacsim_ci +def test_camera_set_world_poses(setup_simulation): + """Test camera function to set specific world pose.""" + sim, dt, camera_cfg = setup_simulation + + camera = MultiMeshRayCasterCamera(camera_cfg) + # play sim + sim.reset() + + # convert to torch tensors + position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + orientation = torch.tensor([QUAT_WORLD], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_world_poses(position.clone(), orientation.clone(), convention="world") + + # check if transform correctly set in output + torch.testing.assert_close(camera.data.pos_w, position) + torch.testing.assert_close(camera.data.quat_w_world, orientation) + + del camera + + +@pytest.mark.isaacsim_ci +def test_camera_set_world_poses_from_view(setup_simulation): + """Test camera function to set specific world pose from view.""" + sim, dt, camera_cfg = setup_simulation + + camera = MultiMeshRayCasterCamera(camera_cfg) + # play sim + sim.reset() + + # convert to torch tensors + eyes = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + quat_ros_gt = torch.tensor([QUAT_ROS], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_world_poses_from_view(eyes.clone(), targets.clone()) + + # check if transform correctly set in output + torch.testing.assert_close(camera.data.pos_w, eyes) + torch.testing.assert_close(camera.data.quat_w_ros, quat_ros_gt) + + del camera + + +@pytest.mark.parametrize("height,width", [(240, 320), (480, 640)]) +@pytest.mark.isaacsim_ci +def test_intrinsic_matrix(setup_simulation, height, width): + """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" + sim, dt, camera_cfg = setup_simulation + + camera_cfg_copy = copy.deepcopy(camera_cfg) + camera_cfg_copy.pattern_cfg.height = height + camera_cfg_copy.pattern_cfg.width = width + camera = MultiMeshRayCasterCamera(camera_cfg_copy) + # play sim + sim.reset() + # Desired properties (obtained from realsense camera at 320x240 resolution) + rs_intrinsic_matrix = [229.31640625, 0.0, 164.810546875, 0.0, 229.826171875, 122.1650390625, 0.0, 0.0, 1.0] + rs_intrinsic_matrix = torch.tensor(rs_intrinsic_matrix, device=camera.device).reshape(3, 3).unsqueeze(0) + # Set matrix into simulator + camera.set_intrinsic_matrices(rs_intrinsic_matrix.clone()) + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # Check that matrix is correct + torch.testing.assert_close(rs_intrinsic_matrix, camera.data.intrinsic_matrices) + + del camera + + +@pytest.mark.isaacsim_ci +def test_throughput(setup_simulation): + """Test camera throughput for different image sizes.""" + sim, dt, camera_cfg = setup_simulation + + # Create directory temp dir to dump the results + file_dir = os.path.dirname(os.path.realpath(__file__)) + temp_dir = os.path.join(file_dir, "output", "camera", "throughput") + os.makedirs(temp_dir, exist_ok=True) + # Create replicator writer + rep_writer = rep.BasicWriter(output_dir=temp_dir, frame_padding=3) + # create camera + camera_cfg_copy = copy.deepcopy(camera_cfg) + camera_cfg_copy.pattern_cfg.height = 480 + camera_cfg_copy.pattern_cfg.width = 640 + camera = MultiMeshRayCasterCamera(camera_cfg_copy) + + # Play simulator + sim.reset() + + # Set camera pose + eyes = torch.tensor([[2.5, 2.5, 2.5]], dtype=torch.float32, device=camera.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + camera.set_world_poses_from_view(eyes, targets) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # Simulate physics + for _ in range(5): + # perform rendering + sim.step() + # update camera + with Timer(f"Time taken for updating camera with shape {camera.image_shape}"): + camera.update(dt) + # Save images + with Timer(f"Time taken for writing data with shape {camera.image_shape} "): + # Pack data back into replicator format to save them using its writer + rep_output = {"annotators": {}} + camera_data = convert_dict_to_backend(camera.data.output, backend="numpy") + for key, data, info in zip(camera_data.keys(), camera_data.values(), camera.data.info[0].values()): + if info is not None: + rep_output["annotators"][key] = {"render_product": {"data": data, **info}} + else: + rep_output["annotators"][key] = {"render_product": {"data": data}} + # Save images + rep_output["trigger_outputs"] = {"on_time": camera.frame[0]} + rep_writer.write(rep_output) + print("----------------------------------------") + # Check image data + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg_copy.pattern_cfg.height, camera_cfg_copy.pattern_cfg.width, 1) + + del camera + + +@pytest.mark.parametrize( + "data_types", + [ + ["distance_to_image_plane", "distance_to_camera", "normals"], + ["distance_to_image_plane"], + ["distance_to_camera"], + ], +) +@pytest.mark.isaacsim_ci +def test_output_equal_to_usdcamera(setup_simulation, data_types): + """Test that ray caster camera output equals USD camera output.""" + sim, dt, camera_cfg = setup_simulation + + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=240, + width=320, + ) + sim_utils.create_prim("/World/Camera_warp", "Xform") + camera_cfg_warp = MultiMeshRayCasterCameraCfg( + prim_path="/World/Camera_warp", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=data_types, + ) + + camera_warp = MultiMeshRayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=240, + width=320, + prim_path="/World/Camera_usd", + update_period=0, + data_types=data_types, + spawn=PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-4, 1.0e5) + ), + ) + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # convert to torch tensors + eyes = torch.tensor([[2.5, 2.5, 4.5]], dtype=torch.float32, device=camera_warp.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera_warp.device) + # set views + camera_warp.set_world_poses_from_view(eyes, targets) + camera_usd.set_world_poses_from_view(eyes, targets) + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # check the intrinsic matrices + torch.testing.assert_close( + camera_usd.data.intrinsic_matrices, + camera_warp.data.intrinsic_matrices, + ) + + # check the apertures + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), + camera_cfg_warp.pattern_cfg.horizontal_aperture, + ) + + # check image data + for data_type in data_types: + if data_type in camera_usd.data.output and data_type in camera_warp.data.output: + if data_type == "distance_to_camera" or data_type == "distance_to_image_plane": + torch.testing.assert_close( + camera_usd.data.output[data_type], + camera_warp.data.output[data_type], + atol=5e-5, + rtol=5e-6, + ) + elif data_type == "normals": + # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case + torch.testing.assert_close( + camera_usd.data.output[data_type][..., :3], + camera_warp.data.output[data_type], + rtol=1e-5, + atol=1e-4, + ) + else: + torch.testing.assert_close( + camera_usd.data.output[data_type], + camera_warp.data.output[data_type], + ) + + del camera_usd, camera_warp + + +@pytest.mark.isaacsim_ci +def test_output_equal_to_usdcamera_offset(setup_simulation): + """Test that ray caster camera output equals USD camera output with offset.""" + sim, dt, camera_cfg = setup_simulation + offset_rot = (-0.1251, 0.3617, 0.8731, -0.3020) + + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=240, + width=320, + ) + sim_utils.create_prim("/World/Camera_warp", "Xform") + camera_cfg_warp = MultiMeshRayCasterCameraCfg( + prim_path="/World/Camera_warp", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(2.5, 2.5, 4.0), rot=offset_rot, convention="ros"), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + ) + camera_warp = MultiMeshRayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=240, + width=320, + prim_path="/World/Camera_usd", + update_period=0, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + spawn=PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-6, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(2.5, 2.5, 4.0), rot=offset_rot, convention="ros"), + ) + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # check image data + torch.testing.assert_close( + camera_usd.data.output["distance_to_image_plane"], + camera_warp.data.output["distance_to_image_plane"], + atol=5e-5, + rtol=5e-6, + ) + torch.testing.assert_close( + camera_usd.data.output["distance_to_camera"], + camera_warp.data.output["distance_to_camera"], + atol=5e-5, + rtol=5e-6, + ) + + # check normals + # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case + torch.testing.assert_close( + camera_usd.data.output["normals"][..., :3], + camera_warp.data.output["normals"], + rtol=1e-5, + atol=1e-4, + ) + + del camera_usd, camera_warp + + +@pytest.mark.isaacsim_ci +def test_output_equal_to_usdcamera_prim_offset(setup_simulation): + """Test that the output of the ray caster camera is equal to the output of the usd camera when both are placed + under an XForm prim that is translated and rotated from the world origin.""" + sim, dt, camera_cfg = setup_simulation + + offset_rot = [-0.1251, 0.3617, 0.8731, -0.3020] + + # gf quat + gf_quatf = Gf.Quatd() + gf_quatf.SetReal(QUAT_OPENGL[0]) + gf_quatf.SetImaginary(tuple(QUAT_OPENGL[1:])) + + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=240, + width=320, + ) + prim_raycast_cam = sim_utils.create_prim("/World/Camera_warp", "Xform") + prim_raycast_cam.GetAttribute("xformOp:translate").Set(tuple(POSITION)) + prim_raycast_cam.GetAttribute("xformOp:orient").Set(gf_quatf) + + camera_cfg_warp = MultiMeshRayCasterCameraCfg( + prim_path="/World/Camera_warp", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0, 0, 2.0), rot=offset_rot, convention="ros"), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + ) + + camera_warp = MultiMeshRayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=240, + width=320, + prim_path="/World/Camera_usd/camera", + update_period=0, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + spawn=PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-6, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(0, 0, 2.0), rot=offset_rot, convention="ros"), + update_latest_camera_pose=True, + ) + prim_usd = sim_utils.create_prim("/World/Camera_usd", "Xform") + prim_usd.GetAttribute("xformOp:translate").Set(tuple(POSITION)) + prim_usd.GetAttribute("xformOp:orient").Set(gf_quatf) + + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # check if pos and orientation are correct + torch.testing.assert_close(camera_warp.data.pos_w[0], camera_usd.data.pos_w[0]) + torch.testing.assert_close(camera_warp.data.quat_w_ros[0], camera_usd.data.quat_w_ros[0]) + + # check image data + torch.testing.assert_close( + camera_usd.data.output["distance_to_image_plane"], + camera_warp.data.output["distance_to_image_plane"], + atol=5e-5, + rtol=5e-6, + ) + torch.testing.assert_close( + camera_usd.data.output["distance_to_camera"], + camera_warp.data.output["distance_to_camera"], + rtol=4e-6, + atol=2e-5, + ) + + # check normals + # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case + torch.testing.assert_close( + camera_usd.data.output["normals"][..., :3], + camera_warp.data.output["normals"], + rtol=1e-5, + atol=1e-4, + ) + + del camera_usd, camera_warp + + +@pytest.mark.parametrize("height,width", [(540, 960), (240, 320)]) +@pytest.mark.isaacsim_ci +def test_output_equal_to_usd_camera_intrinsics(setup_simulation, height, width): + """Test that the output of the ray caster camera and usd camera are the same when both are + initialized with the same intrinsic matrix.""" + sim, dt, camera_cfg = setup_simulation + + # create cameras + offset_rot = [-0.1251, 0.3617, 0.8731, -0.3020] + offset_pos = (2.5, 2.5, 4.0) + intrinsics = [380.0831, 0.0, width / 2, 0.0, 380.0831, height / 2, 0.0, 0.0, 1.0] + sim_utils.create_prim("/World/Camera_warp", "Xform") + # get camera cfgs + camera_warp_cfg = MultiMeshRayCasterCameraCfg( + prim_path="/World/Camera_warp", + mesh_prim_paths=["/World/defaultGroundPlane"], + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), + debug_vis=False, + pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsics, + height=height, + width=width, + focal_length=38.0, + ), + max_distance=25.0, + data_types=["distance_to_image_plane"], + ) + camera_usd_cfg = CameraCfg( + prim_path="/World/Camera_usd", + offset=CameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), + spawn=PinholeCameraCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsics, + height=height, + width=width, + clipping_range=(0.01, 25), + focal_length=38.0, + ), + height=height, + width=width, + data_types=["distance_to_image_plane"], + ) + + # set aperture offsets to 0, as currently not supported for usd camera + camera_warp_cfg.pattern_cfg.horizontal_aperture_offset = 0 + camera_warp_cfg.pattern_cfg.vertical_aperture_offset = 0 + camera_usd_cfg.spawn.horizontal_aperture_offset = 0 + camera_usd_cfg.spawn.vertical_aperture_offset = 0 + # init cameras + camera_warp = MultiMeshRayCasterCamera(camera_warp_cfg) + camera_usd = Camera(camera_usd_cfg) + + # play sim + sim.reset() + sim.play() + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # filter nan and inf from output + cam_warp_output = camera_warp.data.output["distance_to_image_plane"].clone() + cam_usd_output = camera_usd.data.output["distance_to_image_plane"].clone() + cam_warp_output[torch.isnan(cam_warp_output)] = 0 + cam_warp_output[torch.isinf(cam_warp_output)] = 0 + cam_usd_output[torch.isnan(cam_usd_output)] = 0 + cam_usd_output[torch.isinf(cam_usd_output)] = 0 + + # check that both have the same intrinsic matrices + torch.testing.assert_close(camera_warp.data.intrinsic_matrices[0], camera_usd.data.intrinsic_matrices[0]) + + # check the apertures + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), + camera_warp_cfg.pattern_cfg.horizontal_aperture, + ) + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(), + camera_warp_cfg.pattern_cfg.vertical_aperture, + ) + + # check image data + torch.testing.assert_close( + cam_warp_output, + cam_usd_output, + atol=5e-5, + rtol=5e-6, + ) + + del camera_usd, camera_warp + + +@pytest.mark.isaacsim_ci +def test_output_equal_to_usd_camera_when_intrinsics_set(setup_simulation): + """Test that the output of the ray caster camera is equal to the output of the usd camera when both are placed + under an XForm prim and an intrinsic matrix is set.""" + sim, dt, camera_cfg = setup_simulation + + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=540, + width=960, + ) + camera_cfg_warp = MultiMeshRayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=["distance_to_camera"], + ) + + camera_warp = MultiMeshRayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=540, + width=960, + prim_path="/World/Camera_usd", + update_period=0, + data_types=["distance_to_camera"], + spawn=PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-4, 1.0e5) + ), + ) + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # set intrinsic matrix + # NOTE: extend the test to cover aperture offsets once supported by the usd camera + intrinsic_matrix = torch.tensor( + [[380.0831, 0.0, camera_cfg_usd.width / 2, 0.0, 380.0831, camera_cfg_usd.height / 2, 0.0, 0.0, 1.0]], + device=camera_warp.device, + ).reshape(1, 3, 3) + camera_warp.set_intrinsic_matrices(intrinsic_matrix, focal_length=10) + camera_usd.set_intrinsic_matrices(intrinsic_matrix, focal_length=10) + + # set camera position + camera_warp.set_world_poses_from_view( + eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_warp.device), + targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_warp.device), + ) + camera_usd.set_world_poses_from_view( + eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_usd.device), + targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_usd.device), + ) + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # check image data + torch.testing.assert_close( + camera_usd.data.output["distance_to_camera"], + camera_warp.data.output["distance_to_camera"], + rtol=5e-3, + atol=1e-4, + ) + + del camera_usd, camera_warp diff --git a/source/isaaclab/test/sensors/test_multi_tiled_camera.py b/source/isaaclab/test/sensors/test_multi_tiled_camera.py index 04c2ece67d40..a1fb9a178351 100644 --- a/source/isaaclab/test/sensors/test_multi_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_multi_tiled_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,22 +8,21 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" import copy -import numpy as np import random + +import numpy as np +import pytest import torch -import unittest +from flaky import flaky -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep from isaacsim.core.prims import SingleGeometryPrim, SingleRigidPrim from pxr import Gf, UsdGeom @@ -32,502 +31,478 @@ from isaaclab.sensors.camera import TiledCamera, TiledCameraCfg -class TestMultiTiledCamera(unittest.TestCase): - """Test for USD multi tiled Camera sensor.""" - - def setUp(self): - """Create a blank new stage for each test.""" - - self.camera_cfg = TiledCameraCfg( - height=128, - width=256, - offset=TiledCameraCfg.OffsetCfg(pos=(0.0, 0.0, 4.0), rot=(0.0, 0.0, 1.0, 0.0), convention="ros"), - prim_path="/World/Camera", - update_period=0, - data_types=["rgb", "distance_to_camera"], - spawn=sim_utils.PinholeCameraCfg( - focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) - ), - ) - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.01 - # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=self.dt) - self.sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg) - # populate scene - self._populate_scene() - # load stage - stage_utils.update_stage() - - def tearDown(self): - """Stops simulator after each test.""" - # close all the opened viewport from before. - rep.vp_manager.destroy_hydra_textures("Replicator") - # stop simulation - # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - self.sim._timeline.stop() - # clear the stage - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Tests - """ - - def test_multi_tiled_camera_init(self): - """Test initialization of multiple tiled cameras.""" - - num_tiled_cameras = 3 - num_cameras_per_tiled_camera = 7 - - tiled_cameras = [] - for i in range(num_tiled_cameras): - for j in range(num_cameras_per_tiled_camera): - prim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" - camera = TiledCamera(camera_cfg) - tiled_cameras.append(camera) - - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - - # Play sim - self.sim.reset() - +@pytest.fixture() +def setup_camera(): + """Create a blank new stage for each test.""" + camera_cfg = TiledCameraCfg( + height=128, + width=256, + offset=TiledCameraCfg.OffsetCfg(pos=(0.0, 0.0, 4.0), rot=(0.0, 0.0, 1.0, 0.0), convention="ros"), + prim_path="/World/Camera", + update_period=0, + data_types=["rgb", "distance_to_camera"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + ) + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.01 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt) + sim = sim_utils.SimulationContext(sim_cfg) + # populate scene + _populate_scene() + # load stage + sim_utils.update_stage() + yield camera_cfg, sim, dt + # Teardown + rep.vp_manager.destroy_hydra_textures("Replicator") + # stop simulation + # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( + sim._timeline.stop() + # clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_multi_tiled_camera_init(setup_camera): + """Test initialization of multiple tiled cameras.""" + camera_cfg, sim, dt = setup_camera + num_tiled_cameras = 3 + num_cameras_per_tiled_camera = 7 + + tiled_cameras = [] + for i in range(num_tiled_cameras): + for j in range(num_cameras_per_tiled_camera): + sim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" + camera = TiledCamera(camera_cfg) + tiled_cameras.append(camera) + + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + + # Play sim + sim.reset() + + for i, camera in enumerate(tiled_cameras): + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == f"/World/Origin_{i}_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + for camera in tiled_cameras: + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras_per_tiled_camera, 3) + assert camera.data.quat_w_ros.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.quat_w_world.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras_per_tiled_camera, 3, 3) + assert camera.data.image_shape == (camera.cfg.height, camera.cfg.width) + + # Simulate physics + for _ in range(10): + # Initialize data arrays + rgbs = [] + distances = [] + + # perform rendering + sim.step() for i, camera in enumerate(tiled_cameras): - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, f"/World/Origin_{i}_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - for camera in tiled_cameras: - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras_per_tiled_camera, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras_per_tiled_camera, 3, 3)) - self.assertEqual(camera.data.image_shape, (camera.cfg.height, camera.cfg.width)) - - # Simulate physics - for _ in range(10): - # Initialize data arrays - rgbs = [] - distances = [] - - # perform rendering - self.sim.step() - for i, camera in enumerate(tiled_cameras): - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type == "rgb": - im_data = im_data.clone() / 255.0 - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) - ) - for j in range(num_cameras_per_tiled_camera): - self.assertGreater((im_data[j]).mean().item(), 0.0) - rgbs.append(im_data) - elif data_type == "distance_to_camera": - im_data = im_data.clone() - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) - ) - for j in range(num_cameras_per_tiled_camera): - self.assertGreater(im_data[j].mean().item(), 0.0) - distances.append(im_data) - - # Check data from tiled cameras are consistent, assumes >1 tiled cameras - for i in range(1, num_tiled_cameras): - self.assertLess(torch.abs(rgbs[0] - rgbs[i]).mean(), 0.05) # images of same color should be below 0.001 - self.assertLess( - torch.abs(distances[0] - distances[i]).mean(), 0.01 - ) # distances of same scene should be 0 - - for camera in tiled_cameras: - del camera - - def test_all_annotators_multi_tiled_camera(self): - """Test initialization of multiple tiled cameras with all supported annotators.""" - - all_annotator_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ] - - num_tiled_cameras = 2 - num_cameras_per_tiled_camera = 9 - - tiled_cameras = [] - for i in range(num_tiled_cameras): - for j in range(num_cameras_per_tiled_camera): - prim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = all_annotator_types - camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" - camera = TiledCamera(camera_cfg) - tiled_cameras.append(camera) - - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - - # Play sim - self.sim.reset() - + # update camera + camera.update(dt) + # check image data + for data_type, im_data in camera.data.output.items(): + if data_type == "rgb": + im_data = im_data.clone() / 255.0 + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) + for j in range(num_cameras_per_tiled_camera): + assert (im_data[j]).mean().item() > 0.0 + rgbs.append(im_data) + elif data_type == "distance_to_camera": + im_data = im_data.clone() + im_data[torch.isinf(im_data)] = 0 + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) + for j in range(num_cameras_per_tiled_camera): + assert im_data[j].mean().item() > 0.0 + distances.append(im_data) + + # Check data from tiled cameras are consistent, assumes >1 tiled cameras + for i in range(1, num_tiled_cameras): + assert torch.abs(rgbs[0] - rgbs[i]).mean() < 0.05 # images of same color should be below 0.001 + assert torch.abs(distances[0] - distances[i]).mean() < 0.01 # distances of same scene should be 0 + + for camera in tiled_cameras: + del camera + + +@pytest.mark.isaacsim_ci +def test_all_annotators_multi_tiled_camera(setup_camera): + """Test initialization of multiple tiled cameras with all supported annotators.""" + camera_cfg, sim, dt = setup_camera + all_annotator_types = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + + num_tiled_cameras = 2 + num_cameras_per_tiled_camera = 9 + + tiled_cameras = [] + for i in range(num_tiled_cameras): + for j in range(num_cameras_per_tiled_camera): + sim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = all_annotator_types + camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" + camera = TiledCamera(camera_cfg) + tiled_cameras.append(camera) + + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + + # Play sim + sim.reset() + + for i, camera in enumerate(tiled_cameras): + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == f"/World/Origin_{i}_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert sorted(camera.data.output.keys()) == sorted(all_annotator_types) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + for camera in tiled_cameras: + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras_per_tiled_camera, 3) + assert camera.data.quat_w_ros.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.quat_w_world.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras_per_tiled_camera, 3, 3) + assert camera.data.image_shape == (camera.cfg.height, camera.cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() for i, camera in enumerate(tiled_cameras): - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, f"/World/Origin_{i}_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(sorted(camera.data.output.keys()), sorted(all_annotator_types)) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - for camera in tiled_cameras: - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras_per_tiled_camera, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras_per_tiled_camera, 3, 3)) - self.assertEqual(camera.data.image_shape, (camera.cfg.height, camera.cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - for i, camera in enumerate(tiled_cameras): - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type in ["rgb", "normals"]: - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) - ) - elif data_type in [ - "rgba", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ]: - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 4) - ) - for i in range(num_cameras_per_tiled_camera): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - elif data_type in ["motion_vectors"]: - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 2) - ) - for i in range(num_cameras_per_tiled_camera): - self.assertNotEqual(im_data[i].mean().item(), 0.0) - elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) - ) - for i in range(num_cameras_per_tiled_camera): - self.assertGreater(im_data[i].mean().item(), 0.0) - - for camera in tiled_cameras: - # access image data and compare dtype - output = camera.data.output - info = camera.data.info - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(type(info["semantic_segmentation"]), dict) - self.assertEqual(type(info["instance_segmentation_fast"]), dict) - self.assertEqual(type(info["instance_id_segmentation_fast"]), dict) - - for camera in tiled_cameras: - del camera - - def test_different_resolution_multi_tiled_camera(self): - """Test multiple tiled cameras with different resolutions.""" - - num_tiled_cameras = 2 - num_cameras_per_tiled_camera = 6 - - tiled_cameras = [] - resolutions = [(4, 4), (16, 16), (64, 64), (512, 512), (23, 765), (1001, 1)] - for i in range(num_tiled_cameras): - for j in range(num_cameras_per_tiled_camera): - prim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" - camera_cfg.height, camera_cfg.width = resolutions[i] - camera = TiledCamera(camera_cfg) - tiled_cameras.append(camera) - - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - - # Play sim - self.sim.reset() - + # update camera + camera.update(dt) + # check image data + for data_type, im_data in camera.data.output.items(): + if data_type in ["rgb", "normals"]: + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) + elif data_type in [ + "rgba", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ]: + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 4) + for i in range(num_cameras_per_tiled_camera): + assert (im_data[i] / 255.0).mean().item() > 0.0 + elif data_type in ["motion_vectors"]: + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 2) + for i in range(num_cameras_per_tiled_camera): + assert im_data[i].mean().item() != 0.0 + elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) + for i in range(num_cameras_per_tiled_camera): + assert im_data[i].mean().item() > 0.0 + + for camera in tiled_cameras: + # access image data and compare dtype + output = camera.data.output + info = camera.data.info + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.uint8 + assert output["instance_segmentation_fast"].dtype == torch.uint8 + assert output["instance_id_segmentation_fast"].dtype == torch.uint8 + assert isinstance(info["semantic_segmentation"], dict) + assert isinstance(info["instance_segmentation_fast"], dict) + assert isinstance(info["instance_id_segmentation_fast"], dict) + + for camera in tiled_cameras: + del camera + + +@flaky(max_runs=3, min_passes=1) +@pytest.mark.isaacsim_ci +def test_different_resolution_multi_tiled_camera(setup_camera): + """Test multiple tiled cameras with different resolutions.""" + camera_cfg, sim, dt = setup_camera + num_tiled_cameras = 2 + num_cameras_per_tiled_camera = 6 + + tiled_cameras = [] + resolutions = [(16, 16), (23, 765)] + for i in range(num_tiled_cameras): + for j in range(num_cameras_per_tiled_camera): + sim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" + camera_cfg.height, camera_cfg.width = resolutions[i] + camera = TiledCamera(camera_cfg) + tiled_cameras.append(camera) + + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + + # Play sim + sim.reset() + + for i, camera in enumerate(tiled_cameras): + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == f"/World/Origin_{i}_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + for camera in tiled_cameras: + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras_per_tiled_camera, 3) + assert camera.data.quat_w_ros.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.quat_w_world.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras_per_tiled_camera, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras_per_tiled_camera, 3, 3) + assert camera.data.image_shape == (camera.cfg.height, camera.cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() for i, camera in enumerate(tiled_cameras): - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, f"/World/Origin_{i}_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - for camera in tiled_cameras: - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras_per_tiled_camera, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras_per_tiled_camera, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras_per_tiled_camera, 3, 3)) - self.assertEqual(camera.data.image_shape, (camera.cfg.height, camera.cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - for i, camera in enumerate(tiled_cameras): - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type == "rgb": - im_data = im_data.clone() / 255.0 - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) - ) - for j in range(num_cameras_per_tiled_camera): - self.assertGreater((im_data[j]).mean().item(), 0.0) - elif data_type == "distance_to_camera": - im_data = im_data.clone() - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) - ) - for j in range(num_cameras_per_tiled_camera): - self.assertGreater(im_data[j].mean().item(), 0.0) - - for camera in tiled_cameras: - del camera - - def test_frame_offset_multi_tiled_camera(self): - """Test frame offset issue with multiple tiled cameras""" - - num_tiled_cameras = 4 - num_cameras_per_tiled_camera = 4 - - tiled_cameras = [] - for i in range(num_tiled_cameras): - for j in range(num_cameras_per_tiled_camera): - prim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" - camera = TiledCamera(camera_cfg) - tiled_cameras.append(camera) - - # modify scene to be less stochastic - stage = stage_utils.get_current_stage() - for i in range(10): - prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") - color = Gf.Vec3f(1, 1, 1) - UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) - - # play sim - self.sim.reset() - - # simulate some steps first to make sure objects are settled - for i in range(100): - # step simulation - self.sim.step() - # update cameras - for camera in tiled_cameras: - camera.update(self.dt) - - # collect image data - image_befores = [camera.data.output["rgb"].clone() / 255.0 for camera in tiled_cameras] - - # update scene - for i in range(10): - prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") - color = Gf.Vec3f(0, 0, 0) - UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) - - # update rendering - self.sim.step() - + # update camera + camera.update(dt) + # check image data + for data_type, im_data in camera.data.output.items(): + if data_type == "rgb": + im_data = im_data.clone() / 255.0 + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) + for j in range(num_cameras_per_tiled_camera): + assert (im_data[j]).mean().item() > 0.0 + elif data_type == "distance_to_camera": + im_data = im_data.clone() + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) + for j in range(num_cameras_per_tiled_camera): + assert im_data[j].mean().item() > 0.0 + + for camera in tiled_cameras: + del camera + + +@pytest.mark.isaacsim_ci +def test_frame_offset_multi_tiled_camera(setup_camera): + """Test frame offset issue with multiple tiled cameras""" + camera_cfg, sim, dt = setup_camera + num_tiled_cameras = 4 + num_cameras_per_tiled_camera = 4 + + tiled_cameras = [] + for i in range(num_tiled_cameras): + for j in range(num_cameras_per_tiled_camera): + sim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" + camera = TiledCamera(camera_cfg) + tiled_cameras.append(camera) + + # modify scene to be less stochastic + stage = sim_utils.get_current_stage() + for i in range(10): + prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") + color = Gf.Vec3f(1, 1, 1) + UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) + + # play sim + sim.reset() + + # simulate some steps first to make sure objects are settled + for i in range(100): + # step simulation + sim.step() # update cameras for camera in tiled_cameras: - camera.update(self.dt) - - # make sure the image is different - image_afters = [camera.data.output["rgb"].clone() / 255.0 for camera in tiled_cameras] - - # check difference is above threshold - for i in range(num_tiled_cameras): - image_before = image_befores[i] - image_after = image_afters[i] - self.assertGreater( - torch.abs(image_after - image_before).mean(), 0.05 - ) # images of same color should be below 0.001 - - for camera in tiled_cameras: - del camera - - def test_frame_different_poses_multi_tiled_camera(self): - """Test multiple tiled cameras placed at different poses render different images.""" - - num_tiled_cameras = 3 - num_cameras_per_tiled_camera = 4 - positions = [(0.0, 0.0, 4.0), (0.0, 0.0, 4.0), (0.0, 0.0, 3.0)] - rotations = [(0.0, 0.0, 1.0, 0.0), (1.0, 0.0, 1.0, 0.0), (0.0, 0.0, 1.0, 0.0)] - - tiled_cameras = [] - for i in range(num_tiled_cameras): - for j in range(num_cameras_per_tiled_camera): - prim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" - camera_cfg.offset = TiledCameraCfg.OffsetCfg(pos=positions[i], rot=rotations[i], convention="ros") - camera = TiledCamera(camera_cfg) - tiled_cameras.append(camera) - - # Play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Simulate physics - for _ in range(10): - # Initialize data arrays - rgbs = [] - distances = [] - - # perform rendering - self.sim.step() - for i, camera in enumerate(tiled_cameras): - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type == "rgb": - im_data = im_data.clone() / 255.0 - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) - ) - for j in range(num_cameras_per_tiled_camera): - self.assertGreater((im_data[j]).mean().item(), 0.0) - rgbs.append(im_data) - elif data_type == "distance_to_camera": - im_data = im_data.clone() - self.assertEqual( - im_data.shape, (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) - ) - for j in range(num_cameras_per_tiled_camera): - self.assertGreater(im_data[j].mean().item(), 0.0) - distances.append(im_data) - - # Check data from tiled cameras are different, assumes >1 tiled cameras - for i in range(1, num_tiled_cameras): - self.assertGreater( - torch.abs(rgbs[0] - rgbs[i]).mean(), 0.05 - ) # images of same color should be below 0.001 - self.assertGreater( - torch.abs(distances[0] - distances[i]).mean(), 0.01 - ) # distances of same scene should be 0 - - for camera in tiled_cameras: - del camera - - """ - Helper functions. - """ - - @staticmethod - def _populate_scene(): - """Add prims to the scene.""" - # Ground-plane - cfg = sim_utils.GroundPlaneCfg() - cfg.func("/World/defaultGroundPlane", cfg) - # Lights - cfg = sim_utils.SphereLightCfg() - cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) - cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) - # Random objects - random.seed(0) - for i in range(10): - # sample random position - position = np.random.rand(3) - np.asarray([0.05, 0.05, -1.0]) - position *= np.asarray([1.5, 1.5, 0.5]) - # create prim - prim_type = random.choice(["Cube", "Sphere", "Cylinder"]) - prim = prim_utils.create_prim( - f"/World/Objects/Obj_{i:02d}", - prim_type, - translation=position, - scale=(0.25, 0.25, 0.25), - semantic_label=prim_type, - ) - # cast to geom prim - geom_prim = getattr(UsdGeom, prim_type)(prim) - # set random color - color = Gf.Vec3f(random.random(), random.random(), random.random()) - geom_prim.CreateDisplayColorAttr() - geom_prim.GetDisplayColorAttr().Set([color]) - # add rigid properties - SingleGeometryPrim(f"/World/Objects/Obj_{i:02d}", collision=True) - SingleRigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) - - -if __name__ == "__main__": # - run_tests() + camera.update(dt) + + # collect image data + image_befores = [camera.data.output["rgb"].clone() / 255.0 for camera in tiled_cameras] + + # update scene + for i in range(10): + prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") + color = Gf.Vec3f(0, 0, 0) + UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) + + # update rendering + sim.step() + + # update cameras + for camera in tiled_cameras: + camera.update(dt) + + # make sure the image is different + image_afters = [camera.data.output["rgb"].clone() / 255.0 for camera in tiled_cameras] + + # check difference is above threshold + for i in range(num_tiled_cameras): + image_before = image_befores[i] + image_after = image_afters[i] + assert torch.abs(image_after - image_before).mean() > 0.02 # images of same color should be below 0.001 + + for camera in tiled_cameras: + del camera + + +@flaky(max_runs=3, min_passes=1) +@pytest.mark.isaacsim_ci +def test_frame_different_poses_multi_tiled_camera(setup_camera): + """Test multiple tiled cameras placed at different poses render different images.""" + camera_cfg, sim, dt = setup_camera + num_tiled_cameras = 3 + num_cameras_per_tiled_camera = 4 + positions = [(0.0, 0.0, 4.0), (0.0, 0.0, 2.0), (0.0, 0.0, 3.0)] + rotations = [(0.0, 0.0, 1.0, 0.0), (0.0, 0.0, 1.0, 0.0), (0.0, 0.0, 1.0, 0.0)] + + tiled_cameras = [] + for i in range(num_tiled_cameras): + for j in range(num_cameras_per_tiled_camera): + sim_utils.create_prim(f"/World/Origin_{i}_{j}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.prim_path = f"/World/Origin_{i}.*/CameraSensor" + camera_cfg.offset = TiledCameraCfg.OffsetCfg(pos=positions[i], rot=rotations[i], convention="ros") + camera = TiledCamera(camera_cfg) + tiled_cameras.append(camera) + + # Play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Simulate physics + for _ in range(10): + # Initialize data arrays + rgbs = [] + distances = [] + + # perform rendering + sim.step() + for i, camera in enumerate(tiled_cameras): + # update camera + camera.update(dt) + # check image data + for data_type, im_data in camera.data.output.items(): + if data_type == "rgb": + im_data = im_data.clone() / 255.0 + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 3) + for j in range(num_cameras_per_tiled_camera): + assert (im_data[j]).mean().item() > 0.0 + rgbs.append(im_data) + elif data_type == "distance_to_camera": + im_data = im_data.clone() + # replace inf with 0 + im_data[torch.isinf(im_data)] = 0 + assert im_data.shape == (num_cameras_per_tiled_camera, camera.cfg.height, camera.cfg.width, 1) + for j in range(num_cameras_per_tiled_camera): + assert im_data[j].mean().item() > 0.0 + distances.append(im_data) + + # Check data from tiled cameras are different, assumes >1 tiled cameras + for i in range(1, num_tiled_cameras): + assert torch.abs(rgbs[0] - rgbs[i]).mean() > 0.04 # images of same color should be below 0.001 + assert torch.abs(distances[0] - distances[i]).mean() > 0.01 # distances of same scene should be 0 + + for camera in tiled_cameras: + del camera + + +""" +Helper functions. +""" + + +def _populate_scene(): + """Add prims to the scene.""" + # TODO: this causes hang with Kit 107.3??? + # # Ground-plane + # cfg = sim_utils.GroundPlaneCfg() + # cfg.func("/World/defaultGroundPlane", cfg) + # Lights + cfg = sim_utils.SphereLightCfg() + cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) + cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) + # Random objects + random.seed(0) + for i in range(10): + # sample random position + position = np.random.rand(3) - np.asarray([0.05, 0.05, -1.0]) + position *= np.asarray([1.5, 1.5, 0.5]) + # create prim + prim_type = random.choice(["Cube", "Sphere", "Cylinder"]) + prim = sim_utils.create_prim( + f"/World/Objects/Obj_{i:02d}", + prim_type, + translation=position, + scale=(0.25, 0.25, 0.25), + semantic_label=prim_type, + ) + # cast to geom prim + geom_prim = getattr(UsdGeom, prim_type)(prim) + # set random color + color = Gf.Vec3f(random.random(), random.random(), random.random()) + geom_prim.CreateDisplayColorAttr() + geom_prim.GetDisplayColorAttr().Set([color]) + # add rigid properties + SingleGeometryPrim(f"/World/Objects/Obj_{i:02d}", collision=True) + SingleRigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) diff --git a/source/isaaclab/test/sensors/test_outdated_sensor.py b/source/isaaclab/test/sensors/test_outdated_sensor.py index a4c9f7f08d6a..ac0c989c6839 100644 --- a/source/isaaclab/test/sensors/test_outdated_sensor.py +++ b/source/isaaclab/test/sensors/test_outdated_sensor.py @@ -1,24 +1,23 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" -import gymnasium as gym import shutil import tempfile + +import gymnasium as gym +import pytest import torch -import unittest import carb import omni.usd @@ -27,63 +26,55 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestFrameTransformerAfterReset(unittest.TestCase): - """Test cases for checking FrameTransformer values after reset.""" - - @classmethod - def setUpClass(cls): - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - def setUp(self): - # create a temporary directory to store the test datasets - self.temp_dir = tempfile.mkdtemp() - - def tearDown(self): - # delete the temporary directory after the test - shutil.rmtree(self.temp_dir) - - def test_action_state_reocrder_terms(self): - """Check FrameTransformer values after reset.""" - for task_name in ["Isaac-Stack-Cube-Franka-IK-Rel-v0"]: - for device in ["cuda:0", "cpu"]: - for num_envs in [1, 2]: - with self.subTest(task_name=task_name, device=device): - omni.usd.get_context().new_stage() +@pytest.fixture() +def temp_dir(): + """Fixture to create and clean up a temporary directory for test datasets.""" + # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + # create a temporary directory to store the test datasets + temp_dir = tempfile.mkdtemp() + yield temp_dir + # delete the temporary directory after the test + shutil.rmtree(temp_dir) - # parse configuration - env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) - # create environment - env = gym.make(task_name, cfg=env_cfg) +@pytest.mark.parametrize("task_name", ["Isaac-Stack-Cube-Franka-IK-Rel-v0"]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.isaacsim_ci +def test_action_state_recorder_terms(temp_dir, task_name, device, num_envs): + """Check FrameTransformer values after reset.""" + omni.usd.get_context().new_stage() - # disable control on stop - env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + env_cfg.wait_for_textures = False - # reset environment - obs = env.reset()[0] + # create environment + env = gym.make(task_name, cfg=env_cfg) - # get the end effector position after the reset - pre_reset_eef_pos = obs["policy"]["eef_pos"].clone() - print(pre_reset_eef_pos) + # disable control on stop + env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore - # step the environment with idle actions - idle_actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) - obs = env.step(idle_actions)[0] + # reset environment + obs = env.reset()[0] - # get the end effector position after the first step - post_reset_eef_pos = obs["policy"]["eef_pos"] - print(post_reset_eef_pos) + # get the end effector position after the reset + pre_reset_eef_pos = obs["policy"]["eef_pos"].clone() + print(pre_reset_eef_pos) - # check if the end effector position is the same after the reset and the first step - print(torch.all(torch.isclose(pre_reset_eef_pos, post_reset_eef_pos))) - self.assertTrue(torch.all(torch.isclose(pre_reset_eef_pos, post_reset_eef_pos))) + # step the environment with idle actions + idle_actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + obs = env.step(idle_actions)[0] - # close the environment - env.close() + # get the end effector position after the first step + post_reset_eef_pos = obs["policy"]["eef_pos"] + print(post_reset_eef_pos) + # check if the end effector position is the same after the reset and the first step + torch.testing.assert_close(pre_reset_eef_pos, post_reset_eef_pos, atol=1e-5, rtol=1e-3) -if __name__ == "__main__": - run_tests() + # close the environment + env.close() diff --git a/source/isaaclab/test/sensors/test_ray_caster.py b/source/isaaclab/test/sensors/test_ray_caster.py new file mode 100644 index 000000000000..01b2dde1ae2a --- /dev/null +++ b/source/isaaclab/test/sensors/test_ray_caster.py @@ -0,0 +1,241 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy as np +import pytest +import torch +import trimesh + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +# Import after app launch +import warp as wp + +from isaaclab.utils.math import matrix_from_quat, quat_from_euler_xyz, random_orientation +from isaaclab.utils.warp.ops import convert_to_warp_mesh, raycast_dynamic_meshes, raycast_mesh + + +@pytest.fixture(scope="module") +def raycast_setup(): + device = "cuda" if torch.cuda.is_available() else "cpu" + # Base trimesh cube and its Warp conversion + trimesh_mesh = trimesh.creation.box([2, 2, 1]) + single_mesh = [ + convert_to_warp_mesh( + trimesh_mesh.vertices, + trimesh_mesh.faces, + device, + ) + ] + single_mesh_id = single_mesh[0].id + + # Rays + ray_starts = torch.tensor([[0, -0.35, -5], [0.25, 0.35, -5]], dtype=torch.float32, device=device).unsqueeze(0) + ray_directions = torch.tensor([[0, 0, 1], [0, 0, 1]], dtype=torch.float32, device=device).unsqueeze(0) + expected_ray_hits = torch.tensor( + [[0, -0.35, -0.5], [0.25, 0.35, -0.5]], dtype=torch.float32, device=device + ).unsqueeze(0) + + return { + "device": device, + "trimesh_mesh": trimesh_mesh, + "single_mesh_id": single_mesh_id, + "wp_mesh": single_mesh[0], + "ray_starts": ray_starts, + "ray_directions": ray_directions, + "expected_ray_hits": expected_ray_hits, + } + + +def test_raycast_multi_cubes(raycast_setup): + device = raycast_setup["device"] + base_tm = raycast_setup["trimesh_mesh"] + + tm1 = base_tm.copy() + wp_mesh_1 = convert_to_warp_mesh(tm1.vertices, tm1.faces, device) + + translation = np.eye(4) + translation[:3, 3] = [0, 2, 0] + tm2 = base_tm.copy().apply_transform(translation) + wp_mesh_2 = convert_to_warp_mesh(tm2.vertices, tm2.faces, device) + + mesh_ids_wp = wp.array2d([[wp_mesh_1.id, wp_mesh_2.id]], dtype=wp.uint64, device=device) + + ray_directions = raycast_setup["ray_directions"] + + # Case 1 + ray_start = torch.tensor([[0, 0, -5], [0, 2.5, -5]], dtype=torch.float32, device=device).unsqueeze(0) + ray_hits, ray_distance, ray_normal, ray_face_id, mesh_ids = raycast_dynamic_meshes( + ray_start, + ray_directions, + mesh_ids_wp, + return_distance=True, + return_normal=True, + return_face_id=True, + return_mesh_id=True, + ) + + torch.testing.assert_close(ray_hits, torch.tensor([[[0, 0, -0.5], [0, 2.5, -0.5]]], device=device)) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], device=device, dtype=torch.float32)) + assert torch.equal(mesh_ids, torch.tensor([[0, 1]], dtype=torch.int32, device=device)) + + # Case 2 (explicit poses/orientations) + ray_start = torch.tensor([[0, 0, -5], [0, 4.5, -5]], dtype=torch.float32, device=device).unsqueeze(0) + ray_hits, ray_distance, ray_normal, ray_face_id, mesh_ids = raycast_dynamic_meshes( + ray_start, + ray_directions, + mesh_ids_wp, + return_distance=True, + return_normal=True, + return_face_id=True, + mesh_positions_w=torch.tensor([[[0, 0, 0], [0, 2, 0]]], dtype=torch.float32, device=device), + mesh_orientations_w=torch.tensor([[[1, 0, 0, 0], [1, 0, 0, 0]]], dtype=torch.float32, device=device), + return_mesh_id=True, + ) + + torch.testing.assert_close(ray_hits, torch.tensor([[[0, 0, -0.5], [0, 4.5, -0.5]]], device=device)) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], device=device, dtype=torch.float32)) + assert torch.equal(mesh_ids, torch.tensor([[0, 1]], dtype=torch.int32, device=device)) + + +def test_raycast_single_cube(raycast_setup): + device = raycast_setup["device"] + ray_starts = raycast_setup["ray_starts"] + ray_directions = raycast_setup["ray_directions"] + mesh = raycast_setup["wp_mesh"] + expected_ray_hits = raycast_setup["expected_ray_hits"] + single_mesh_id = raycast_setup["single_mesh_id"] + + # Single-mesh helper + ray_hits, ray_distance, ray_normal, ray_face_id = raycast_mesh( + ray_starts, + ray_directions, + mesh, + return_distance=True, + return_normal=True, + return_face_id=True, + ) + torch.testing.assert_close(ray_hits, expected_ray_hits) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], device=device, dtype=torch.float32)) + torch.testing.assert_close(ray_face_id, torch.tensor([[3, 8]], dtype=torch.int32, device=device)) + + # Multi-mesh API with one mesh + ray_hits, ray_distance, ray_normal, ray_face_id, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + ) + torch.testing.assert_close(ray_hits, expected_ray_hits) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], device=device, dtype=torch.float32)) + torch.testing.assert_close(ray_face_id, torch.tensor([[3, 8]], dtype=torch.int32, device=device)) + + +def test_raycast_moving_cube(raycast_setup): + device = raycast_setup["device"] + ray_starts = raycast_setup["ray_starts"] + ray_directions = raycast_setup["ray_directions"] + single_mesh_id = raycast_setup["single_mesh_id"] + expected_ray_hits = raycast_setup["expected_ray_hits"] + + for distance in torch.linspace(0, 1, 10, device=device): + ray_hits, ray_distance, ray_normal, ray_face_id, mesh_id = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + return_mesh_id=True, + mesh_positions_w=torch.tensor([[0, 0, distance.item()]], dtype=torch.float32, device=device), + ) + offset = torch.tensor([[0, 0, distance.item()], [0, 0, distance.item()]], dtype=torch.float32, device=device) + torch.testing.assert_close(ray_hits, expected_ray_hits + offset.unsqueeze(0)) + torch.testing.assert_close(ray_distance, distance + torch.tensor([[4.5, 4.5]], device=device)) + torch.testing.assert_close( + ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], device=device, dtype=torch.float32) + ) + torch.testing.assert_close(ray_face_id, torch.tensor([[3, 8]], dtype=torch.int32, device=device)) + + +def test_raycast_rotated_cube(raycast_setup): + device = raycast_setup["device"] + ray_starts = raycast_setup["ray_starts"] + ray_directions = raycast_setup["ray_directions"] + single_mesh_id = raycast_setup["single_mesh_id"] + expected_ray_hits = raycast_setup["expected_ray_hits"] + + cube_rotation = quat_from_euler_xyz( + torch.tensor([0.0], device=device), torch.tensor([0.0], device=device), torch.tensor([np.pi], device=device) + ) + ray_hits, ray_distance, ray_normal, ray_face_id, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + mesh_orientations_w=cube_rotation.unsqueeze(0), + ) + torch.testing.assert_close(ray_hits, expected_ray_hits) + torch.testing.assert_close(ray_distance, torch.tensor([[4.5, 4.5]], device=device)) + torch.testing.assert_close(ray_normal, torch.tensor([[[0, 0, -1], [0, 0, -1]]], device=device, dtype=torch.float32)) + # Rotated cube swaps face IDs + torch.testing.assert_close(ray_face_id, torch.tensor([[8, 3]], dtype=torch.int32, device=device)) + + +def test_raycast_random_cube(raycast_setup): + device = raycast_setup["device"] + base_tm = raycast_setup["trimesh_mesh"] + ray_starts = raycast_setup["ray_starts"] + ray_directions = raycast_setup["ray_directions"] + single_mesh_id = raycast_setup["single_mesh_id"] + + for orientation in random_orientation(10, device): + pos = torch.tensor([[0.0, 0.0, torch.rand(1, device=device).item()]], dtype=torch.float32, device=device) + + tf_hom = np.eye(4) + tf_hom[:3, :3] = matrix_from_quat(orientation).cpu().numpy() + tf_hom[:3, 3] = pos.squeeze(0).cpu().numpy() + + tf_mesh = base_tm.copy().apply_transform(tf_hom) + wp_mesh = convert_to_warp_mesh(tf_mesh.vertices, tf_mesh.faces, device) + + # Raycast transformed, static mesh + ray_hits, ray_distance, ray_normal, ray_face_id, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[wp_mesh.id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + ) + # Raycast original mesh with pose provided + ray_hits_m, ray_distance_m, ray_normal_m, ray_face_id_m, _ = raycast_dynamic_meshes( + ray_starts, + ray_directions, + wp.array2d([[single_mesh_id]], dtype=wp.uint64, device=device), + return_distance=True, + return_normal=True, + return_face_id=True, + mesh_positions_w=pos, + mesh_orientations_w=orientation.view(1, 1, -1), + ) + + torch.testing.assert_close(ray_hits, ray_hits_m) + torch.testing.assert_close(ray_distance, ray_distance_m) + torch.testing.assert_close(ray_normal, ray_normal_m) + torch.testing.assert_close(ray_face_id, ray_face_id_m) diff --git a/source/isaaclab/test/sensors/test_ray_caster_camera.py b/source/isaaclab/test/sensors/test_ray_caster_camera.py index 816857019303..8b4c2f4a973a 100644 --- a/source/isaaclab/test/sensors/test_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_ray_caster_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,22 +8,20 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" import copy -import numpy as np import os + +import numpy as np +import pytest import torch -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep from pxr import Gf @@ -42,809 +40,847 @@ QUAT_OPENGL = [0.33985113, 0.17591988, 0.42470818, 0.82047324] QUAT_WORLD = [-0.3647052, -0.27984815, -0.1159169, 0.88047623] - -class TestWarpCamera(unittest.TestCase): - """Test for isaaclab camera sensor""" - - """ - Test Setup and Teardown - """ - - def setUp(self): - """Create a blank new stage for each test.""" - camera_pattern_cfg = patterns.PinholeCameraPatternCfg( - focal_length=24.0, - horizontal_aperture=20.955, - height=480, - width=640, - ) - self.camera_cfg = RayCasterCameraCfg( - prim_path="/World/Camera", - mesh_prim_paths=["/World/defaultGroundPlane"], - update_period=0, - offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), - debug_vis=False, - pattern_cfg=camera_pattern_cfg, - data_types=[ - "distance_to_image_plane", - ], - ) - # Create a new stage - stage_utils.create_new_stage() - # create xform because placement of camera directly under world is not supported - prim_utils.create_prim("/World/Camera", "Xform") - # Simulation time-step - self.dt = 0.01 - # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=self.dt) - self.sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg) - # Ground-plane - mesh = make_plane(size=(100, 100), height=0.0, center_zero=True) - create_prim_from_mesh("/World/defaultGroundPlane", mesh) - # load stage - stage_utils.update_stage() - - def tearDown(self): - """Stops simulator after each test.""" - # close all the opened viewport from before. - rep.vp_manager.destroy_hydra_textures("Replicator") - # stop simulation - # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - self.sim._timeline.stop() - # clear the stage - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Tests - """ - - def test_camera_init(self): - """Test camera initialization.""" - # Create camera - camera = RayCasterCamera(cfg=self.camera_cfg) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (1, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (1, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (1, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (1, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (1, 3, 3)) - self.assertEqual( - camera.data.image_shape, (self.camera_cfg.pattern_cfg.height, self.camera_cfg.pattern_cfg.width) - ) - self.assertEqual(camera.data.info, [{self.camera_cfg.data_types[0]: None}]) - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for im_data in camera.data.output.values(): - self.assertEqual( - im_data.shape, (1, self.camera_cfg.pattern_cfg.height, self.camera_cfg.pattern_cfg.width, 1) - ) - - def test_camera_resolution(self): - """Test camera resolution is correctly set.""" - # Create camera - camera = RayCasterCamera(cfg=self.camera_cfg) - # Play sim - self.sim.reset() - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - camera.update(self.dt) - # access image data and compare shapes - for im_data in camera.data.output.values(): - self.assertTrue( - im_data.shape == (1, self.camera_cfg.pattern_cfg.height, self.camera_cfg.pattern_cfg.width, 1) - ) - - def test_depth_clipping(self): - """Test depth clipping. - - .. note:: - - This test is the same for all camera models to enforce the same clipping behavior. - """ - prim_utils.create_prim("/World/CameraZero", "Xform") - prim_utils.create_prim("/World/CameraNone", "Xform") - prim_utils.create_prim("/World/CameraMax", "Xform") - - # get camera cfgs - camera_cfg_zero = RayCasterCameraCfg( - prim_path="/World/CameraZero", - mesh_prim_paths=["/World/defaultGroundPlane"], - offset=RayCasterCameraCfg.OffsetCfg( - pos=(2.5, 2.5, 6.0), rot=(0.9914449, 0.0, 0.1305, 0.0), convention="world" - ), - pattern_cfg=patterns.PinholeCameraPatternCfg().from_intrinsic_matrix( - focal_length=38.0, - intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], - height=540, - width=960, - ), - max_distance=10.0, - data_types=["distance_to_image_plane", "distance_to_camera"], - depth_clipping_behavior="zero", - ) - camera_zero = RayCasterCamera(camera_cfg_zero) - - camera_cfg_none = copy.deepcopy(camera_cfg_zero) - camera_cfg_none.prim_path = "/World/CameraNone" - camera_cfg_none.depth_clipping_behavior = "none" - camera_none = RayCasterCamera(camera_cfg_none) - - camera_cfg_max = copy.deepcopy(camera_cfg_zero) - camera_cfg_max.prim_path = "/World/CameraMax" - camera_cfg_max.depth_clipping_behavior = "max" - camera_max = RayCasterCamera(camera_cfg_max) - - # Play sim - self.sim.reset() - - camera_zero.update(self.dt) - camera_none.update(self.dt) - camera_max.update(self.dt) - - # none clipping should contain inf values - self.assertTrue(torch.isinf(camera_none.data.output["distance_to_camera"]).any()) - self.assertTrue(torch.isnan(camera_none.data.output["distance_to_image_plane"]).any()) - self.assertTrue( - camera_none.data.output["distance_to_camera"][ - ~torch.isinf(camera_none.data.output["distance_to_camera"]) - ].max() - > camera_cfg_zero.max_distance - ) - self.assertTrue( - camera_none.data.output["distance_to_image_plane"][ - ~torch.isnan(camera_none.data.output["distance_to_image_plane"]) - ].max() - > camera_cfg_zero.max_distance - ) - - # zero clipping should result in zero values - self.assertTrue( - torch.all( - camera_zero.data.output["distance_to_camera"][ - torch.isinf(camera_none.data.output["distance_to_camera"]) - ] - == 0.0 - ) - ) - self.assertTrue( - torch.all( - camera_zero.data.output["distance_to_image_plane"][ - torch.isnan(camera_none.data.output["distance_to_image_plane"]) - ] - == 0.0 - ) - ) - self.assertTrue(camera_zero.data.output["distance_to_camera"].max() <= camera_cfg_zero.max_distance) - self.assertTrue(camera_zero.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.max_distance) - - # max clipping should result in max values - self.assertTrue( - torch.all( - camera_max.data.output["distance_to_camera"][torch.isinf(camera_none.data.output["distance_to_camera"])] - == camera_cfg_zero.max_distance - ) - ) - self.assertTrue( - torch.all( - camera_max.data.output["distance_to_image_plane"][ - torch.isnan(camera_none.data.output["distance_to_image_plane"]) - ] - == camera_cfg_zero.max_distance - ) - ) - self.assertTrue(camera_max.data.output["distance_to_camera"].max() <= camera_cfg_zero.max_distance) - self.assertTrue(camera_max.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.max_distance) - - def test_camera_init_offset(self): - """Test camera initialization with offset using different conventions.""" - # define the same offset in all conventions - # -- ROS convention - cam_cfg_offset_ros = copy.deepcopy(self.camera_cfg) - cam_cfg_offset_ros.offset = RayCasterCameraCfg.OffsetCfg( - pos=POSITION, - rot=QUAT_ROS, - convention="ros", - ) - prim_utils.create_prim("/World/CameraOffsetRos", "Xform") - cam_cfg_offset_ros.prim_path = "/World/CameraOffsetRos" - camera_ros = RayCasterCamera(cam_cfg_offset_ros) - # -- OpenGL convention - cam_cfg_offset_opengl = copy.deepcopy(self.camera_cfg) - cam_cfg_offset_opengl.offset = RayCasterCameraCfg.OffsetCfg( - pos=POSITION, - rot=QUAT_OPENGL, - convention="opengl", - ) - prim_utils.create_prim("/World/CameraOffsetOpengl", "Xform") - cam_cfg_offset_opengl.prim_path = "/World/CameraOffsetOpengl" - camera_opengl = RayCasterCamera(cam_cfg_offset_opengl) - # -- World convention - cam_cfg_offset_world = copy.deepcopy(self.camera_cfg) - cam_cfg_offset_world.offset = RayCasterCameraCfg.OffsetCfg( - pos=POSITION, - rot=QUAT_WORLD, - convention="world", - ) - prim_utils.create_prim("/World/CameraOffsetWorld", "Xform") - cam_cfg_offset_world.prim_path = "/World/CameraOffsetWorld" - camera_world = RayCasterCamera(cam_cfg_offset_world) - - # play sim - self.sim.reset() - - # update cameras - camera_world.update(self.dt) - camera_opengl.update(self.dt) - camera_ros.update(self.dt) - - # check that all transforms are set correctly - np.testing.assert_allclose(camera_ros.data.pos_w[0].cpu().numpy(), cam_cfg_offset_ros.offset.pos) - np.testing.assert_allclose(camera_opengl.data.pos_w[0].cpu().numpy(), cam_cfg_offset_opengl.offset.pos) - np.testing.assert_allclose(camera_world.data.pos_w[0].cpu().numpy(), cam_cfg_offset_world.offset.pos) - - # check if transform correctly set in output - np.testing.assert_allclose(camera_ros.data.pos_w[0].cpu().numpy(), cam_cfg_offset_ros.offset.pos, rtol=1e-5) - np.testing.assert_allclose(camera_ros.data.quat_w_ros[0].cpu().numpy(), QUAT_ROS, rtol=1e-5) - np.testing.assert_allclose(camera_ros.data.quat_w_opengl[0].cpu().numpy(), QUAT_OPENGL, rtol=1e-5) - np.testing.assert_allclose(camera_ros.data.quat_w_world[0].cpu().numpy(), QUAT_WORLD, rtol=1e-5) - - def test_camera_init_intrinsic_matrix(self): - """Test camera initialization from intrinsic matrix.""" - # get the first camera - camera_1 = RayCasterCamera(cfg=self.camera_cfg) - # get intrinsic matrix - self.sim.reset() - intrinsic_matrix = camera_1.data.intrinsic_matrices[0].cpu().flatten().tolist() - self.tearDown() - # reinit the first camera - self.setUp() - camera_1 = RayCasterCamera(cfg=self.camera_cfg) - # initialize from intrinsic matrix - intrinsic_camera_cfg = RayCasterCameraCfg( - prim_path="/World/Camera", - mesh_prim_paths=["/World/defaultGroundPlane"], - update_period=0, - offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), - debug_vis=False, - pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( - intrinsic_matrix=intrinsic_matrix, - height=self.camera_cfg.pattern_cfg.height, - width=self.camera_cfg.pattern_cfg.width, - focal_length=self.camera_cfg.pattern_cfg.focal_length, - ), - data_types=[ - "distance_to_image_plane", - ], - ) - camera_2 = RayCasterCamera(cfg=intrinsic_camera_cfg) - - # play sim - self.sim.reset() - self.sim.play() - - # update cameras - camera_1.update(self.dt) - camera_2.update(self.dt) - +DEBUG_PLOTS = False + + +def setup() -> tuple[sim_utils.SimulationContext, RayCasterCameraCfg, float]: + # Create a blank new stage + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=480, + width=640, + ) + camera_cfg = RayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=[ + "distance_to_image_plane", + ], + ) + # Create a new stage + sim_utils.create_new_stage() + # create xform because placement of camera directly under world is not supported + sim_utils.create_prim("/World/Camera", "Xform") + # Simulation time-step + dt = 0.01 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt) + sim = sim_utils.SimulationContext(sim_cfg) + # Ground-plane + mesh = make_plane(size=(100, 100), height=0.0, center_zero=True) + create_prim_from_mesh("/World/defaultGroundPlane", mesh) + # load stage + sim_utils.update_stage() + return sim, camera_cfg, dt + + +def teardown(sim: sim_utils.SimulationContext): + # Teardown + # close all the opened viewport from before. + rep.vp_manager.destroy_hydra_textures("Replicator") + # stop simulation + # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( + sim._timeline.stop() + # clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.fixture +def setup_sim(): + """Setup and teardown for each test.""" + sim, camera_cfg, dt = setup() + yield sim, camera_cfg, dt + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_camera_init(setup_sim): + """Test camera initialization.""" + sim, camera_cfg, dt = setup_sim + # Create camera + camera = RayCasterCamera(cfg=camera_cfg) + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check buffers that exist and have correct shapes + assert camera.data.pos_w.shape == (1, 3) + assert camera.data.quat_w_ros.shape == (1, 4) + assert camera.data.quat_w_world.shape == (1, 4) + assert camera.data.quat_w_opengl.shape == (1, 4) + assert camera.data.intrinsic_matrices.shape == (1, 3, 3) + assert camera.data.image_shape == (camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width) + assert camera.data.info == [{camera_cfg.data_types[0]: None}] + # Simulate physics + for _ in range(10): + sim.step() + camera.update(dt) # check image data - torch.testing.assert_close( - camera_1.data.output["distance_to_image_plane"], - camera_2.data.output["distance_to_image_plane"], - ) - # check that both intrinsic matrices are the same - torch.testing.assert_close( - camera_1.data.intrinsic_matrices[0], - camera_2.data.intrinsic_matrices[0], - ) - - def test_multi_camera_init(self): - """Test multi-camera initialization.""" - # create two cameras with different prim paths - # -- camera 1 - cam_cfg_1 = copy.deepcopy(self.camera_cfg) - cam_cfg_1.prim_path = "/World/Camera_1" - prim_utils.create_prim("/World/Camera_1", "Xform") - # Create camera - cam_1 = RayCasterCamera(cam_cfg_1) - # -- camera 2 - cam_cfg_2 = copy.deepcopy(self.camera_cfg) - cam_cfg_2.prim_path = "/World/Camera_2" - prim_utils.create_prim("/World/Camera_2", "Xform") - cam_2 = RayCasterCamera(cam_cfg_2) - - # check that the loaded meshes are equal - self.assertTrue(cam_1.meshes == cam_2.meshes) - - # play sim - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - cam_1.update(self.dt) - cam_2.update(self.dt) - # check image data - for cam in [cam_1, cam_2]: - for im_data in cam.data.output.values(): - self.assertEqual( - im_data.shape, (1, self.camera_cfg.pattern_cfg.height, self.camera_cfg.pattern_cfg.width, 1) - ) - - def test_camera_set_world_poses(self): - """Test camera function to set specific world pose.""" - camera = RayCasterCamera(self.camera_cfg) - # play sim - self.sim.reset() - - # convert to torch tensors - position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) - orientation = torch.tensor([QUAT_WORLD], dtype=torch.float32, device=camera.device) - # set new pose - camera.set_world_poses(position.clone(), orientation.clone(), convention="world") - - # check if transform correctly set in output - torch.testing.assert_close(camera.data.pos_w, position) - torch.testing.assert_close(camera.data.quat_w_world, orientation) - - def test_camera_set_world_poses_from_view(self): - """Test camera function to set specific world pose from view.""" - camera = RayCasterCamera(self.camera_cfg) - # play sim - self.sim.reset() - - # convert to torch tensors - eyes = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) - targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) - quat_ros_gt = torch.tensor([QUAT_ROS], dtype=torch.float32, device=camera.device) - # set new pose - camera.set_world_poses_from_view(eyes.clone(), targets.clone()) - - # check if transform correctly set in output - torch.testing.assert_close(camera.data.pos_w, eyes) - torch.testing.assert_close(camera.data.quat_w_ros, quat_ros_gt) - - def test_intrinsic_matrix(self): - """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.pattern_cfg.height = 240 - camera_cfg.pattern_cfg.width = 320 - camera = RayCasterCamera(camera_cfg) - # play sim - self.sim.reset() - # Desired properties (obtained from realsense camera at 320x240 resolution) - rs_intrinsic_matrix = [229.31640625, 0.0, 164.810546875, 0.0, 229.826171875, 122.1650390625, 0.0, 0.0, 1.0] - rs_intrinsic_matrix = torch.tensor(rs_intrinsic_matrix, device=camera.device).reshape(3, 3).unsqueeze(0) - # Set matrix into simulator - camera.set_intrinsic_matrices(rs_intrinsic_matrix.clone()) - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # Check that matrix is correct - torch.testing.assert_close(rs_intrinsic_matrix, camera.data.intrinsic_matrices) - - def test_throughput(self): - """Checks that the single camera gets created properly with a rig.""" - # Create directory temp dir to dump the results - file_dir = os.path.dirname(os.path.realpath(__file__)) - temp_dir = os.path.join(file_dir, "output", "camera", "throughput") - os.makedirs(temp_dir, exist_ok=True) - # Create replicator writer - rep_writer = rep.BasicWriter(output_dir=temp_dir, frame_padding=3) - # create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.pattern_cfg.height = 480 - camera_cfg.pattern_cfg.width = 640 - camera = RayCasterCamera(camera_cfg) - - # Play simulator - self.sim.reset() - - # Set camera pose - eyes = torch.tensor([[2.5, 2.5, 2.5]], dtype=torch.float32, device=camera.device) - targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) - camera.set_world_poses_from_view(eyes, targets) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - # Simulate physics - for _ in range(5): - # perform rendering - self.sim.step() - # update camera - with Timer(f"Time taken for updating camera with shape {camera.image_shape}"): - camera.update(self.dt) - # Save images - with Timer(f"Time taken for writing data with shape {camera.image_shape} "): - # Pack data back into replicator format to save them using its writer - rep_output = {"annotators": {}} - camera_data = convert_dict_to_backend({k: v[0] for k, v in camera.data.output.items()}, backend="numpy") - for key, data, info in zip(camera_data.keys(), camera_data.values(), camera.data.info[0].values()): - if info is not None: - rep_output["annotators"][key] = {"render_product": {"data": data, **info}} - else: - rep_output["annotators"][key] = {"render_product": {"data": data}} - # Save images - rep_output["trigger_outputs"] = {"on_time": camera.frame[0]} - rep_writer.write(rep_output) - print("----------------------------------------") - # Check image data - for im_data in camera.data.output.values(): - self.assertEqual(im_data.shape, (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1)) - - def test_output_equal_to_usdcamera(self): - camera_pattern_cfg = patterns.PinholeCameraPatternCfg( - focal_length=24.0, - horizontal_aperture=20.955, - height=240, - width=320, - ) - prim_utils.create_prim("/World/Camera_warp", "Xform") - camera_cfg_warp = RayCasterCameraCfg( - prim_path="/World/Camera", - mesh_prim_paths=["/World/defaultGroundPlane"], - update_period=0, - offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), - debug_vis=False, - pattern_cfg=camera_pattern_cfg, - data_types=["distance_to_image_plane", "distance_to_camera", "normals"], - ) - - camera_warp = RayCasterCamera(camera_cfg_warp) - - # create usd camera - camera_cfg_usd = CameraCfg( - height=240, - width=320, - prim_path="/World/Camera_usd", - update_period=0, - data_types=["distance_to_image_plane", "distance_to_camera", "normals"], - spawn=PinholeCameraCfg( - focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-4, 1.0e5) - ), - ) - camera_usd = Camera(camera_cfg_usd) - - # play sim - self.sim.reset() - self.sim.play() - - # convert to torch tensors - eyes = torch.tensor([[2.5, 2.5, 4.5]], dtype=torch.float32, device=camera_warp.device) - targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera_warp.device) - # set views - camera_warp.set_world_poses_from_view(eyes, targets) - camera_usd.set_world_poses_from_view(eyes, targets) - - # perform steps - for _ in range(5): - self.sim.step() - + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1) + + # check the camera reset + camera.reset() + assert torch.all(camera.frame == 0) + # Simulate physics + for _ in range(10): + sim.step() + camera.update(dt) + camera.reset(env_ids=[0]) + assert camera.frame[0] == 0 + + +@pytest.mark.isaacsim_ci +def test_camera_resolution(setup_sim): + """Test camera resolution is correctly set.""" + sim, camera_cfg, dt = setup_sim + # Create camera + camera = RayCasterCamera(cfg=camera_cfg) + # Play sim + sim.reset() + camera.update(dt) + # access image data and compare shapes + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1) + + +@pytest.mark.isaacsim_ci +def test_depth_clipping(setup_sim): + """Test depth clipping. + + .. note:: + + This test is the same for all camera models to enforce the same clipping behavior. + """ + sim, camera_cfg, dt = setup_sim + sim_utils.create_prim("/World/CameraZero", "Xform") + sim_utils.create_prim("/World/CameraNone", "Xform") + sim_utils.create_prim("/World/CameraMax", "Xform") + + # get camera cfgs + camera_cfg_zero = RayCasterCameraCfg( + prim_path="/World/CameraZero", + mesh_prim_paths=["/World/defaultGroundPlane"], + offset=RayCasterCameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(0.9914449, 0.0, 0.1305, 0.0), convention="world"), + pattern_cfg=patterns.PinholeCameraPatternCfg().from_intrinsic_matrix( + focal_length=38.0, + intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], + height=540, + width=960, + ), + max_distance=10.0, + data_types=["distance_to_image_plane", "distance_to_camera"], + depth_clipping_behavior="zero", + ) + camera_zero = RayCasterCamera(camera_cfg_zero) + + camera_cfg_none = copy.deepcopy(camera_cfg_zero) + camera_cfg_none.prim_path = "/World/CameraNone" + camera_cfg_none.depth_clipping_behavior = "none" + camera_none = RayCasterCamera(camera_cfg_none) + + camera_cfg_max = copy.deepcopy(camera_cfg_zero) + camera_cfg_max.prim_path = "/World/CameraMax" + camera_cfg_max.depth_clipping_behavior = "max" + camera_max = RayCasterCamera(camera_cfg_max) + + # Play sim + sim.reset() + + camera_zero.update(dt) + camera_none.update(dt) + camera_max.update(dt) + + # none clipping should contain inf values + assert torch.isinf(camera_none.data.output["distance_to_camera"]).any() + assert torch.isnan(camera_none.data.output["distance_to_image_plane"]).any() + assert ( + camera_none.data.output["distance_to_camera"][~torch.isinf(camera_none.data.output["distance_to_camera"])].max() + > camera_cfg_zero.max_distance + ) + assert ( + camera_none.data.output["distance_to_image_plane"][ + ~torch.isnan(camera_none.data.output["distance_to_image_plane"]) + ].max() + > camera_cfg_zero.max_distance + ) + + # zero clipping should result in zero values + assert torch.all( + camera_zero.data.output["distance_to_camera"][torch.isinf(camera_none.data.output["distance_to_camera"])] == 0.0 + ) + assert torch.all( + camera_zero.data.output["distance_to_image_plane"][ + torch.isnan(camera_none.data.output["distance_to_image_plane"]) + ] + == 0.0 + ) + assert camera_zero.data.output["distance_to_camera"].max() <= camera_cfg_zero.max_distance + assert camera_zero.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.max_distance + + # max clipping should result in max values + assert torch.all( + camera_max.data.output["distance_to_camera"][torch.isinf(camera_none.data.output["distance_to_camera"])] + == camera_cfg_zero.max_distance + ) + assert torch.all( + camera_max.data.output["distance_to_image_plane"][ + torch.isnan(camera_none.data.output["distance_to_image_plane"]) + ] + == camera_cfg_zero.max_distance + ) + assert camera_max.data.output["distance_to_camera"].max() <= camera_cfg_zero.max_distance + assert camera_max.data.output["distance_to_image_plane"].max() <= camera_cfg_zero.max_distance + + +@pytest.mark.isaacsim_ci +def test_camera_init_offset(setup_sim): + """Test camera initialization with offset using different conventions.""" + sim, camera_cfg, dt = setup_sim + # define the same offset in all conventions + # -- ROS convention + cam_cfg_offset_ros = copy.deepcopy(camera_cfg) + cam_cfg_offset_ros.offset = RayCasterCameraCfg.OffsetCfg( + pos=(POSITION[0], POSITION[1], POSITION[2]), + rot=(QUAT_ROS[0], QUAT_ROS[1], QUAT_ROS[2], QUAT_ROS[3]), + convention="ros", + ) + sim_utils.create_prim("/World/CameraOffsetRos", "Xform") + cam_cfg_offset_ros.prim_path = "/World/CameraOffsetRos" + camera_ros = RayCasterCamera(cam_cfg_offset_ros) + # -- OpenGL convention + cam_cfg_offset_opengl = copy.deepcopy(camera_cfg) + cam_cfg_offset_opengl.offset = RayCasterCameraCfg.OffsetCfg( + pos=(POSITION[0], POSITION[1], POSITION[2]), + rot=(QUAT_OPENGL[0], QUAT_OPENGL[1], QUAT_OPENGL[2], QUAT_OPENGL[3]), + convention="opengl", + ) + sim_utils.create_prim("/World/CameraOffsetOpengl", "Xform") + cam_cfg_offset_opengl.prim_path = "/World/CameraOffsetOpengl" + camera_opengl = RayCasterCamera(cam_cfg_offset_opengl) + # -- World convention + cam_cfg_offset_world = copy.deepcopy(camera_cfg) + cam_cfg_offset_world.offset = RayCasterCameraCfg.OffsetCfg( + pos=(POSITION[0], POSITION[1], POSITION[2]), + rot=(QUAT_WORLD[0], QUAT_WORLD[1], QUAT_WORLD[2], QUAT_WORLD[3]), + convention="world", + ) + sim_utils.create_prim("/World/CameraOffsetWorld", "Xform") + cam_cfg_offset_world.prim_path = "/World/CameraOffsetWorld" + camera_world = RayCasterCamera(cam_cfg_offset_world) + + # play sim + sim.reset() + + # update cameras + camera_world.update(dt) + camera_opengl.update(dt) + camera_ros.update(dt) + + # check that all transforms are set correctly + np.testing.assert_allclose(camera_ros.data.pos_w[0].cpu().numpy(), cam_cfg_offset_ros.offset.pos) + np.testing.assert_allclose(camera_opengl.data.pos_w[0].cpu().numpy(), cam_cfg_offset_opengl.offset.pos) + np.testing.assert_allclose(camera_world.data.pos_w[0].cpu().numpy(), cam_cfg_offset_world.offset.pos) + + # check if transform correctly set in output + np.testing.assert_allclose(camera_ros.data.pos_w[0].cpu().numpy(), cam_cfg_offset_ros.offset.pos, rtol=1e-5) + np.testing.assert_allclose(camera_ros.data.quat_w_ros[0].cpu().numpy(), QUAT_ROS, rtol=1e-5) + np.testing.assert_allclose(camera_ros.data.quat_w_opengl[0].cpu().numpy(), QUAT_OPENGL, rtol=1e-5) + np.testing.assert_allclose(camera_ros.data.quat_w_world[0].cpu().numpy(), QUAT_WORLD, rtol=1e-5) + + +@pytest.mark.isaacsim_ci +def test_camera_init_intrinsic_matrix(setup_sim): + """Test camera initialization from intrinsic matrix.""" + sim, camera_cfg, dt = setup_sim + # get the first camera + camera_1 = RayCasterCamera(cfg=camera_cfg) + # get intrinsic matrix + sim.reset() + intrinsic_matrix = camera_1.data.intrinsic_matrices[0].cpu().flatten().tolist() + teardown(sim) + # reinit the first camera + sim, camera_cfg, dt = setup() + camera_1 = RayCasterCamera(cfg=camera_cfg) + # initialize from intrinsic matrix + intrinsic_camera_cfg = RayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), + debug_vis=False, + pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsic_matrix, + height=camera_cfg.pattern_cfg.height, + width=camera_cfg.pattern_cfg.width, + focal_length=camera_cfg.pattern_cfg.focal_length, + ), + data_types=[ + "distance_to_image_plane", + ], + ) + camera_2 = RayCasterCamera(cfg=intrinsic_camera_cfg) + + # play sim + sim.reset() + sim.play() + + # update cameras + camera_1.update(dt) + camera_2.update(dt) + + # check image data + torch.testing.assert_close( + camera_1.data.output["distance_to_image_plane"], + camera_2.data.output["distance_to_image_plane"], + ) + # check that both intrinsic matrices are the same + torch.testing.assert_close( + camera_1.data.intrinsic_matrices[0], + camera_2.data.intrinsic_matrices[0], + ) + + +@pytest.mark.isaacsim_ci +def test_multi_camera_init(setup_sim): + """Test multi-camera initialization.""" + sim, camera_cfg, dt = setup_sim + # create two cameras with different prim paths + # -- camera 1 + cam_cfg_1 = copy.deepcopy(camera_cfg) + cam_cfg_1.prim_path = "/World/Camera_1" + sim_utils.create_prim("/World/Camera_1", "Xform") + # Create camera + cam_1 = RayCasterCamera(cam_cfg_1) + # -- camera 2 + cam_cfg_2 = copy.deepcopy(camera_cfg) + cam_cfg_2.prim_path = "/World/Camera_2" + sim_utils.create_prim("/World/Camera_2", "Xform") + cam_2 = RayCasterCamera(cam_cfg_2) + + # check that the loaded meshes are equal + assert cam_1.meshes == cam_2.meshes + + # play sim + sim.reset() + + # Simulate for a few steps + for _ in range(5): + sim.step() + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() # update camera - camera_usd.update(self.dt) - camera_warp.update(self.dt) - - # check the intrinsic matrices - torch.testing.assert_close( - camera_usd.data.intrinsic_matrices, - camera_warp.data.intrinsic_matrices, - ) - - # check the apertures - torch.testing.assert_close( - camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), - camera_cfg_warp.pattern_cfg.horizontal_aperture, - ) - torch.testing.assert_close( - camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(), - ( - camera_cfg_warp.pattern_cfg.horizontal_aperture - * camera_cfg_warp.pattern_cfg.height - / camera_cfg_warp.pattern_cfg.width - ), - ) - + cam_1.update(dt) + cam_2.update(dt) # check image data - torch.testing.assert_close( - camera_usd.data.output["distance_to_image_plane"], - camera_warp.data.output["distance_to_image_plane"], - ) - torch.testing.assert_close( - camera_usd.data.output["distance_to_camera"], - camera_warp.data.output["distance_to_camera"], - atol=5e-5, - rtol=5e-6, - ) - - # check normals - # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case - torch.testing.assert_close( - camera_usd.data.output["normals"][..., :3], - camera_warp.data.output["normals"], - rtol=1e-5, - atol=1e-4, - ) - - def test_output_equal_to_usdcamera_offset(self): - offset_rot = [-0.1251, 0.3617, 0.8731, -0.3020] - - camera_pattern_cfg = patterns.PinholeCameraPatternCfg( - focal_length=24.0, - horizontal_aperture=20.955, - height=240, - width=320, - ) - prim_utils.create_prim("/World/Camera_warp", "Xform") - camera_cfg_warp = RayCasterCameraCfg( - prim_path="/World/Camera", - mesh_prim_paths=["/World/defaultGroundPlane"], - update_period=0, - offset=RayCasterCameraCfg.OffsetCfg(pos=(2.5, 2.5, 4.0), rot=offset_rot, convention="ros"), - debug_vis=False, - pattern_cfg=camera_pattern_cfg, - data_types=["distance_to_image_plane", "distance_to_camera", "normals"], - ) - - camera_warp = RayCasterCamera(camera_cfg_warp) - - # create usd camera - camera_cfg_usd = CameraCfg( - height=240, - width=320, - prim_path="/World/Camera_usd", - update_period=0, - data_types=["distance_to_image_plane", "distance_to_camera", "normals"], - spawn=PinholeCameraCfg( - focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-6, 1.0e5) - ), - offset=CameraCfg.OffsetCfg(pos=(2.5, 2.5, 4.0), rot=offset_rot, convention="ros"), - ) - camera_usd = Camera(camera_cfg_usd) - - # play sim - self.sim.reset() - self.sim.play() - - # perform steps - for _ in range(5): - self.sim.step() - + for cam in [cam_1, cam_2]: + for im_data in cam.data.output.values(): + assert im_data.shape == (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1) + + +@pytest.mark.isaacsim_ci +def test_camera_set_world_poses(setup_sim): + """Test camera function to set specific world pose.""" + sim, camera_cfg, dt = setup_sim + camera = RayCasterCamera(camera_cfg) + # play sim + sim.reset() + + # convert to torch tensors + position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + orientation = torch.tensor([QUAT_WORLD], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_world_poses(position.clone(), orientation.clone(), convention="world") + + # check if transform correctly set in output + torch.testing.assert_close(camera.data.pos_w, position) + torch.testing.assert_close(camera.data.quat_w_world, orientation) + + +@pytest.mark.isaacsim_ci +def test_camera_set_world_poses_from_view(setup_sim): + """Test camera function to set specific world pose from view.""" + sim, camera_cfg, dt = setup_sim + camera = RayCasterCamera(camera_cfg) + # play sim + sim.reset() + + # convert to torch tensors + eyes = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + quat_ros_gt = torch.tensor([QUAT_ROS], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_world_poses_from_view(eyes.clone(), targets.clone()) + + # check if transform correctly set in output + torch.testing.assert_close(camera.data.pos_w, eyes) + torch.testing.assert_close(camera.data.quat_w_ros, quat_ros_gt) + + +@pytest.mark.isaacsim_ci +def test_intrinsic_matrix(setup_sim): + """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" + sim, camera_cfg, dt = setup_sim + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.pattern_cfg.height = 240 + camera_cfg.pattern_cfg.width = 320 + camera = RayCasterCamera(camera_cfg) + # play sim + sim.reset() + # Desired properties (obtained from realsense camera at 320x240 resolution) + rs_intrinsic_matrix = [229.31640625, 0.0, 164.810546875, 0.0, 229.826171875, 122.1650390625, 0.0, 0.0, 1.0] + rs_intrinsic_matrix = torch.tensor(rs_intrinsic_matrix, device=camera.device).reshape(3, 3).unsqueeze(0) + # Set matrix into simulator + camera.set_intrinsic_matrices(rs_intrinsic_matrix.clone()) + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() # update camera - camera_usd.update(self.dt) - camera_warp.update(self.dt) - - # check image data - torch.testing.assert_close( - camera_usd.data.output["distance_to_image_plane"], - camera_warp.data.output["distance_to_image_plane"], - rtol=1e-3, - atol=1e-5, - ) - torch.testing.assert_close( - camera_usd.data.output["distance_to_camera"], - camera_warp.data.output["distance_to_camera"], - rtol=1e-3, - atol=1e-5, - ) - - # check normals - # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case - torch.testing.assert_close( - camera_usd.data.output["normals"][..., :3], - camera_warp.data.output["normals"], - rtol=1e-5, - atol=1e-4, - ) - - def test_output_equal_to_usdcamera_prim_offset(self): - """Test that the output of the ray caster camera is equal to the output of the usd camera when both are placed - under an XForm prim that is translated and rotated from the world origin - .""" - offset_rot = [-0.1251, 0.3617, 0.8731, -0.3020] - - # gf quat - gf_quatf = Gf.Quatd() - gf_quatf.SetReal(QUAT_OPENGL[0]) - gf_quatf.SetImaginary(tuple(QUAT_OPENGL[1:])) - - camera_pattern_cfg = patterns.PinholeCameraPatternCfg( - focal_length=24.0, - horizontal_aperture=20.955, - height=240, - width=320, - ) - prim_raycast_cam = prim_utils.create_prim("/World/Camera_warp", "Xform") - prim_raycast_cam.GetAttribute("xformOp:translate").Set(tuple(POSITION)) - prim_raycast_cam.GetAttribute("xformOp:orient").Set(gf_quatf) - - camera_cfg_warp = RayCasterCameraCfg( - prim_path="/World/Camera_warp", - mesh_prim_paths=["/World/defaultGroundPlane"], - update_period=0, - offset=RayCasterCameraCfg.OffsetCfg(pos=(0, 0, 2.0), rot=offset_rot, convention="ros"), - debug_vis=False, - pattern_cfg=camera_pattern_cfg, - data_types=["distance_to_image_plane", "distance_to_camera", "normals"], - ) - - camera_warp = RayCasterCamera(camera_cfg_warp) - - # create usd camera - camera_cfg_usd = CameraCfg( - height=240, - width=320, - prim_path="/World/Camera_usd/camera", - update_period=0, - data_types=["distance_to_image_plane", "distance_to_camera", "normals"], - spawn=PinholeCameraCfg( - focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-6, 1.0e5) - ), - offset=CameraCfg.OffsetCfg(pos=(0, 0, 2.0), rot=offset_rot, convention="ros"), - ) - prim_usd = prim_utils.create_prim("/World/Camera_usd", "Xform") - prim_usd.GetAttribute("xformOp:translate").Set(tuple(POSITION)) - prim_usd.GetAttribute("xformOp:orient").Set(gf_quatf) - - camera_usd = Camera(camera_cfg_usd) - - # play sim - self.sim.reset() - self.sim.play() - - # perform steps - for _ in range(5): - self.sim.step() - + camera.update(dt) + # Check that matrix is correct + torch.testing.assert_close(rs_intrinsic_matrix, camera.data.intrinsic_matrices) + + +@pytest.mark.isaacsim_ci +def test_throughput(setup_sim): + """Checks that the single camera gets created properly with a rig.""" + sim, camera_cfg, dt = setup_sim + # Create directory temp dir to dump the results + file_dir = os.path.dirname(os.path.realpath(__file__)) + temp_dir = os.path.join(file_dir, "output", "camera", "throughput") + os.makedirs(temp_dir, exist_ok=True) + # Create replicator writer + rep_writer = rep.BasicWriter(output_dir=temp_dir, frame_padding=3) + # create camera + camera_cfg.pattern_cfg.height = 480 + camera_cfg.pattern_cfg.width = 640 + camera = RayCasterCamera(camera_cfg) + + # Play simulator + sim.reset() + + # Set camera pose + eyes = torch.tensor([[2.5, 2.5, 2.5]], dtype=torch.float32, device=camera.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + camera.set_world_poses_from_view(eyes, targets) + + # Simulate for a few steps + for _ in range(5): + sim.step() + # Simulate physics + for _ in range(5): + # perform rendering + sim.step() # update camera - camera_usd.update(self.dt) - camera_warp.update(self.dt) - - # check if pos and orientation are correct - torch.testing.assert_close(camera_warp.data.pos_w[0], camera_usd.data.pos_w[0]) - torch.testing.assert_close(camera_warp.data.quat_w_ros[0], camera_usd.data.quat_w_ros[0]) - - # check image data - torch.testing.assert_close( - camera_usd.data.output["distance_to_image_plane"], - camera_warp.data.output["distance_to_image_plane"], - rtol=1e-3, - atol=1e-5, - ) - torch.testing.assert_close( - camera_usd.data.output["distance_to_camera"], - camera_warp.data.output["distance_to_camera"], - rtol=4e-6, - atol=2e-5, - ) - - # check normals - # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case - torch.testing.assert_close( - camera_usd.data.output["normals"][..., :3], - camera_warp.data.output["normals"], - rtol=1e-5, - atol=1e-4, - ) + with Timer(f"Time taken for updating camera with shape {camera.image_shape}"): + camera.update(dt) + # Save images + with Timer(f"Time taken for writing data with shape {camera.image_shape} "): + # Pack data back into replicator format to save them using its writer + rep_output = {"annotators": {}} + camera_data = convert_dict_to_backend({k: v[0] for k, v in camera.data.output.items()}, backend="numpy") + for key, data, info in zip(camera_data.keys(), camera_data.values(), camera.data.info[0].values()): + if info is not None: + rep_output["annotators"][key] = {"render_product": {"data": data, **info}} + else: + rep_output["annotators"][key] = {"render_product": {"data": data}} + # Save images + rep_output["trigger_outputs"] = {"on_time": camera.frame[0]} + rep_writer.write(rep_output) + print("----------------------------------------") + # Check image data + for im_data in camera.data.output.values(): + assert im_data.shape == (1, camera_cfg.pattern_cfg.height, camera_cfg.pattern_cfg.width, 1) + + +@pytest.mark.isaacsim_ci +def test_output_equal_to_usdcamera(setup_sim): + sim, camera_cfg, dt = setup_sim + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=240, + width=320, + ) + sim_utils.create_prim("/World/Camera_warp", "Xform") + camera_cfg_warp = RayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + ) + + camera_warp = RayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=240, + width=320, + prim_path="/World/Camera_usd", + update_period=0, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + spawn=PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-4, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), + ) + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # convert to torch tensors + eyes = torch.tensor([[2.5, 2.5, 4.5]], dtype=torch.float32, device=camera_warp.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera_warp.device) + # set views + camera_warp.set_world_poses_from_view(eyes, targets) + camera_usd.set_world_poses_from_view(eyes, targets) + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # check the intrinsic matrices + torch.testing.assert_close( + camera_usd.data.intrinsic_matrices, + camera_warp.data.intrinsic_matrices, + ) + + # check the apertures + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), + camera_cfg_warp.pattern_cfg.horizontal_aperture, + ) + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(), + ( + camera_cfg_warp.pattern_cfg.horizontal_aperture + * camera_cfg_warp.pattern_cfg.height + / camera_cfg_warp.pattern_cfg.width + ), + ) + + # check image data + torch.testing.assert_close( + camera_usd.data.output["distance_to_image_plane"], + camera_warp.data.output["distance_to_image_plane"], + ) + torch.testing.assert_close( + camera_usd.data.output["distance_to_camera"], + camera_warp.data.output["distance_to_camera"], + atol=5e-5, + rtol=5e-6, + ) + + # check normals + # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case + torch.testing.assert_close( + camera_usd.data.output["normals"][..., :3], + camera_warp.data.output["normals"], + rtol=1e-5, + atol=1e-4, + ) + + +@pytest.mark.isaacsim_ci +def test_output_equal_to_usdcamera_offset(setup_sim): + sim, camera_cfg, dt = setup_sim + offset_rot = [-0.1251, 0.3617, 0.8731, -0.3020] + + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=240, + width=320, + ) + sim_utils.create_prim("/World/Camera_warp", "Xform") + camera_cfg_warp = RayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=RayCasterCameraCfg.OffsetCfg(pos=(2.5, 2.5, 4.0), rot=tuple(offset_rot), convention="ros"), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + ) + + camera_warp = RayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=240, + width=320, + prim_path="/World/Camera_usd", + update_period=0, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + spawn=PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-6, 1.0e5) + ), + offset=CameraCfg.OffsetCfg( + pos=(2.5, 2.5, 4.0), rot=(offset_rot[0], offset_rot[1], offset_rot[2], offset_rot[3]), convention="ros" + ), + ) + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # check image data + torch.testing.assert_close( + camera_usd.data.output["distance_to_image_plane"], + camera_warp.data.output["distance_to_image_plane"], + rtol=1e-3, + atol=1e-5, + ) + torch.testing.assert_close( + camera_usd.data.output["distance_to_camera"], + camera_warp.data.output["distance_to_camera"], + rtol=1e-3, + atol=1e-5, + ) + + # check normals + # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case + torch.testing.assert_close( + camera_usd.data.output["normals"][..., :3], + camera_warp.data.output["normals"], + rtol=1e-5, + atol=1e-4, + ) + + +@pytest.mark.isaacsim_ci +def test_output_equal_to_usdcamera_prim_offset(setup_sim): + """Test that the output of the ray caster camera is equal to the output of the usd camera when both are placed + under an XForm prim that is translated and rotated from the world origin + .""" + sim, camera_cfg, dt = setup_sim + offset_rot = (-0.1251, 0.3617, 0.8731, -0.3020) + + # gf quat + gf_quatf = Gf.Quatd() + gf_quatf.SetReal(QUAT_OPENGL[0]) + gf_quatf.SetImaginary(tuple(QUAT_OPENGL[1:])) + + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=24.0, + horizontal_aperture=20.955, + height=240, + width=320, + ) + prim_raycast_cam = sim_utils.create_prim("/World/Camera_warp", "Xform") + prim_raycast_cam.GetAttribute("xformOp:translate").Set(tuple(POSITION)) + prim_raycast_cam.GetAttribute("xformOp:orient").Set(gf_quatf) + + camera_cfg_warp = RayCasterCameraCfg( + prim_path="/World/Camera_warp", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=RayCasterCameraCfg.OffsetCfg(pos=(0, 0, 2.0), rot=offset_rot, convention="ros"), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + ) + + camera_warp = RayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=240, + width=320, + prim_path="/World/Camera_usd/camera", + update_period=0, + data_types=["distance_to_image_plane", "distance_to_camera", "normals"], + spawn=PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-6, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(0, 0, 2.0), rot=offset_rot, convention="ros"), + update_latest_camera_pose=True, + ) + prim_usd = sim_utils.create_prim("/World/Camera_usd", "Xform") + prim_usd.GetAttribute("xformOp:translate").Set(tuple(POSITION)) + prim_usd.GetAttribute("xformOp:orient").Set(gf_quatf) + + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # check if pos and orientation are correct + torch.testing.assert_close(camera_warp.data.pos_w[0], camera_usd.data.pos_w[0]) + torch.testing.assert_close(camera_warp.data.quat_w_ros[0], camera_usd.data.quat_w_ros[0]) + + # check image data + torch.testing.assert_close( + camera_usd.data.output["distance_to_image_plane"], + camera_warp.data.output["distance_to_image_plane"], + rtol=1e-3, + atol=1e-5, + ) + torch.testing.assert_close( + camera_usd.data.output["distance_to_camera"], + camera_warp.data.output["distance_to_camera"], + rtol=4e-6, + atol=2e-5, + ) + + # check normals + # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case + torch.testing.assert_close( + camera_usd.data.output["normals"][..., :3], + camera_warp.data.output["normals"], + rtol=1e-5, + atol=1e-4, + ) + + +@pytest.mark.parametrize("focal_length", [0.193, 1.93, 19.3]) +@pytest.mark.isaacsim_ci +def test_output_equal_to_usd_camera_intrinsics(setup_sim, focal_length): + """ + Test that the output of the ray caster camera and usd camera are the same when both are + initialized with the same intrinsic matrix. + """ - def test_output_equal_to_usd_camera_intrinsics(self): - """ - Test that the output of the ray caster camera and usd camera are the same when both are - initialized with the same intrinsic matrix. - """ - - # create cameras - offset_rot = [-0.1251, 0.3617, 0.8731, -0.3020] - offset_pos = (2.5, 2.5, 4.0) - intrinsics = [380.0831, 0.0, 467.7916, 0.0, 380.0831, 262.0532, 0.0, 0.0, 1.0] - prim_utils.create_prim("/World/Camera_warp", "Xform") - # get camera cfgs - camera_warp_cfg = RayCasterCameraCfg( - prim_path="/World/Camera_warp", - mesh_prim_paths=["/World/defaultGroundPlane"], - offset=RayCasterCameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), - debug_vis=False, - pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( - intrinsic_matrix=intrinsics, - height=540, - width=960, - focal_length=38.0, - ), - max_distance=20.0, - data_types=["distance_to_image_plane"], - ) - camera_usd_cfg = CameraCfg( - prim_path="/World/Camera_usd", - offset=CameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), - spawn=PinholeCameraCfg.from_intrinsic_matrix( - intrinsic_matrix=intrinsics, - height=540, - width=960, - clipping_range=(0.01, 20), - focal_length=38.0, - ), + sim, camera_cfg, dt = setup_sim + # create cameras + offset_rot = (-0.1251, 0.3617, 0.8731, -0.3020) + offset_pos = (2.5, 2.5, 4.0) + intrinsics = [380.0831, 0.0, 480.0, 0.0, 380.0831, 270.0, 0.0, 0.0, 1.0] + sim_utils.create_prim("/World/Camera_warp", "Xform") + # get camera cfgs + camera_warp_cfg = RayCasterCameraCfg( + prim_path="/World/Camera_warp", + mesh_prim_paths=["/World/defaultGroundPlane"], + offset=RayCasterCameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), + debug_vis=False, + pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsics, height=540, width=960, - data_types=["distance_to_image_plane"], - ) - - # set aperture offsets to 0, as currently not supported for usd camera - camera_warp_cfg.pattern_cfg.horizontal_aperture_offset = 0 - camera_warp_cfg.pattern_cfg.vertical_aperture_offset = 0 - camera_usd_cfg.spawn.horizontal_aperture_offset = 0 - camera_usd_cfg.spawn.vertical_aperture_offset = 0 - # init cameras - camera_warp = RayCasterCamera(camera_warp_cfg) - camera_usd = Camera(camera_usd_cfg) - - # play sim - self.sim.reset() - self.sim.play() - - # perform steps - for _ in range(5): - self.sim.step() - - # update camera - camera_usd.update(self.dt) - camera_warp.update(self.dt) - - # filter nan and inf from output - cam_warp_output = camera_warp.data.output["distance_to_image_plane"].clone() - cam_usd_output = camera_usd.data.output["distance_to_image_plane"].clone() - cam_warp_output[torch.isnan(cam_warp_output)] = 0 - cam_warp_output[torch.isinf(cam_warp_output)] = 0 - cam_usd_output[torch.isnan(cam_usd_output)] = 0 - cam_usd_output[torch.isinf(cam_usd_output)] = 0 - - # check that both have the same intrinsic matrices - torch.testing.assert_close(camera_warp.data.intrinsic_matrices[0], camera_usd.data.intrinsic_matrices[0]) - - # check the apertures - torch.testing.assert_close( - camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), - camera_warp_cfg.pattern_cfg.horizontal_aperture, - ) - torch.testing.assert_close( - camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(), - camera_warp_cfg.pattern_cfg.vertical_aperture, - ) - - # check image data + focal_length=focal_length, + ), + depth_clipping_behavior="max", + max_distance=20.0, + data_types=["distance_to_image_plane"], + ) + camera_usd_cfg = CameraCfg( + prim_path="/World/Camera_usd", + offset=CameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), + spawn=PinholeCameraCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsics, + height=540, + width=960, + clipping_range=(0.01, 20), + focal_length=focal_length, + ), + height=540, + width=960, + depth_clipping_behavior="max", + data_types=["distance_to_image_plane"], + ) + + # set aperture offsets to 0, as currently not supported for usd camera + camera_warp_cfg.pattern_cfg.horizontal_aperture_offset = 0 + camera_warp_cfg.pattern_cfg.vertical_aperture_offset = 0 + camera_usd_cfg.spawn.horizontal_aperture_offset = 0 + camera_usd_cfg.spawn.vertical_aperture_offset = 0 + # init cameras + camera_warp = RayCasterCamera(camera_warp_cfg) + camera_usd = Camera(camera_usd_cfg) + + # play sim + sim.reset() + sim.play() + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + # filter nan and inf from output + cam_warp_output = camera_warp.data.output["distance_to_image_plane"].clone() + cam_usd_output = camera_usd.data.output["distance_to_image_plane"].clone() + cam_warp_output[torch.isnan(cam_warp_output)] = 0 + cam_warp_output[torch.isinf(cam_warp_output)] = 0 + cam_usd_output[torch.isnan(cam_usd_output)] = 0 + cam_usd_output[torch.isinf(cam_usd_output)] = 0 + + # check that both have the same intrinsic matrices + torch.testing.assert_close(camera_warp.data.intrinsic_matrices[0], camera_usd.data.intrinsic_matrices[0]) + + # check the apertures + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), + camera_warp_cfg.pattern_cfg.horizontal_aperture, + ) + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(), + camera_warp_cfg.pattern_cfg.vertical_aperture, + ) + + if DEBUG_PLOTS: + # plot both images next to each other plus their difference in a 1x3 grid figure + import matplotlib.pyplot as plt + + fig, axs = plt.subplots(1, 3, figsize=(15, 5)) + usd_plt = axs[0].imshow(cam_usd_output[0].cpu().numpy()) + fig.colorbar(usd_plt, ax=axs[0]) + axs[0].set_title("USD") + warp_plt = axs[1].imshow(cam_warp_output[0].cpu().numpy()) + fig.colorbar(warp_plt, ax=axs[1]) + axs[1].set_title("WARP") + diff_plt = axs[2].imshow(torch.abs(cam_usd_output - cam_warp_output)[0].cpu().numpy()) + fig.colorbar(diff_plt, ax=axs[2]) + axs[2].set_title("Difference") + # save figure + plt.tight_layout() + plt.savefig( + f"{os.path.dirname(os.path.abspath(__file__))}/output/test_output_equal_to_usd_camera_intrinsics_{focal_length}.png" + ) + plt.close() + + # check image data + if focal_length != 0.193: + # FIXME: 0.193 is not working on the IsaacSim/ UsdGeom side, add back once fixed torch.testing.assert_close( cam_warp_output, cam_usd_output, @@ -852,75 +888,110 @@ def test_output_equal_to_usd_camera_intrinsics(self): rtol=5e-6, ) - def test_output_equal_to_usd_camera_when_intrinsics_set(self): - """ - Test that the output of the ray caster camera is equal to the output of the usd camera when both are placed - under an XForm prim and an intrinsic matrix is set. - """ - - camera_pattern_cfg = patterns.PinholeCameraPatternCfg( - focal_length=24.0, - horizontal_aperture=20.955, - height=540, - width=960, - ) - camera_cfg_warp = RayCasterCameraCfg( - prim_path="/World/Camera", - mesh_prim_paths=["/World/defaultGroundPlane"], - update_period=0, - offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), - debug_vis=False, - pattern_cfg=camera_pattern_cfg, - data_types=["distance_to_camera"], - ) + del camera_warp, camera_usd - camera_warp = RayCasterCamera(camera_cfg_warp) - # create usd camera - camera_cfg_usd = CameraCfg( - height=540, - width=960, - prim_path="/World/Camera_usd", - update_period=0, - data_types=["distance_to_camera"], - spawn=PinholeCameraCfg( - focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1e-4, 1.0e5) - ), - ) - camera_usd = Camera(camera_cfg_usd) - - # play sim - self.sim.reset() - self.sim.play() - - # set intrinsic matrix - # NOTE: extend the test to cover aperture offsets once supported by the usd camera - intrinsic_matrix = torch.tensor( - [[380.0831, 0.0, camera_cfg_usd.width / 2, 0.0, 380.0831, camera_cfg_usd.height / 2, 0.0, 0.0, 1.0]], - device=camera_warp.device, - ).reshape(1, 3, 3) - camera_warp.set_intrinsic_matrices(intrinsic_matrix, focal_length=10) - camera_usd.set_intrinsic_matrices(intrinsic_matrix, focal_length=10) - - # set camera position - camera_warp.set_world_poses_from_view( - eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_warp.device), - targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_warp.device), - ) - camera_usd.set_world_poses_from_view( - eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_usd.device), - targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_usd.device), - ) - - # perform steps - for _ in range(5): - self.sim.step() - - # update camera - camera_usd.update(self.dt) - camera_warp.update(self.dt) - - # check image data +@pytest.mark.parametrize("focal_length_aperture", [(0.193, 0.20955), (1.93, 2.0955), (19.3, 20.955), (0.193, 20.955)]) +@pytest.mark.isaacsim_ci +def test_output_equal_to_usd_camera_when_intrinsics_set(setup_sim, focal_length_aperture): + """ + Test that the output of the ray caster camera is equal to the output of the usd camera when both are placed + under an XForm prim and an intrinsic matrix is set. + """ + # unpack focal length and aperture + focal_length, aperture = focal_length_aperture + + sim, camera_cfg, dt = setup_sim + camera_pattern_cfg = patterns.PinholeCameraPatternCfg( + focal_length=focal_length, + horizontal_aperture=aperture, + height=540, + width=960, + ) + camera_cfg_warp = RayCasterCameraCfg( + prim_path="/World/Camera", + mesh_prim_paths=["/World/defaultGroundPlane"], + update_period=0, + offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), + debug_vis=False, + pattern_cfg=camera_pattern_cfg, + data_types=["distance_to_camera"], + ) + + camera_warp = RayCasterCamera(camera_cfg_warp) + + # create usd camera + camera_cfg_usd = CameraCfg( + height=540, + width=960, + prim_path="/World/Camera_usd", + update_period=0, + data_types=["distance_to_camera"], + spawn=PinholeCameraCfg( + focal_length=focal_length, focus_distance=400.0, horizontal_aperture=aperture, clipping_range=(1e-4, 1.0e5) + ), + ) + camera_usd = Camera(camera_cfg_usd) + + # play sim + sim.reset() + sim.play() + + # set intrinsic matrix + # NOTE: extend the test to cover aperture offsets once supported by the usd camera + # intrinsic_matrix = torch.tensor( + # [[380.0831, 0.0, camera_cfg_usd.width / 2, 0.0, 380.0831, camera_cfg_usd.height / 2, 0.0, 0.0, 1.0]], + # device=camera_warp.device, + # ).reshape(1, 3, 3) + # camera_warp.set_intrinsic_matrices(intrinsic_matrix, focal_length=10) + # camera_usd.set_intrinsic_matrices(intrinsic_matrix, focal_length=10) + + # set camera position + camera_warp.set_world_poses_from_view( + eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_warp.device), + targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_warp.device), + ) + camera_usd.set_world_poses_from_view( + eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_usd.device), + targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_usd.device), + ) + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_warp.update(dt) + + if DEBUG_PLOTS: + # plot both images next to each other plus their difference in a 1x3 grid figure + import matplotlib.pyplot as plt + + fig, axs = plt.subplots(1, 3, figsize=(15, 5)) + usd_plt = axs[0].imshow(camera_usd.data.output["distance_to_camera"][0].cpu().numpy()) + fig.colorbar(usd_plt, ax=axs[0]) + axs[0].set_title("USD") + warp_plt = axs[1].imshow(camera_warp.data.output["distance_to_camera"][0].cpu().numpy()) + fig.colorbar(warp_plt, ax=axs[1]) + axs[1].set_title("WARP") + diff_plt = axs[2].imshow( + torch.abs(camera_usd.data.output["distance_to_camera"] - camera_warp.data.output["distance_to_camera"])[0] + .cpu() + .numpy() + ) + fig.colorbar(diff_plt, ax=axs[2]) + axs[2].set_title("Difference") + # save figure + plt.tight_layout() + plt.savefig( + f"{os.path.dirname(os.path.abspath(__file__))}/output/test_output_equal_to_usd_camera_when_intrinsics_set_{focal_length}_{aperture}.png" + ) + plt.close() + + # check image data + if focal_length != 0.193: + # FIXME: 0.193 is not working on the IsaacSim/ UsdGeom side, add back once fixed torch.testing.assert_close( camera_usd.data.output["distance_to_camera"], camera_warp.data.output["distance_to_camera"], @@ -928,15 +999,16 @@ def test_output_equal_to_usd_camera_when_intrinsics_set(self): atol=1e-4, ) - def test_sensor_print(self): - """Test sensor print is working correctly.""" - # Create sensor - sensor = RayCasterCamera(cfg=self.camera_cfg) - # Play sim - self.sim.reset() - # print info - print(sensor) + del camera_warp, camera_usd -if __name__ == "__main__": - run_tests() +@pytest.mark.isaacsim_ci +def test_sensor_print(setup_sim): + """Test sensor print is working correctly.""" + sim, camera_cfg, dt = setup_sim + # Create sensor + sensor = RayCasterCamera(cfg=camera_cfg) + # Play sim + sim.reset() + # print info + print(sensor) diff --git a/source/isaaclab/test/sensors/test_ray_caster_patterns.py b/source/isaaclab/test/sensors/test_ray_caster_patterns.py new file mode 100644 index 000000000000..cab9e4af7245 --- /dev/null +++ b/source/isaaclab/test/sensors/test_ray_caster_patterns.py @@ -0,0 +1,426 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import math + +import pytest +import torch + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=False).app + +# Import after app launch +from isaaclab.sensors.ray_caster.patterns import patterns, patterns_cfg + + +@pytest.fixture(scope="module", params=["cuda", "cpu"]) +def device(request): + """Fixture to parameterize tests over both CUDA and CPU devices.""" + if request.param == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + return request.param + + +class TestGridPattern: + """Test cases for grid_pattern function.""" + + @pytest.mark.parametrize( + "size,resolution,ordering,expected_num_rays", + [ + ((2.0, 2.0), 1.0, "xy", 9), # 3x3 grid + ((2.0, 2.0), 0.5, "xy", 25), # 5x5 grid + ((4.0, 2.0), 1.0, "xy", 15), # 5x3 grid + ((2.0, 4.0), 1.0, "yx", 15), # 3x5 grid + ((1.0, 1.0), 0.25, "xy", 25), # 5x5 grid with smaller size + ], + ) + def test_grid_pattern_num_rays(self, device, size, resolution, ordering, expected_num_rays): + """Test that grid pattern generates the correct number of rays.""" + cfg = patterns_cfg.GridPatternCfg(size=size, resolution=resolution, ordering=ordering) + ray_starts, ray_directions = patterns.grid_pattern(cfg, device) + + assert ray_starts.shape[0] == expected_num_rays + assert ray_directions.shape[0] == expected_num_rays + assert ray_starts.shape[1] == 3 + assert ray_directions.shape[1] == 3 + + @pytest.mark.parametrize("ordering", ["xy", "yx"]) + def test_grid_pattern_ordering(self, device, ordering): + """Test that grid pattern respects the ordering parameter.""" + cfg = patterns_cfg.GridPatternCfg(size=(2.0, 2.0), resolution=1.0, ordering=ordering) + ray_starts, ray_directions = patterns.grid_pattern(cfg, device) + + # Check that the rays are ordered correctly + if ordering == "xy": + # For "xy" ordering, x should change faster than y + # First few rays should have same y, different x + assert ray_starts[0, 1] == ray_starts[1, 1] # Same y + assert ray_starts[0, 0] != ray_starts[1, 0] # Different x + else: # "yx" + # For "yx" ordering, y should change faster than x + # First few rays should have same x, different y + assert ray_starts[0, 0] == ray_starts[1, 0] # Same x + assert ray_starts[0, 1] != ray_starts[1, 1] # Different y + + @pytest.mark.parametrize("direction", [(0.0, 0.0, -1.0), (0.0, 0.0, 1.0), (1.0, 0.0, 0.0)]) + def test_grid_pattern_direction(self, device, direction): + """Test that grid pattern uses the specified direction.""" + cfg = patterns_cfg.GridPatternCfg(size=(2.0, 2.0), resolution=1.0, direction=direction) + ray_starts, ray_directions = patterns.grid_pattern(cfg, device) + + expected_direction = torch.tensor(direction, device=device) + # All rays should have the same direction - check in batch + expected_directions = expected_direction.unsqueeze(0).expand_as(ray_directions) + torch.testing.assert_close(ray_directions, expected_directions) + + def test_grid_pattern_bounds(self, device): + """Test that grid pattern respects the size bounds.""" + size = (2.0, 4.0) + cfg = patterns_cfg.GridPatternCfg(size=size, resolution=1.0) + ray_starts, ray_directions = patterns.grid_pattern(cfg, device) + + # Check that all rays are within bounds + assert ray_starts[:, 0].min() >= -size[0] / 2 + assert ray_starts[:, 0].max() <= size[0] / 2 + assert ray_starts[:, 1].min() >= -size[1] / 2 + assert ray_starts[:, 1].max() <= size[1] / 2 + # Z should be 0 for grid pattern + torch.testing.assert_close(ray_starts[:, 2], torch.zeros_like(ray_starts[:, 2])) + + def test_grid_pattern_invalid_ordering(self, device): + """Test that invalid ordering raises ValueError.""" + cfg = patterns_cfg.GridPatternCfg(size=(2.0, 2.0), resolution=1.0, ordering="invalid") + with pytest.raises(ValueError, match="Ordering must be 'xy' or 'yx'"): + patterns.grid_pattern(cfg, device) + + def test_grid_pattern_invalid_resolution(self, device): + """Test that invalid resolution raises ValueError.""" + cfg = patterns_cfg.GridPatternCfg(size=(2.0, 2.0), resolution=-1.0) + with pytest.raises(ValueError, match="Resolution must be greater than 0"): + patterns.grid_pattern(cfg, device) + + +class TestLidarPattern: + """Test cases for lidar_pattern function.""" + + @pytest.mark.parametrize( + "horizontal_fov_range,horizontal_res,channels,vertical_fov_range", + [ + # Test 360 degree horizontal FOV + ((-180.0, 180.0), 90.0, 1, (-10.0, -10.0)), + ((-180.0, 180.0), 45.0, 1, (-10.0, -10.0)), + ((-180.0, 180.0), 1.0, 1, (-10.0, -10.0)), + # Test partial horizontal FOV + ((-90.0, 90.0), 30.0, 1, (-10.0, -10.0)), + ((0.0, 180.0), 45.0, 1, (-10.0, -10.0)), + # Test 360 no overlap case + ((-180.0, 180.0), 90.0, 1, (0.0, 0.0)), + # Test partial FOV case + ((-90.0, 90.0), 90.0, 1, (0.0, 0.0)), + # Test multiple channels + ((-180.0, 180.0), 90.0, 16, (-15.0, 15.0)), + ((-180.0, 180.0), 45.0, 32, (-30.0, 10.0)), + # Test single channel, different vertical angles + ((-180.0, 180.0), 90.0, 1, (45.0, 45.0)), + ], + ) + def test_lidar_pattern_num_rays(self, device, horizontal_fov_range, horizontal_res, channels, vertical_fov_range): + """Test that lidar pattern generates the correct number of rays.""" + cfg = patterns_cfg.LidarPatternCfg( + horizontal_fov_range=horizontal_fov_range, + horizontal_res=horizontal_res, + channels=channels, + vertical_fov_range=vertical_fov_range, + ) + ray_starts, ray_directions = patterns.lidar_pattern(cfg, device) + + # Calculate expected number of horizontal angles + if abs(abs(horizontal_fov_range[0] - horizontal_fov_range[1]) - 360.0) < 1e-6: + # 360 degree FOV - exclude last point to avoid overlap + expected_num_horizontal = ( + math.ceil((horizontal_fov_range[1] - horizontal_fov_range[0]) / horizontal_res) + 1 + ) - 1 + else: + expected_num_horizontal = ( + math.ceil((horizontal_fov_range[1] - horizontal_fov_range[0]) / horizontal_res) + 1 + ) + + expected_num_rays = channels * expected_num_horizontal + + assert ray_starts.shape[0] == expected_num_rays, ( + f"Expected {expected_num_rays} rays, got {ray_starts.shape[0]} rays. " + f"Horizontal angles: {expected_num_horizontal}, channels: {channels}" + ) + assert ray_directions.shape[0] == expected_num_rays + assert ray_starts.shape[1] == 3 + assert ray_directions.shape[1] == 3 + + def test_lidar_pattern_basic_properties(self, device): + """Test that ray directions are normalized and rays start from origin.""" + cfg = patterns_cfg.LidarPatternCfg( + horizontal_fov_range=(-180.0, 180.0), + horizontal_res=45.0, + channels=8, + vertical_fov_range=(-15.0, 15.0), + ) + ray_starts, ray_directions = patterns.lidar_pattern(cfg, device) + + # Check that all directions are unit vectors + norms = torch.norm(ray_directions, dim=1) + torch.testing.assert_close(norms, torch.ones_like(norms), rtol=1e-5, atol=1e-5) + + # All rays should start from origin + torch.testing.assert_close(ray_starts, torch.zeros_like(ray_starts)) + + @pytest.mark.parametrize( + "vertical_fov_range,channels", + [ + ((-15.0, 15.0), 4), + ((-30.0, 10.0), 5), + ((0.0, 0.0), 1), + ], + ) + def test_lidar_pattern_vertical_channels(self, device, vertical_fov_range, channels): + """Test that vertical channels are distributed correctly.""" + cfg = patterns_cfg.LidarPatternCfg( + horizontal_fov_range=(0.0, 0.0), # Single horizontal direction + horizontal_res=1.0, + channels=channels, + vertical_fov_range=vertical_fov_range, + ) + ray_starts, ray_directions = patterns.lidar_pattern(cfg, device) + + assert ray_starts.shape[0] == channels + + # Check that z-components span the vertical range + # For single horizontal direction (0,0), the z component is sin(vertical_angle) + z_components = ray_directions[:, 2] + expected_min_z = math.sin(math.radians(vertical_fov_range[0])) + expected_max_z = math.sin(math.radians(vertical_fov_range[1])) + + assert torch.isclose(z_components.min(), torch.tensor(expected_min_z, device=device), atol=1e-5) + assert torch.isclose(z_components.max(), torch.tensor(expected_max_z, device=device), atol=1e-5) + + @pytest.mark.parametrize( + "horizontal_fov_range,horizontal_res,expected_num_rays,expected_angular_spacing", + [ + # Test case from the bug fix: 360 deg FOV with 90 deg resolution + ((-180.0, 180.0), 90.0, 4, 90.0), + # Test case: 360 deg FOV with 45 deg resolution + ((-180.0, 180.0), 45.0, 8, 45.0), + # Test case: 180 deg FOV with 90 deg resolution + ((-90.0, 90.0), 90.0, 3, 90.0), + # Test case: 180 deg FOV with 60 deg resolution (avoids atan2 discontinuity at ยฑ180ยฐ) + ((-90.0, 90.0), 60.0, 4, 60.0), + # Test case: 360 deg FOV with 120 deg resolution + ((-180.0, 180.0), 120.0, 3, 120.0), + ], + ) + def test_lidar_pattern_exact_angles( + self, device, horizontal_fov_range, horizontal_res, expected_num_rays, expected_angular_spacing + ): + """Test that lidar pattern generates rays with correct count and angular spacing. + + This test verifies the fix for the horizontal angle calculation to ensure + the actual resolution matches the requested resolution. + """ + cfg = patterns_cfg.LidarPatternCfg( + horizontal_fov_range=horizontal_fov_range, + horizontal_res=horizontal_res, + channels=1, + vertical_fov_range=(0.0, 0.0), + ) + ray_starts, ray_directions = patterns.lidar_pattern(cfg, device) + + # Check that we have the right number of rays + assert ray_starts.shape[0] == expected_num_rays, ( + f"Expected {expected_num_rays} rays, got {ray_starts.shape[0]} rays" + ) + + # Calculate angles from directions + angles = torch.atan2(ray_directions[:, 1], ray_directions[:, 0]) + angles_deg = torch.rad2deg(angles) + + # Sort angles for easier checking + angles_deg_sorted = torch.sort(angles_deg)[0] + + # Check angular spacing between consecutive rays + for i in range(len(angles_deg_sorted) - 1): + angular_diff = abs(angles_deg_sorted[i + 1].item() - angles_deg_sorted[i].item()) + # Allow small tolerance for floating point errors + assert abs(angular_diff - expected_angular_spacing) < 1.0, ( + f"Angular spacing {angular_diff:.2f}ยฐ does not match expected {expected_angular_spacing}ยฐ" + ) + + # For 360 degree FOV, also check that first and last angles wrap correctly + is_360 = abs(abs(horizontal_fov_range[0] - horizontal_fov_range[1]) - 360.0) < 1e-6 + if is_360: + # The gap from last angle back to first angle (wrapping around) should also match spacing + first_angle = angles_deg_sorted[0].item() + last_angle = angles_deg_sorted[-1].item() + wraparound_diff = (first_angle + 360) - last_angle + assert abs(wraparound_diff - expected_angular_spacing) < 1.0, ( + f"Wraparound spacing {wraparound_diff:.2f}ยฐ does not match expected {expected_angular_spacing}ยฐ" + ) + + +class TestBpearlPattern: + """Test cases for bpearl_pattern function.""" + + @pytest.mark.parametrize( + "horizontal_fov,horizontal_res", + [ + (360.0, 10.0), # Default config + (360.0, 5.0), + (180.0, 10.0), + (90.0, 5.0), + ], + ) + def test_bpearl_pattern_horizontal_params(self, device, horizontal_fov, horizontal_res): + """Test bpearl pattern with different horizontal parameters.""" + cfg = patterns_cfg.BpearlPatternCfg( + horizontal_fov=horizontal_fov, + horizontal_res=horizontal_res, + ) + ray_starts, ray_directions = patterns.bpearl_pattern(cfg, device) + + # Calculate expected number of horizontal angles + expected_num_horizontal = int(horizontal_fov / horizontal_res) + expected_num_rays = len(cfg.vertical_ray_angles) * expected_num_horizontal + + assert ray_starts.shape[0] == expected_num_rays + + def test_bpearl_pattern_basic_properties(self, device): + """Test that ray directions are normalized and rays start from origin.""" + cfg = patterns_cfg.BpearlPatternCfg() + ray_starts, ray_directions = patterns.bpearl_pattern(cfg, device) + + # Check that all directions are unit vectors + norms = torch.norm(ray_directions, dim=1) + torch.testing.assert_close(norms, torch.ones_like(norms), rtol=1e-5, atol=1e-5) + + # All rays should start from origin + torch.testing.assert_close(ray_starts, torch.zeros_like(ray_starts)) + + def test_bpearl_pattern_custom_vertical_angles(self, device): + """Test bpearl pattern with custom vertical angles.""" + custom_angles = [10.0, 20.0, 30.0, 40.0, 50.0] + cfg = patterns_cfg.BpearlPatternCfg( + horizontal_fov=360.0, + horizontal_res=90.0, + vertical_ray_angles=custom_angles, + ) + ray_starts, ray_directions = patterns.bpearl_pattern(cfg, device) + + # 360/90 = 4 horizontal angles, 5 custom vertical angles + expected_num_rays = 4 * 5 + assert ray_starts.shape[0] == expected_num_rays + + +class TestPinholeCameraPattern: + """Test cases for pinhole_camera_pattern function.""" + + @pytest.mark.parametrize( + "width,height", + [ + (640, 480), + (1920, 1080), + (320, 240), + (100, 100), + ], + ) + def test_pinhole_camera_pattern_num_rays(self, device, width, height): + """Test that pinhole camera pattern generates the correct number of rays.""" + cfg = patterns_cfg.PinholeCameraPatternCfg( + width=width, + height=height, + ) + + # Create a simple intrinsic matrix for testing + # Using identity-like matrix with focal lengths and principal point at center + fx = fy = 500.0 + cx = width / 2 + cy = height / 2 + intrinsic_matrix = torch.tensor( + [[fx, 0, cx], [0, fy, cy], [0, 0, 1]], + device=device, + ) + + # Pattern expects batch of intrinsic matrices + intrinsic_matrices = intrinsic_matrix.unsqueeze(0) + + ray_starts, ray_directions = patterns.pinhole_camera_pattern(cfg, intrinsic_matrices, device) + + expected_num_rays = width * height + assert ray_starts.shape == (1, expected_num_rays, 3) + assert ray_directions.shape == (1, expected_num_rays, 3) + + def test_pinhole_camera_pattern_basic_properties(self, device): + """Test that ray directions are normalized and rays start from origin.""" + cfg = patterns_cfg.PinholeCameraPatternCfg(width=100, height=100) + + fx = fy = 500.0 + cx = cy = 50.0 + intrinsic_matrix = torch.tensor( + [[fx, 0, cx], [0, fy, cy], [0, 0, 1]], + device=device, + ).unsqueeze(0) + + ray_starts, ray_directions = patterns.pinhole_camera_pattern(cfg, intrinsic_matrix, device) + + # Check that all directions are unit vectors + norms = torch.norm(ray_directions, dim=2) + torch.testing.assert_close(norms, torch.ones_like(norms), rtol=1e-5, atol=1e-5) + + # All rays should start from origin + torch.testing.assert_close(ray_starts, torch.zeros_like(ray_starts)) + + def test_pinhole_camera_pattern_batch(self, device): + """Test that pinhole camera pattern works with batched intrinsic matrices.""" + cfg = patterns_cfg.PinholeCameraPatternCfg(width=50, height=50) + + # Create batch of 3 different intrinsic matrices + batch_size = 3 + intrinsic_matrices = [] + for i in range(batch_size): + fx = fy = 500.0 + i * 100 + cx = cy = 25.0 + intrinsic_matrices.append(torch.tensor([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], device=device)) + intrinsic_matrices = torch.stack(intrinsic_matrices) + + ray_starts, ray_directions = patterns.pinhole_camera_pattern(cfg, intrinsic_matrices, device) + + expected_num_rays = 50 * 50 + assert ray_starts.shape == (batch_size, expected_num_rays, 3) + assert ray_directions.shape == (batch_size, expected_num_rays, 3) + + # Check that different batches have different ray directions (due to different intrinsics) + assert not torch.allclose(ray_directions[0], ray_directions[1]) + + def test_pinhole_camera_from_intrinsic_matrix(self, device): + """Test creating PinholeCameraPatternCfg from intrinsic matrix.""" + width, height = 640, 480 + fx, fy = 500.0, 500.0 + cx, cy = 320.0, 240.0 + + intrinsic_list = [fx, 0, cx, 0, fy, cy, 0, 0, 1] + + cfg = patterns_cfg.PinholeCameraPatternCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsic_list, + width=width, + height=height, + ) + + assert cfg.width == width + assert cfg.height == height + assert cfg.focal_length == 24.0 # default + + # The apertures should be calculated based on the intrinsic matrix + assert cfg.horizontal_aperture > 0 + assert cfg.vertical_aperture > 0 diff --git a/source/isaaclab/test/sensors/test_sensor_base.py b/source/isaaclab/test/sensors/test_sensor_base.py new file mode 100644 index 000000000000..1f41ba4ab4e5 --- /dev/null +++ b/source/isaaclab/test/sensors/test_sensor_base.py @@ -0,0 +1,227 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +app_launcher = AppLauncher(headless=True) +simulation_app = app_launcher.app + + +"""Rest everything follows.""" + +from collections.abc import Sequence +from dataclasses import dataclass + +import pytest +import torch + +import isaaclab.sim as sim_utils +from isaaclab.sensors import SensorBase, SensorBaseCfg +from isaaclab.utils import configclass + + +@dataclass +class DummyData: + count: torch.Tensor = None + + +class DummySensor(SensorBase): + def __init__(self, cfg): + super().__init__(cfg) + self._data = DummyData() + + def _initialize_impl(self): + super()._initialize_impl() + self._data.count = torch.zeros((self._num_envs), dtype=torch.int, device=self.device) + + @property + def data(self): + # update sensors if needed + self._update_outdated_buffers() + # return the data (where `_data` is the data for the sensor) + return self._data + + def _update_buffers_impl(self, env_ids: Sequence[int]): + self._data.count[env_ids] += 1 + + def reset(self, env_ids: Sequence[int] | None = None): + super().reset(env_ids=env_ids) + # Resolve sensor ids + if env_ids is None: + env_ids = slice(None) + self._data.count[env_ids] = 0 + + +@configclass +class DummySensorCfg(SensorBaseCfg): + class_type = DummySensor + + prim_path = "/World/envs/env_.*/Cube/dummy_sensor" + + +def _populate_scene(): + """""" + + # Ground-plane + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/defaultGroundPlane", cfg) + # Lights + cfg = sim_utils.SphereLightCfg() + cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) + cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) + + # create prims + for i in range(5): + _ = sim_utils.create_prim( + f"/World/envs/env_{i:02d}/Cube", + "Cube", + translation=(i * 1.0, 0.0, 0.0), + scale=(0.25, 0.25, 0.25), + ) + + +@pytest.fixture +def create_dummy_sensor(request, device): + # Create a new stage + sim_utils.create_new_stage() + + # Simulation time-step + dt = 0.01 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt, device=device) + sim = sim_utils.SimulationContext(sim_cfg) + + # create sensor + _populate_scene() + + sensor_cfg = DummySensorCfg() + + sim_utils.update_stage() + + yield sensor_cfg, sim, dt + + # stop simulation + # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( + sim._timeline.stop() + # clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.parametrize("device", ("cpu", "cuda")) +def test_sensor_init(create_dummy_sensor, device): + """Test that the sensor initializes, steps without update, and forces update.""" + + sensor_cfg, sim, dt = create_dummy_sensor + sensor = DummySensor(cfg=sensor_cfg) + + # Play sim + sim.step() + + sim.reset() + + assert sensor.is_initialized + assert int(sensor.num_instances) == 5 + + # test that the data is not updated + for i in range(10): + sim.step() + sensor.update(dt=dt, force_recompute=True) + expected_value = i + 1 + torch.testing.assert_close( + sensor.data.count, + torch.tensor(expected_value, device=device, dtype=torch.int32).repeat(sensor.num_instances), + ) + assert sensor.data.count.shape[0] == 5 + + # test that the data is not updated if sensor.data is not accessed + for _ in range(5): + sim.step() + sensor.update(dt=dt, force_recompute=False) + torch.testing.assert_close( + sensor._data.count, + torch.tensor(expected_value, device=device, dtype=torch.int32).repeat(sensor.num_instances), + ) + + +@pytest.mark.parametrize("device", ("cpu", "cuda")) +def test_sensor_update_rate(create_dummy_sensor, device): + """Test that the update_rate configuration parameter works by checking the value of the data is old for an update + period of 2. + """ + sensor_cfg, sim, dt = create_dummy_sensor + sensor_cfg.update_period = 2 * dt + sensor = DummySensor(cfg=sensor_cfg) + + # Play sim + sim.step() + + sim.reset() + + assert sensor.is_initialized + assert int(sensor.num_instances) == 5 + expected_value = 1 + for i in range(10): + sim.step() + sensor.update(dt=dt, force_recompute=True) + # count should he half of the number of steps + torch.testing.assert_close( + sensor.data.count, + torch.tensor(expected_value, device=device, dtype=torch.int32).repeat(sensor.num_instances), + ) + expected_value += i % 2 + + +@pytest.mark.parametrize("device", ("cpu", "cuda")) +def test_sensor_reset(create_dummy_sensor, device): + """Test that sensor can be reset for all or partial env ids.""" + sensor_cfg, sim, dt = create_dummy_sensor + sensor = DummySensor(cfg=sensor_cfg) + + # Play sim + sim.step() + sim.reset() + + assert sensor.is_initialized + assert int(sensor.num_instances) == 5 + for i in range(5): + sim.step() + sensor.update(dt=dt) + # count should he half of the number of steps + torch.testing.assert_close( + sensor.data.count, + torch.tensor(i + 1, device=device, dtype=torch.int32).repeat(sensor.num_instances), + ) + + sensor.reset() + + for j in range(5): + sim.step() + sensor.update(dt=dt) + # count should he half of the number of steps + torch.testing.assert_close( + sensor.data.count, + torch.tensor(j + 1, device=device, dtype=torch.int32).repeat(sensor.num_instances), + ) + + reset_ids = [2, 4] + cont_ids = [0, 1, 3] + sensor.reset(env_ids=reset_ids) + + for k in range(5): + sim.step() + sensor.update(dt=dt) + # count should he half of the number of steps + torch.testing.assert_close( + sensor.data.count[reset_ids], + torch.tensor(k + 1, device=device, dtype=torch.int32).repeat(len(reset_ids)), + ) + torch.testing.assert_close( + sensor.data.count[cont_ids], + torch.tensor(k + 6, device=device, dtype=torch.int32).repeat(len(cont_ids)), + ) diff --git a/source/isaaclab/test/sensors/test_tiled_camera.py b/source/isaaclab/test/sensors/test_tiled_camera.py index dad4ba99e488..f160ef35df84 100644 --- a/source/isaaclab/test/sensors/test_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_tiled_camera.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,25 +8,23 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" import copy -import numpy as np import random + +import numpy as np +import pytest import torch -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep from isaacsim.core.prims import SingleGeometryPrim, SingleRigidPrim -from pxr import Gf, Semantics, UsdGeom +from pxr import Gf, UsdGeom import isaaclab.sim as sim_utils from isaaclab.sensors.camera import Camera, CameraCfg, TiledCamera, TiledCameraCfg @@ -34,1646 +32,1732 @@ from isaaclab.utils.timer import Timer -class TestTiledCamera(unittest.TestCase): - """Test for USD tiled Camera sensor.""" - - def setUp(self): - """Create a blank new stage for each test.""" - self.camera_cfg = TiledCameraCfg( - height=128, - width=256, - offset=TiledCameraCfg.OffsetCfg(pos=(0.0, 0.0, 4.0), rot=(0.0, 0.0, 1.0, 0.0), convention="ros"), - prim_path="/World/Camera", - update_period=0, - data_types=["rgb", "distance_to_camera"], - spawn=sim_utils.PinholeCameraCfg( - focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) - ), - ) - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.01 - # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=self.dt) - self.sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg) - # populate scene - self._populate_scene() - # load stage - stage_utils.update_stage() - - def tearDown(self): - """Stops simulator after each test.""" - # close all the opened viewport from before. - rep.vp_manager.destroy_hydra_textures("Replicator") - # stop simulation - # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - self.sim._timeline.stop() - # clear the stage - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Tests - """ - - def test_single_camera_init(self): - """Test single camera initialization.""" - # Create camera - camera = TiledCamera(self.camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[0].GetPath().pathString, self.camera_cfg.prim_path) - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (1, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (1, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (1, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (1, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (1, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for im_type, im_data in camera.data.output.items(): - if im_type == "rgb": - self.assertEqual(im_data.shape, (1, self.camera_cfg.height, self.camera_cfg.width, 3)) - self.assertGreater((im_data / 255.0).mean().item(), 0.0) - elif im_type == "distance_to_camera": - self.assertEqual(im_data.shape, (1, self.camera_cfg.height, self.camera_cfg.width, 1)) - self.assertGreater(im_data.mean().item(), 0.0) - del camera - - def test_depth_clipping_max(self): - """Test depth max clipping.""" - # get camera cfgs - camera_cfg = TiledCameraCfg( - prim_path="/World/Camera", - offset=TiledCameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), - spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( - focal_length=38.0, - intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], - height=540, - width=960, - clipping_range=(4.9, 5.0), - ), +@pytest.fixture(scope="function") +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, + width=256, + offset=TiledCameraCfg.OffsetCfg(pos=(0.0, 0.0, 4.0), rot=(0.0, 0.0, 1.0, 0.0), convention="ros"), + prim_path="/World/Camera", + update_period=0, + data_types=["rgb", "distance_to_camera"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + ) + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.01 + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt, device=device) + sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg) + # populate scene + _populate_scene() + # load stage + sim_utils.update_stage() + yield sim, camera_cfg, dt + # Teardown + rep.vp_manager.destroy_hydra_textures("Replicator") + sim._timeline.stop() + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_single_camera_init(setup_camera, device): + """Test single camera initialization.""" + sim, camera_cfg, dt = setup_camera + # Create camera + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[0].GetPath().pathString == camera_cfg.prim_path + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (1, 3) + assert camera.data.quat_w_ros.shape == (1, 4) + assert camera.data.quat_w_world.shape == (1, 4) + assert camera.data.quat_w_opengl.shape == (1, 4) + assert camera.data.intrinsic_matrices.shape == (1, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for im_type, im_data in camera.data.output.items(): + if im_type == "rgb": + assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 3) + assert (im_data / 255.0).mean() > 0.0 + elif im_type == "distance_to_camera": + assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 1) + assert im_data.mean() > 0.0 + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_depth_clipping_max(setup_camera, device): + """Test depth max clipping.""" + sim, _, dt = setup_camera + # get camera cfgs + camera_cfg = TiledCameraCfg( + prim_path="/World/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), + spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( + focal_length=38.0, + intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], height=540, width=960, - data_types=["depth"], - depth_clipping_behavior="max", - ) - camera = TiledCamera(camera_cfg) - - # Play sim - self.sim.reset() - - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - camera.update(self.dt) - - self.assertTrue(len(camera.data.output["depth"][torch.isinf(camera.data.output["depth"])]) == 0) - self.assertTrue(camera.data.output["depth"].min() >= camera_cfg.spawn.clipping_range[0]) - self.assertTrue(camera.data.output["depth"].max() <= camera_cfg.spawn.clipping_range[1]) - - del camera - - def test_depth_clipping_none(self): - """Test depth none clipping.""" - # get camera cfgs - camera_cfg = TiledCameraCfg( - prim_path="/World/Camera", - offset=TiledCameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), - spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( - focal_length=38.0, - intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], - height=540, - width=960, - clipping_range=(4.9, 5.0), - ), + clipping_range=(4.9, 5.0), + ), + height=540, + width=960, + data_types=["depth"], + depth_clipping_behavior="max", + ) + camera = TiledCamera(camera_cfg) + + # Play sim + sim.reset() + + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + camera.update(dt) + + assert len(camera.data.output["depth"][torch.isinf(camera.data.output["depth"])]) == 0 + assert camera.data.output["depth"].min() >= camera_cfg.spawn.clipping_range[0] + assert camera.data.output["depth"].max() <= camera_cfg.spawn.clipping_range[1] + + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_depth_clipping_none(setup_camera, device): + """Test depth none clipping.""" + sim, _, dt = setup_camera + # get camera cfgs + camera_cfg = TiledCameraCfg( + prim_path="/World/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), + spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( + focal_length=38.0, + intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], height=540, width=960, - data_types=["depth"], - depth_clipping_behavior="none", - ) - camera = TiledCamera(camera_cfg) - - # Play sim - self.sim.reset() - - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - camera.update(self.dt) - - self.assertTrue(len(camera.data.output["depth"][torch.isinf(camera.data.output["depth"])]) > 0) - self.assertTrue(camera.data.output["depth"].min() >= camera_cfg.spawn.clipping_range[0]) - self.assertTrue( + clipping_range=(4.9, 5.0), + ), + height=540, + width=960, + data_types=["depth"], + depth_clipping_behavior="none", + ) + camera = TiledCamera(camera_cfg) + + # Play sim + sim.reset() + + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + camera.update(dt) + + assert len(camera.data.output["depth"][torch.isinf(camera.data.output["depth"])]) > 0 + assert camera.data.output["depth"].min() >= camera_cfg.spawn.clipping_range[0] + if len(camera.data.output["depth"][~torch.isinf(camera.data.output["depth"])]) > 0: + assert ( camera.data.output["depth"][~torch.isinf(camera.data.output["depth"])].max() <= camera_cfg.spawn.clipping_range[1] ) - del camera - - def test_depth_clipping_zero(self): - """Test depth zero clipping.""" - # get camera cfgs - camera_cfg = TiledCameraCfg( - prim_path="/World/Camera", - offset=TiledCameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), - spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( - focal_length=38.0, - intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], - height=540, - width=960, - clipping_range=(4.9, 5.0), - ), + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_depth_clipping_zero(setup_camera, device): + """Test depth zero clipping.""" + sim, _, dt = setup_camera + # get camera cfgs + camera_cfg = TiledCameraCfg( + prim_path="/World/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(2.5, 2.5, 6.0), rot=(-0.125, 0.362, 0.873, -0.302), convention="ros"), + spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix( + focal_length=38.0, + intrinsic_matrix=[380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0], height=540, width=960, - data_types=["depth"], - depth_clipping_behavior="zero", - ) - camera = TiledCamera(camera_cfg) - - # Play sim - self.sim.reset() - - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - camera.update(self.dt) - - self.assertTrue(len(camera.data.output["depth"][torch.isinf(camera.data.output["depth"])]) == 0) - self.assertTrue(camera.data.output["depth"].min() == 0.0) - self.assertTrue(camera.data.output["depth"].max() <= camera_cfg.spawn.clipping_range[1]) - - del camera - - def test_multi_camera_init(self): - """Test multi-camera initialization.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for im_type, im_data in camera.data.output.items(): - if im_type == "rgb": - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 3)) - for i in range(4): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - elif im_type == "distance_to_camera": - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) - for i in range(4): - self.assertGreater(im_data[i].mean().item(), 0.0) - del camera - - def test_rgb_only_camera(self): - """Test initialization with only RGB.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["rgb"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(sorted(camera.data.output.keys()), sorted(["rgb", "rgba"])) - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - im_data = camera.data.output["rgb"] - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 3)) - for i in range(4): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - # Check data type of image - self.assertEqual(camera.data.output["rgb"].dtype, torch.uint8) - del camera - - def test_data_types(self): - """Test single camera initialization.""" - # Create camera - camera_cfg_distance = copy.deepcopy(self.camera_cfg) - camera_cfg_distance.data_types = ["distance_to_camera"] - camera_cfg_distance.prim_path = "/World/CameraDistance" - camera_distance = TiledCamera(camera_cfg_distance) - camera_cfg_depth = copy.deepcopy(self.camera_cfg) - camera_cfg_depth.data_types = ["depth"] - camera_cfg_depth.prim_path = "/World/CameraDepth" - camera_depth = TiledCamera(camera_cfg_depth) - camera_cfg_both = copy.deepcopy(self.camera_cfg) - camera_cfg_both.data_types = ["distance_to_camera", "depth"] - camera_cfg_both.prim_path = "/World/CameraBoth" - camera_both = TiledCamera(camera_cfg_both) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera_distance.is_initialized) - self.assertTrue(camera_depth.is_initialized) - self.assertTrue(camera_both.is_initialized) - self.assertListEqual(list(camera_distance.data.output.keys()), ["distance_to_camera"]) - self.assertListEqual(list(camera_depth.data.output.keys()), ["depth"]) - self.assertListEqual(list(camera_both.data.output.keys()), ["depth", "distance_to_camera"]) - - del camera_distance, camera_depth, camera_both - - def test_depth_only_camera(self): - """Test initialization with only depth.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["distance_to_camera"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(sorted(camera.data.output.keys()), ["distance_to_camera"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - im_data = camera.data.output["distance_to_camera"] - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) - for i in range(4): - self.assertGreater((im_data[i]).mean().item(), 0.0) - # Check data type of image - self.assertEqual(camera.data.output["distance_to_camera"].dtype, torch.float) - del camera - - def test_rgba_only_camera(self): - """Test initialization with only RGBA.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["rgba"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["rgba"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 4)) - for i in range(4): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - # Check data type of image - self.assertEqual(camera.data.output["rgba"].dtype, torch.uint8) - del camera - - def test_distance_to_camera_only_camera(self): - """Test initialization with only distance_to_camera.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["distance_to_camera"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["distance_to_camera"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) - for i in range(4): - self.assertGreater((im_data[i]).mean().item(), 0.0) - # Check data type of image - self.assertEqual(camera.data.output["distance_to_camera"].dtype, torch.float) - del camera - - def test_distance_to_image_plane_only_camera(self): - """Test initialization with only distance_to_image_plane.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["distance_to_image_plane"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["distance_to_image_plane"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) - for i in range(4): - self.assertGreater((im_data[i]).mean().item(), 0.0) - # Check data type of image - self.assertEqual(camera.data.output["distance_to_image_plane"].dtype, torch.float) - del camera - - def test_normals_only_camera(self): - """Test initialization with only normals.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["normals"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["normals"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 3)) - for i in range(4): - self.assertGreater((im_data[i]).mean().item(), 0.0) - # Check data type of image - self.assertEqual(camera.data.output["normals"].dtype, torch.float) - del camera - - def test_motion_vectors_only_camera(self): - """Test initialization with only motion_vectors.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["motion_vectors"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["motion_vectors"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 2)) + clipping_range=(4.9, 5.0), + ), + height=540, + width=960, + data_types=["depth"], + depth_clipping_behavior="zero", + ) + camera = TiledCamera(camera_cfg) + + # Play sim + sim.reset() + + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + camera.update(dt) + + assert len(camera.data.output["depth"][torch.isinf(camera.data.output["depth"])]) == 0 + assert camera.data.output["depth"].min() == 0.0 + assert camera.data.output["depth"].max() <= camera_cfg.spawn.clipping_range[1] + + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_multi_camera_init(setup_camera, device): + """Test multi-camera initialization.""" + sim, camera_cfg, dt = setup_camera + + num_cameras = 9 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for im_type, im_data in camera.data.output.items(): + if im_type == "rgb": + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 3) for i in range(4): - self.assertNotEqual((im_data[i]).mean().item(), 0.0) - # Check data type of image - self.assertEqual(camera.data.output["motion_vectors"].dtype, torch.float) - del camera - - def test_semantic_segmentation_colorize_only_camera(self): - """Test initialization with only semantic_segmentation.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["semantic_segmentation"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["semantic_segmentation"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 4)) + assert (im_data[i] / 255.0).mean() > 0.0 + elif im_type == "distance_to_camera": + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) for i in range(4): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - # Check data type of image and info - self.assertEqual(camera.data.output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(type(camera.data.info["semantic_segmentation"]), dict) - del camera - - def test_instance_segmentation_fast_colorize_only_camera(self): - """Test initialization with only instance_segmentation_fast.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["instance_segmentation_fast"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["instance_segmentation_fast"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 4)) + assert im_data[i].mean() > 0.0 + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rgb_only_camera(setup_camera, device): + """Test initialization with only RGB data type.""" + sim, camera_cfg, dt = setup_camera + num_cameras = 9 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["rgb"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["rgba", "rgb"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + im_data = camera.data.output["rgb"] + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 3) + for i in range(4): + assert (im_data[i] / 255.0).mean() > 0.0 + assert camera.data.output["rgb"].dtype == torch.uint8 + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_data_types(setup_camera, device): + """Test different data types for camera initialization.""" + sim, camera_cfg, dt = setup_camera + # Create camera + camera_cfg_distance = copy.deepcopy(camera_cfg) + camera_cfg_distance.data_types = ["distance_to_camera"] + camera_cfg_distance.prim_path = "/World/CameraDistance" + camera_distance = TiledCamera(camera_cfg_distance) + camera_cfg_depth = copy.deepcopy(camera_cfg) + camera_cfg_depth.data_types = ["depth"] + camera_cfg_depth.prim_path = "/World/CameraDepth" + camera_depth = TiledCamera(camera_cfg_depth) + camera_cfg_both = copy.deepcopy(camera_cfg) + camera_cfg_both.data_types = ["distance_to_camera", "depth"] + camera_cfg_both.prim_path = "/World/CameraBoth" + camera_both = TiledCamera(camera_cfg_both) + + # Play sim + sim.reset() + + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check if cameras are initialized + assert camera_distance.is_initialized + assert camera_depth.is_initialized + assert camera_both.is_initialized + + # Check if camera prims are set correctly and that they are camera prims + assert camera_distance._sensor_prims[0].GetPath().pathString == "/World/CameraDistance" + assert isinstance(camera_distance._sensor_prims[0], UsdGeom.Camera) + assert camera_depth._sensor_prims[0].GetPath().pathString == "/World/CameraDepth" + assert isinstance(camera_depth._sensor_prims[0], UsdGeom.Camera) + assert camera_both._sensor_prims[0].GetPath().pathString == "/World/CameraBoth" + assert isinstance(camera_both._sensor_prims[0], UsdGeom.Camera) + assert list(camera_distance.data.output.keys()) == ["distance_to_camera"] + assert list(camera_depth.data.output.keys()) == ["depth"] + assert list(camera_both.data.output.keys()) == ["depth", "distance_to_camera"] + + del camera_distance + del camera_depth + del camera_both + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_depth_only_camera(setup_camera, device): + """Test initialization with only depth.""" + sim, camera_cfg, dt = setup_camera + num_cameras = 9 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["distance_to_camera"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["distance_to_camera"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + im_data = camera.data.output["distance_to_camera"] + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(4): + assert im_data[i].mean() > 0.0 + assert camera.data.output["distance_to_camera"].dtype == torch.float + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rgba_only_camera(setup_camera, device): + """Test initialization with only RGBA.""" + sim, camera_cfg, dt = setup_camera + num_cameras = 9 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["rgba"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["rgba"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) + for i in range(4): + assert (im_data[i] / 255.0).mean() > 0.0 + assert camera.data.output["rgba"].dtype == torch.uint8 + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["distance_to_camera"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["distance_to_camera"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(4): + assert im_data[i].mean() > 0.0 + assert camera.data.output["distance_to_camera"].dtype == torch.float + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["distance_to_image_plane"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["distance_to_image_plane"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(4): + assert im_data[i].mean() > 0.0 + assert camera.data.output["distance_to_image_plane"].dtype == torch.float + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_normals_only_camera(setup_camera, device): + """Test initialization with only normals.""" + sim, camera_cfg, dt = setup_camera + num_cameras = 9 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["normals"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["normals"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 3) + for i in range(4): + assert im_data[i].mean() > 0.0 + # check normal norm is approximately 1 + norms = torch.norm(im_data, dim=-1) + assert torch.allclose(norms, torch.ones_like(norms), atol=1e-9) + assert camera.data.output["normals"].dtype == torch.float + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_motion_vectors_only_camera(setup_camera, device): + """Test initialization with only motion_vectors.""" + sim, camera_cfg, dt = setup_camera + num_cameras = 9 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["motion_vectors"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["motion_vectors"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 2) + for i in range(4): + assert im_data[i].mean() != 0.0 + assert camera.data.output["motion_vectors"].dtype == torch.float + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["semantic_segmentation"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["semantic_segmentation"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) + for i in range(4): + assert (im_data[i] / 255.0).mean() > 0.0 + assert camera.data.output["semantic_segmentation"].dtype == torch.uint8 + assert isinstance(camera.data.info["semantic_segmentation"], dict) + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["instance_segmentation_fast"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["instance_segmentation_fast"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) + for i in range(num_cameras): + assert (im_data[i] / 255.0).mean() > 0.0 + assert camera.data.output["instance_segmentation_fast"].dtype == torch.uint8 + assert isinstance(camera.data.info["instance_segmentation_fast"], dict) + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["instance_id_segmentation_fast"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["instance_id_segmentation_fast"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) + for i in range(num_cameras): + assert (im_data[i] / 255.0).mean() > 0.0 + assert camera.data.output["instance_id_segmentation_fast"].dtype == torch.uint8 + assert isinstance(camera.data.info["instance_id_segmentation_fast"], dict) + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["semantic_segmentation"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera_cfg.colorize_semantic_segmentation = False + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["semantic_segmentation"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(num_cameras): + assert im_data[i].to(dtype=float).mean() > 0.0 + assert camera.data.output["semantic_segmentation"].dtype == torch.int32 + assert isinstance(camera.data.info["semantic_segmentation"], dict) + + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["instance_segmentation_fast"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera_cfg.colorize_instance_segmentation = False + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["instance_segmentation_fast"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(num_cameras): + assert im_data[i].to(dtype=float).mean() > 0.0 + assert camera.data.output["instance_segmentation_fast"].dtype == torch.int32 + assert isinstance(camera.data.info["instance_segmentation_fast"], dict) + del 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 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = ["instance_id_segmentation_fast"] + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera_cfg.colorize_instance_id_segmentation = False + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert list(camera.data.output.keys()) == ["instance_id_segmentation_fast"] + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for _, im_data in camera.data.output.items(): + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(num_cameras): + assert im_data[i].to(dtype=float).mean() > 0.0 + assert camera.data.output["instance_id_segmentation_fast"].dtype == torch.int32 + assert isinstance(camera.data.info["instance_id_segmentation_fast"], dict) + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_all_annotators_camera(setup_camera, device): + """Test initialization with all supported annotators.""" + sim, camera_cfg, dt = setup_camera + all_annotator_types = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + + num_cameras = 9 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = all_annotator_types + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert sorted(camera.data.output.keys()) == sorted(all_annotator_types) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for data_type, im_data in camera.data.output.items(): + if data_type in ["rgb", "normals"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 3) + elif data_type in [ + "rgba", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) for i in range(num_cameras): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - # Check data type of image and info - self.assertEqual(camera.data.output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(type(camera.data.info["instance_segmentation_fast"]), dict) - del camera - - def test_instance_id_segmentation_fast_colorize_only_camera(self): - """Test initialization with only instance_id_segmentation_fast.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["instance_id_segmentation_fast"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["instance_id_segmentation_fast"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 4)) + assert (im_data[i] / 255.0).mean() > 0.0 + elif data_type in ["motion_vectors"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 2) for i in range(num_cameras): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - # Check data type of image and info - self.assertEqual(camera.data.output["instance_id_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(type(camera.data.info["instance_id_segmentation_fast"]), dict) - del camera - - def test_semantic_segmentation_non_colorize_only_camera(self): - """Test initialization with only semantic_segmentation.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["semantic_segmentation"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera_cfg.colorize_semantic_segmentation = False - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["semantic_segmentation"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) + assert im_data[i].mean() != 0.0 + elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) for i in range(num_cameras): - self.assertGreater(im_data[i].to(dtype=float).mean().item(), 0.0) - # Check data type of image and info - self.assertEqual(camera.data.output["semantic_segmentation"].dtype, torch.int32) - self.assertEqual(type(camera.data.info["semantic_segmentation"]), dict) - - del camera - - def test_instance_segmentation_fast_non_colorize_only_camera(self): - """Test initialization with only instance_segmentation_fast.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["instance_segmentation_fast"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera_cfg.colorize_instance_segmentation = False - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["instance_segmentation_fast"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) + assert im_data[i].mean() > 0.0 + + # access image data and compare dtype + output = camera.data.output + info = camera.data.info + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.uint8 + assert output["instance_segmentation_fast"].dtype == torch.uint8 + assert output["instance_id_segmentation_fast"].dtype == torch.uint8 + assert isinstance(info["semantic_segmentation"], dict) + assert isinstance(info["instance_segmentation_fast"], dict) + assert isinstance(info["instance_id_segmentation_fast"], dict) + + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + + num_cameras = 2 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.height = 40 + camera_cfg.width = 40 + camera_cfg.data_types = all_annotator_types + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert sorted(camera.data.output.keys()) == sorted(all_annotator_types) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + camera.update(dt) + # check image data + for data_type, im_data in camera.data.output.items(): + if data_type in ["rgb", "normals"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 3) + elif data_type in [ + "rgba", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) for i in range(num_cameras): - self.assertGreater(im_data[i].to(dtype=float).mean().item(), 0.0) - # Check data type of image and info - self.assertEqual(camera.data.output["instance_segmentation_fast"].dtype, torch.int32) - self.assertEqual(type(camera.data.info["instance_segmentation_fast"]), dict) - del camera - - def test_instance_id_segmentation_fast_non_colorize_only_camera(self): - """Test initialization with only instance_id_segmentation_fast.""" - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = ["instance_id_segmentation_fast"] - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera_cfg.colorize_instance_id_segmentation = False - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(list(camera.data.output.keys()), ["instance_id_segmentation_fast"]) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for _, im_data in camera.data.output.items(): - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) + assert (im_data[i] / 255.0).mean() > 0.0 + elif data_type in ["motion_vectors"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 2) for i in range(num_cameras): - self.assertGreater(im_data[i].to(dtype=float).mean().item(), 0.0) - # Check data type of image and info - self.assertEqual(camera.data.output["instance_id_segmentation_fast"].dtype, torch.int32) - self.assertEqual(type(camera.data.info["instance_id_segmentation_fast"]), dict) - del camera - - def test_all_annotators_camera(self): - """Test initialization with all supported annotators.""" - all_annotator_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ] - - num_cameras = 9 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = all_annotator_types - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(sorted(camera.data.output.keys()), sorted(all_annotator_types)) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type in ["rgb", "normals"]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 3)) - elif data_type in [ - "rgba", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 4)) - for i in range(num_cameras): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - elif data_type in ["motion_vectors"]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 2)) - for i in range(num_cameras): - self.assertNotEqual(im_data[i].mean().item(), 0.0) - elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) - for i in range(num_cameras): - self.assertGreater(im_data[i].mean().item(), 0.0) - - # access image data and compare dtype - output = camera.data.output - info = camera.data.info - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(type(info["semantic_segmentation"]), dict) - self.assertEqual(type(info["instance_segmentation_fast"]), dict) - self.assertEqual(type(info["instance_id_segmentation_fast"]), dict) - - del camera - - def test_all_annotators_low_resolution_camera(self): - """Test initialization with all supported annotators.""" - all_annotator_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ] - - num_cameras = 2 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.height = 40 - camera_cfg.width = 40 - camera_cfg.data_types = all_annotator_types - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(sorted(camera.data.output.keys()), sorted(all_annotator_types)) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (camera_cfg.height, camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type in ["rgb", "normals"]: - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 3)) - elif data_type in [ - "rgba", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ]: - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 4)) - for i in range(num_cameras): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - elif data_type in ["motion_vectors"]: - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 2)) - for i in range(num_cameras): - self.assertNotEqual(im_data[i].mean().item(), 0.0) - elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 1)) - for i in range(num_cameras): - self.assertGreater(im_data[i].mean().item(), 0.0) - - # access image data and compare dtype - output = camera.data.output - info = camera.data.info - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(type(info["semantic_segmentation"]), dict) - self.assertEqual(type(info["instance_segmentation_fast"]), dict) - self.assertEqual(type(info["instance_id_segmentation_fast"]), dict) - - del camera - - def test_all_annotators_non_perfect_square_number_camera(self): - """Test initialization with all supported annotators.""" - all_annotator_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ] - - num_cameras = 11 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.data_types = all_annotator_types - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(sorted(camera.data.output.keys()), sorted(all_annotator_types)) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (self.camera_cfg.height, self.camera_cfg.width)) - - # Simulate physics - for _ in range(10): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type in ["rgb", "normals"]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 3)) - elif data_type in [ - "rgba", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 4)) - for i in range(num_cameras): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.0) - elif data_type in ["motion_vectors"]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 2)) - for i in range(num_cameras): - self.assertNotEqual(im_data[i].mean().item(), 0.0) - elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: - self.assertEqual(im_data.shape, (num_cameras, self.camera_cfg.height, self.camera_cfg.width, 1)) - for i in range(num_cameras): - self.assertGreater(im_data[i].mean().item(), 0.0) - - # access image data and compare dtype - output = camera.data.output - info = camera.data.info - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(type(info["semantic_segmentation"]), dict) - self.assertEqual(type(info["instance_segmentation_fast"]), dict) - self.assertEqual(type(info["instance_id_segmentation_fast"]), dict) - - del camera - - def test_all_annotators_instanceable(self): - """Test initialization with all supported annotators on instanceable assets.""" - all_annotator_types = [ - "rgb", - "rgba", - "depth", - "distance_to_camera", - "distance_to_image_plane", - "normals", - "motion_vectors", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ] - - num_cameras = 10 - for i in range(num_cameras): - prim_utils.create_prim(f"/World/Origin_{i}", "Xform", translation=(0.0, i, 0.0)) - - # Create a stage with 10 instanceable cubes, where each camera points to one cube - stage = stage_utils.get_current_stage() - for i in range(10): - # Remove objects added to stage by default - stage.RemovePrim(f"/World/Objects/Obj_{i:02d}") - # Add instanceable cubes - prim_utils.create_prim( - f"/World/Cube_{i}", - "Xform", - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - translation=(0.0, i, 5.0), - orientation=(1.0, 0.0, 0.0, 0.0), - scale=(5.0, 5.0, 5.0), - ) - prim = stage.GetPrimAtPath(f"/World/Cube_{i}") - sem = Semantics.SemanticsAPI.Apply(prim, "Semantics") - sem.CreateSemanticTypeAttr() - sem.CreateSemanticDataAttr() - sem.GetSemanticTypeAttr().Set("class") - sem.GetSemanticDataAttr().Set("cube") - - # Create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.height = 120 - camera_cfg.width = 80 - camera_cfg.data_types = all_annotator_types - camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" - camera_cfg.offset.pos = (0.0, 0.0, 5.5) - camera = TiledCamera(camera_cfg) - # Check simulation parameter is set correctly - self.assertTrue(self.sim.has_rtx_sensors()) - # Play sim - self.sim.reset() - # Check if camera is initialized - self.assertTrue(camera.is_initialized) - # Check if camera prim is set correctly and that it is a camera prim - self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_1/CameraSensor") - self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) - self.assertListEqual(sorted(camera.data.output.keys()), sorted(all_annotator_types)) - - # Check buffers that exists and have correct shapes - self.assertEqual(camera.data.pos_w.shape, (num_cameras, 3)) - self.assertEqual(camera.data.quat_w_ros.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_world.shape, (num_cameras, 4)) - self.assertEqual(camera.data.quat_w_opengl.shape, (num_cameras, 4)) - self.assertEqual(camera.data.intrinsic_matrices.shape, (num_cameras, 3, 3)) - self.assertEqual(camera.data.image_shape, (camera_cfg.height, camera_cfg.width)) - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Simulate physics - for _ in range(2): - # perform rendering - self.sim.step() - # update camera - camera.update(self.dt) - # check image data - for data_type, im_data in camera.data.output.items(): - if data_type in ["rgb", "normals"]: - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 3)) - elif data_type in [ - "rgba", - "semantic_segmentation", - "instance_segmentation_fast", - "instance_id_segmentation_fast", - ]: - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 4)) - # semantic_segmentation has mean 0.43 - # rgba has mean 0.38 - # instance_segmentation_fast has mean 0.42 - # instance_id_segmentation_fast has mean 0.55-0.62 - for i in range(num_cameras): - self.assertGreater((im_data[i] / 255.0).mean().item(), 0.3) - elif data_type in ["motion_vectors"]: - # motion vectors have mean 0.2 - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 2)) - for i in range(num_cameras): - self.assertGreater(im_data[i].abs().mean().item(), 0.15) - elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: - # depth has mean 2.7 - # distance_to_image_plane has mean 3.1 - self.assertEqual(im_data.shape, (num_cameras, camera_cfg.height, camera_cfg.width, 1)) - for i in range(num_cameras): - self.assertGreater(im_data[i].mean().item(), 2.5) - - # access image data and compare dtype - output = camera.data.output - info = camera.data.info - self.assertEqual(output["rgb"].dtype, torch.uint8) - self.assertEqual(output["rgba"].dtype, torch.uint8) - self.assertEqual(output["depth"].dtype, torch.float) - self.assertEqual(output["distance_to_camera"].dtype, torch.float) - self.assertEqual(output["distance_to_image_plane"].dtype, torch.float) - self.assertEqual(output["normals"].dtype, torch.float) - self.assertEqual(output["motion_vectors"].dtype, torch.float) - self.assertEqual(output["semantic_segmentation"].dtype, torch.uint8) - self.assertEqual(output["instance_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(output["instance_id_segmentation_fast"].dtype, torch.uint8) - self.assertEqual(type(info["semantic_segmentation"]), dict) - self.assertEqual(type(info["instance_segmentation_fast"]), dict) - self.assertEqual(type(info["instance_id_segmentation_fast"]), dict) - - del camera - - def test_throughput(self): - """Test tiled camera throughput.""" - - # create camera - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.height = 480 - camera_cfg.width = 640 - camera = TiledCamera(camera_cfg) - - # Play simulator - self.sim.reset() - - # Simulate for a few steps - # note: This is a workaround to ensure that the textures are loaded. - # Check "Known Issues" section in the documentation for more details. - for _ in range(5): - self.sim.step() - - # Simulate physics - for _ in range(5): - # perform rendering - self.sim.step() - # update camera - with Timer(f"Time taken for updating camera with shape {camera.image_shape}"): - camera.update(self.dt) - # Check image data - for im_type, im_data in camera.data.output.items(): - if im_type == "rgb": - self.assertEqual(im_data.shape, (1, camera_cfg.height, camera_cfg.width, 3)) - self.assertGreater((im_data / 255.0).mean().item(), 0.0) - elif im_type == "distance_to_camera": - self.assertEqual(im_data.shape, (1, camera_cfg.height, camera_cfg.width, 1)) - self.assertGreater(im_data.mean().item(), 0.0) - del camera - - def test_output_equal_to_usd_camera_intrinsics(self): - """ - 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. - """ - - # create cameras - offset_rot = (-0.1251, 0.3617, 0.8731, -0.3020) - offset_pos = (2.5, 2.5, 4.0) - intrinsics = [380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0] - # get camera cfgs - # TODO: add clipping range back, once correctly supported by tiled camera - camera_tiled_cfg = TiledCameraCfg( - prim_path="/World/Camera_tiled", - offset=TiledCameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), - spawn=sim_utils.PinholeCameraCfg.from_intrinsic_matrix( - intrinsic_matrix=intrinsics, - height=540, - width=960, - focal_length=38.0, - # clipping_range=(0.01, 20), - ), - height=540, - width=960, - data_types=["depth"], - ) - camera_usd_cfg = CameraCfg( - prim_path="/World/Camera_usd", - offset=CameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), - spawn=sim_utils.PinholeCameraCfg.from_intrinsic_matrix( - intrinsic_matrix=intrinsics, - height=540, - width=960, - focal_length=38.0, - # clipping_range=(0.01, 20), - ), - height=540, - width=960, - data_types=["distance_to_image_plane"], - ) - - # set aperture offsets to 0, as currently not supported for usd/ tiled camera - camera_tiled_cfg.spawn.horizontal_aperture_offset = 0 - camera_tiled_cfg.spawn.vertical_aperture_offset = 0 - camera_usd_cfg.spawn.horizontal_aperture_offset = 0 - camera_usd_cfg.spawn.vertical_aperture_offset = 0 - # init cameras - camera_tiled = TiledCamera(camera_tiled_cfg) - camera_usd = Camera(camera_usd_cfg) - - # play sim - self.sim.reset() - self.sim.play() - - # perform steps - for _ in range(5): - self.sim.step() - + assert im_data[i].mean() != 0.0 + elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(num_cameras): + assert im_data[i].mean() > 0.0 + + # access image data and compare dtype + output = camera.data.output + info = camera.data.info + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.uint8 + assert output["instance_segmentation_fast"].dtype == torch.uint8 + assert output["instance_id_segmentation_fast"].dtype == torch.uint8 + assert isinstance(info["semantic_segmentation"], dict) + assert isinstance(info["instance_segmentation_fast"], dict) + assert isinstance(info["instance_id_segmentation_fast"], dict) + + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + + num_cameras = 11 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.data_types = all_annotator_types + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert sorted(camera.data.output.keys()) == sorted(all_annotator_types) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() # update camera - camera_usd.update(self.dt) - camera_tiled.update(self.dt) - - # filter nan and inf from output - cam_tiled_output = camera_tiled.data.output["depth"].clone() - cam_usd_output = camera_usd.data.output["distance_to_image_plane"].clone() - cam_tiled_output[torch.isnan(cam_tiled_output)] = 0 - cam_tiled_output[torch.isinf(cam_tiled_output)] = 0 - cam_usd_output[torch.isnan(cam_usd_output)] = 0 - cam_usd_output[torch.isinf(cam_usd_output)] = 0 - - # check that both have the same intrinsic matrices - torch.testing.assert_close(camera_tiled.data.intrinsic_matrices[0], camera_usd.data.intrinsic_matrices[0]) - - # check the apertures - torch.testing.assert_close( - camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), - camera_tiled._sensor_prims[0].GetHorizontalApertureAttr().Get(), - ) - torch.testing.assert_close( - camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(), - camera_tiled._sensor_prims[0].GetVerticalApertureAttr().Get(), - ) - + camera.update(dt) # check image data - torch.testing.assert_close( - cam_tiled_output[..., 0], - cam_usd_output[..., 0], - atol=5e-5, - rtol=5e-6, + for data_type, im_data in camera.data.output.items(): + if data_type in ["rgb", "normals"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 3) + elif data_type in [ + "rgba", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) + for i in range(num_cameras): + assert (im_data[i] / 255.0).mean() > 0.0 + elif data_type in ["motion_vectors"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 2) + for i in range(num_cameras): + assert im_data[i].mean() != 0.0 + elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(num_cameras): + assert im_data[i].mean() > 0.0 + + # access image data and compare dtype + output = camera.data.output + info = camera.data.info + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.uint8 + assert output["instance_segmentation_fast"].dtype == torch.uint8 + assert output["instance_id_segmentation_fast"].dtype == torch.uint8 + assert isinstance(info["semantic_segmentation"], dict) + assert isinstance(info["instance_segmentation_fast"], dict) + assert isinstance(info["instance_id_segmentation_fast"], dict) + + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +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 = [ + "rgb", + "rgba", + "depth", + "distance_to_camera", + "distance_to_image_plane", + "normals", + "motion_vectors", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ] + + num_cameras = 10 + for i in range(num_cameras): + sim_utils.create_prim(f"/World/Origin_{i}", "Xform", translation=(0.0, i, 0.0)) + + # Create a stage with 10 instanceable cubes, where each camera points to one cube + stage = sim_utils.get_current_stage() + for i in range(10): + # Remove objects added to stage by default + stage.RemovePrim(f"/World/Objects/Obj_{i:02d}") + # Add instanceable cubes + sim_utils.create_prim( + f"/World/Cube_{i}", + "Xform", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + translation=(0.0, i, 5.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(5.0, 5.0, 5.0), ) - - del camera_tiled - del camera_usd - - def test_sensor_print(self): - """Test sensor print is working correctly.""" - # Create sensor - sensor = TiledCamera(cfg=self.camera_cfg) - # Play sim - self.sim.reset() - # print info - print(sensor) - - def test_frame_offset_small_resolution(self): - """Test frame offset issue with small resolution camera.""" - # Create sensor - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.height = 80 - camera_cfg.width = 80 - tiled_camera = TiledCamera(camera_cfg) - # play sim - self.sim.reset() - # simulate some steps first to make sure objects are settled - for i in range(100): - # step simulation - self.sim.step() - # update camera - tiled_camera.update(self.dt) - # collect image data - image_before = tiled_camera.data.output["rgb"].clone() / 255.0 - - # update scene - stage = stage_utils.get_current_stage() - for i in range(10): - prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") - color = Gf.Vec3f(0, 0, 0) - UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) - - # update rendering - self.sim.step() + prim = stage.GetPrimAtPath(f"/World/Cube_{i}") + sim_utils.add_labels(prim, labels=["cube"], instance_name="class") + + # Create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.height = 120 + camera_cfg.width = 80 + camera_cfg.data_types = all_annotator_types + camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" + camera_cfg.offset.pos = (0.0, 0.0, 5.5) + camera = TiledCamera(camera_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if camera is initialized + assert camera.is_initialized + # Check if camera prim is set correctly and that it is a camera prim + assert camera._sensor_prims[1].GetPath().pathString == "/World/Origin_1/CameraSensor" + assert isinstance(camera._sensor_prims[0], UsdGeom.Camera) + assert sorted(camera.data.output.keys()) == sorted(all_annotator_types) + + # Check buffers that exists and have correct shapes + assert camera.data.pos_w.shape == (num_cameras, 3) + assert camera.data.quat_w_ros.shape == (num_cameras, 4) + assert camera.data.quat_w_world.shape == (num_cameras, 4) + assert camera.data.quat_w_opengl.shape == (num_cameras, 4) + assert camera.data.intrinsic_matrices.shape == (num_cameras, 3, 3) + assert camera.data.image_shape == (camera_cfg.height, camera_cfg.width) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Simulate physics + for _ in range(2): + # perform rendering + sim.step() # update camera - tiled_camera.update(self.dt) - - # make sure the image is different - image_after = tiled_camera.data.output["rgb"].clone() / 255.0 - - # check difference is above threshold - self.assertGreater( - torch.abs(image_after - image_before).mean(), 0.05 - ) # images of same color should be below 0.001 - - def test_frame_offset_large_resolution(self): - """Test frame offset issue with large resolution camera.""" - # Create sensor - camera_cfg = copy.deepcopy(self.camera_cfg) - camera_cfg.height = 480 - camera_cfg.width = 480 - tiled_camera = TiledCamera(camera_cfg) - - # modify scene to be less stochastic - stage = stage_utils.get_current_stage() - for i in range(10): - prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") - color = Gf.Vec3f(1, 1, 1) - UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) - - # play sim - self.sim.reset() - # simulate some steps first to make sure objects are settled - for i in range(100): - # step simulation - self.sim.step() - # update camera - tiled_camera.update(self.dt) - # collect image data - image_before = tiled_camera.data.output["rgb"].clone() / 255.0 - - # update scene - for i in range(10): - prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") - color = Gf.Vec3f(0, 0, 0) - UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) - - # update rendering - self.sim.step() + camera.update(dt) + # check image data + for data_type, im_data in camera.data.output.items(): + if data_type in ["rgb", "normals"]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 3) + elif data_type in [ + "rgba", + "semantic_segmentation", + "instance_segmentation_fast", + "instance_id_segmentation_fast", + ]: + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 4) + # semantic_segmentation has mean 0.43 + # rgba has mean 0.38 + # instance_segmentation_fast has mean 0.42 + # instance_id_segmentation_fast has mean 0.55-0.62 + for i in range(num_cameras): + assert (im_data[i] / 255.0).mean() > 0.2 + elif data_type in ["motion_vectors"]: + # motion vectors have mean 0.2 + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 2) + for i in range(num_cameras): + assert (im_data[i].abs().mean()) > 0.15 + elif data_type in ["depth", "distance_to_camera", "distance_to_image_plane"]: + # depth has mean 2.7 + # distance_to_image_plane has mean 3.1 + assert im_data.shape == (num_cameras, camera_cfg.height, camera_cfg.width, 1) + for i in range(num_cameras): + assert im_data[i].mean() > 2.5 + + # access image data and compare dtype + output = camera.data.output + info = camera.data.info + assert output["rgb"].dtype == torch.uint8 + assert output["rgba"].dtype == torch.uint8 + assert output["depth"].dtype == torch.float + assert output["distance_to_camera"].dtype == torch.float + assert output["distance_to_image_plane"].dtype == torch.float + assert output["normals"].dtype == torch.float + assert output["motion_vectors"].dtype == torch.float + assert output["semantic_segmentation"].dtype == torch.uint8 + assert output["instance_segmentation_fast"].dtype == torch.uint8 + assert output["instance_id_segmentation_fast"].dtype == torch.uint8 + assert isinstance(info["semantic_segmentation"], dict) + assert isinstance(info["instance_segmentation_fast"], dict) + assert isinstance(info["instance_id_segmentation_fast"], dict) + + del camera + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.isaacsim_ci +def test_throughput(setup_camera, device): + """Test tiled camera throughput.""" + sim, camera_cfg, dt = setup_camera + # create camera + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.height = 480 + camera_cfg.width = 640 + camera = TiledCamera(camera_cfg) + + # Play simulator + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Simulate physics + for _ in range(5): + # perform rendering + sim.step() # update camera - tiled_camera.update(self.dt) - - # make sure the image is different - image_after = tiled_camera.data.output["rgb"].clone() / 255.0 - - # check difference is above threshold - self.assertGreater( - torch.abs(image_after - image_before).mean(), 0.05 - ) # images of same color should be below 0.001 - + with Timer(f"Time taken for updating camera with shape {camera.image_shape}"): + camera.update(dt) + # Check image data + for im_type, im_data in camera.data.output.items(): + if im_type == "rgb": + assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 3) + assert (im_data / 255.0).mean() > 0.0 + elif im_type == "distance_to_camera": + assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 1) + assert im_data.mean() > 0.0 + del camera + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_output_equal_to_usd_camera_intrinsics(setup_camera, device): """ - Helper functions. + 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. """ - - @staticmethod - def _populate_scene(): - """Add prims to the scene.""" - # Ground-plane - cfg = sim_utils.GroundPlaneCfg() - cfg.func("/World/defaultGroundPlane", cfg) - # Lights - cfg = sim_utils.SphereLightCfg() - cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) - cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) - # Random objects - random.seed(0) - for i in range(10): - # sample random position - position = np.random.rand(3) - np.asarray([0.05, 0.05, -1.0]) - position *= np.asarray([1.5, 1.5, 0.5]) - # create prim - prim_type = random.choice(["Cube", "Sphere", "Cylinder"]) - prim = prim_utils.create_prim( - f"/World/Objects/Obj_{i:02d}", - prim_type, - translation=position, - scale=(0.25, 0.25, 0.25), - semantic_label=prim_type, - ) - # cast to geom prim - geom_prim = getattr(UsdGeom, prim_type)(prim) - # set random color - color = Gf.Vec3f(random.random(), random.random(), random.random()) - geom_prim.CreateDisplayColorAttr() - geom_prim.GetDisplayColorAttr().Set([color]) - # add rigid properties - SingleGeometryPrim(f"/World/Objects/Obj_{i:02d}", collision=True) - SingleRigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) - - -if __name__ == "__main__": - run_tests() + sim, _, dt = setup_camera + # create cameras + offset_rot = (-0.1251, 0.3617, 0.8731, -0.3020) + offset_pos = (2.5, 2.5, 4.0) + intrinsics = [380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0] + # get camera cfgs + # TODO: add clipping range back, once correctly supported by tiled camera + camera_tiled_cfg = TiledCameraCfg( + prim_path="/World/Camera_tiled", + offset=TiledCameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), + spawn=sim_utils.PinholeCameraCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsics, + height=540, + width=960, + ), + height=540, + width=960, + data_types=["depth"], + ) + camera_usd_cfg = CameraCfg( + prim_path="/World/Camera_usd", + offset=CameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"), + spawn=sim_utils.PinholeCameraCfg.from_intrinsic_matrix( + intrinsic_matrix=intrinsics, + height=540, + width=960, + ), + height=540, + width=960, + data_types=["distance_to_image_plane"], + ) + + # set aperture offsets to 0, as currently not supported for usd/ tiled camera + camera_tiled_cfg.spawn.horizontal_aperture_offset = 0 + camera_tiled_cfg.spawn.vertical_aperture_offset = 0 + camera_usd_cfg.spawn.horizontal_aperture_offset = 0 + camera_usd_cfg.spawn.vertical_aperture_offset = 0 + # init cameras + camera_tiled = TiledCamera(camera_tiled_cfg) + camera_usd = Camera(camera_usd_cfg) + + # play sim + sim.reset() + sim.play() + + # perform steps + for _ in range(5): + sim.step() + + # update camera + camera_usd.update(dt) + camera_tiled.update(dt) + + # filter nan and inf from output + cam_tiled_output = camera_tiled.data.output["depth"].clone() + cam_usd_output = camera_usd.data.output["distance_to_image_plane"].clone() + cam_tiled_output[torch.isnan(cam_tiled_output)] = 0 + cam_tiled_output[torch.isinf(cam_tiled_output)] = 0 + cam_usd_output[torch.isnan(cam_usd_output)] = 0 + cam_usd_output[torch.isinf(cam_usd_output)] = 0 + + # check that both have the same intrinsic matrices + torch.testing.assert_close(camera_tiled.data.intrinsic_matrices[0], camera_usd.data.intrinsic_matrices[0]) + + # check the apertures + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(), + camera_tiled._sensor_prims[0].GetHorizontalApertureAttr().Get(), + ) + torch.testing.assert_close( + camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(), + camera_tiled._sensor_prims[0].GetVerticalApertureAttr().Get(), + ) + + # check image data + torch.testing.assert_close( + cam_tiled_output[..., 0], + cam_usd_output[..., 0], + atol=5e-5, + rtol=5e-6, + ) + + del camera_tiled + del camera_usd + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_sensor_print(setup_camera, device): + """Test sensor print is working correctly.""" + sim, camera_cfg, _ = setup_camera + # Create sensor + sensor = TiledCamera(cfg=camera_cfg) + # Play sim + sim.reset() + # print info + print(sensor) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.isaacsim_ci +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 + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.height = 80 + camera_cfg.width = 80 + camera_cfg.offset.pos = (0.0, 0.0, 0.5) + tiled_camera = TiledCamera(camera_cfg) + # play sim + sim.reset() + # simulate some steps first to make sure objects are settled + stage = sim_utils.get_current_stage() + for i in range(10): + prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") + UsdGeom.Gprim(prim).GetOrderedXformOps()[2].Set(Gf.Vec3d(1.0, 1.0, 1.0)) + for i in range(100): + # step simulation + sim.step() + # update camera + tiled_camera.update(dt) + # collect image data + image_before = tiled_camera.data.output["rgb"].clone() / 255.0 + + # update scene + for i in range(10): + prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") + color = Gf.Vec3f(0, 0, 0) + UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) + + # update rendering + sim.step() + # update camera + tiled_camera.update(dt) + + # make sure the image is different + image_after = tiled_camera.data.output["rgb"].clone() / 255.0 + + # check difference is above threshold + 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, device): + """Test frame offset issue with large resolution camera.""" + sim, camera_cfg, dt = setup_camera + # Create sensor + camera_cfg = copy.deepcopy(camera_cfg) + camera_cfg.height = 480 + camera_cfg.width = 480 + tiled_camera = TiledCamera(camera_cfg) + + # modify scene to be less stochastic + stage = sim_utils.get_current_stage() + for i in range(10): + prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") + color = Gf.Vec3f(1, 1, 1) + UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) + + # play sim + sim.reset() + # simulate some steps first to make sure objects are settled + for i in range(100): + # step simulation + sim.step() + # update camera + tiled_camera.update(dt) + # collect image data + image_before = tiled_camera.data.output["rgb"].clone() / 255.0 + + # update scene + for i in range(10): + prim = stage.GetPrimAtPath(f"/World/Objects/Obj_{i:02d}") + color = Gf.Vec3f(0, 0, 0) + UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([color]) + + # update rendering + sim.step() + # update camera + tiled_camera.update(dt) + + # make sure the image is different + image_after = tiled_camera.data.output["rgb"].clone() / 255.0 + + # check difference is above threshold + assert torch.abs(image_after - image_before).mean() > 0.01 # images of same color should be below 0.001 + + +""" +Helper functions. +""" + + +@staticmethod +def _populate_scene(): + """Add prims to the scene.""" + # Ground-plane + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/defaultGroundPlane", cfg) + # Lights + cfg = sim_utils.SphereLightCfg() + cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) + cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) + # Random objects + random.seed(0) + np.random.seed(0) + torch.manual_seed(0) + for i in range(10): + # sample random position + position = np.random.rand(3) - np.asarray([0.05, 0.05, -1.0]) + position *= np.asarray([1.5, 1.5, 0.5]) + # create prim + prim_type = random.choice(["Cube", "Sphere", "Cylinder"]) + prim = sim_utils.create_prim( + f"/World/Objects/Obj_{i:02d}", + prim_type, + translation=position, + scale=(0.25, 0.25, 0.25), + semantic_label=prim_type, + ) + # cast to geom prim + geom_prim = getattr(UsdGeom, prim_type)(prim) + # set random color + color = Gf.Vec3f(random.random(), random.random(), random.random()) + geom_prim.CreateDisplayColorAttr() + geom_prim.GetDisplayColorAttr().Set([color]) + # add rigid properties + SingleGeometryPrim(f"/World/Objects/Obj_{i:02d}", collision=True) + SingleRigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) diff --git a/source/isaaclab/test/sensors/test_tiled_camera_env.py b/source/isaaclab/test/sensors/test_tiled_camera_env.py index 9b0aaf37d2de..5c4d33f6a58e 100644 --- a/source/isaaclab/test/sensors/test_tiled_camera_env.py +++ b/source/isaaclab/test/sensors/test_tiled_camera_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,7 +8,7 @@ import argparse import sys -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # add argparse arguments parser = argparse.ArgumentParser( @@ -25,15 +25,14 @@ sys.argv[1:] = args_cli.unittest_args # launch the simulator -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app - +simulation_app = AppLauncher(headless=True, enable_cameras=True).app """Rest everything follows.""" -import gymnasium as gym import sys -import unittest + +import gymnasium as gym +import pytest import omni.usd @@ -44,106 +43,101 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestTiledCameraCartpole(unittest.TestCase): - """Test cases for all registered environments.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - cls.registered_tasks.append("Isaac-Cartpole-RGB-Camera-Direct-v0") - print(">>> All registered environments:", cls.registered_tasks) - - def test_tiled_resolutions_tiny(self): - """Define settings for resolution and number of environments""" - num_envs = 1024 - tile_widths = range(32, 48) - tile_heights = range(32, 48) - self._launch_tests(tile_widths, tile_heights, num_envs) - - def test_tiled_resolutions_small(self): - """Define settings for resolution and number of environments""" - num_envs = 300 - tile_widths = range(128, 156) - tile_heights = range(128, 156) - self._launch_tests(tile_widths, tile_heights, num_envs) - - def test_tiled_resolutions_medium(self): - """Define settings for resolution and number of environments""" - num_envs = 64 - tile_widths = range(320, 400, 20) - tile_heights = range(320, 400, 20) - self._launch_tests(tile_widths, tile_heights, num_envs) - - def test_tiled_resolutions_large(self): - """Define settings for resolution and number of environments""" - num_envs = 4 - tile_widths = range(480, 640, 40) - tile_heights = range(480, 640, 40) - self._launch_tests(tile_widths, tile_heights, num_envs) - - def test_tiled_resolutions_edge_cases(self): - """Define settings for resolution and number of environments""" - num_envs = 1000 - tile_widths = [12, 67, 93, 147] - tile_heights = [12, 67, 93, 147] - self._launch_tests(tile_widths, tile_heights, num_envs) - - def test_tiled_num_envs_edge_cases(self): - """Define settings for resolution and number of environments""" - num_envs = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 53, 359, 733, 927] - tile_widths = [67, 93, 147] - tile_heights = [67, 93, 147] - for n_envs in num_envs: - self._launch_tests(tile_widths, tile_heights, n_envs) - - """ - Helper functions. - """ - - def _launch_tests(self, tile_widths: int, tile_heights: int, num_envs: int): - """Run through different resolutions for tiled rendering""" - device = "cuda:0" - task_name = "Isaac-Cartpole-RGB-Camera-Direct-v0" - # iterate over all registered environments - for width in tile_widths: - for height in tile_heights: - with self.subTest(width=width, height=height): - # create a new stage - omni.usd.get_context().new_stage() - # parse configuration - env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg = parse_env_cfg( - task_name, device=device, num_envs=num_envs - ) - env_cfg.tiled_camera.width = width - env_cfg.tiled_camera.height = height - print(f">>> Running test for resolution: {width} x {height}") - # check environment - self._run_environment(env_cfg) - # close the environment - print(f">>> Closing environment: {task_name}") - print("-" * 80) - - def _run_environment(self, env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg): - """Run environment and capture a rendered image.""" - # create environment - env: ManagerBasedRLEnv | DirectRLEnv = gym.make("Isaac-Cartpole-RGB-Camera-Direct-v0", cfg=env_cfg) - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - env.sim.set_setting("/physics/cooking/ujitsoCollisionCooking", False) - - # reset environment - obs, _ = env.reset() - # save image - if args_cli.save_images: - save_images_to_file( - obs["policy"] + 0.93, - f"output_{env.num_envs}_{env_cfg.tiled_camera.width}x{env_cfg.tiled_camera.height}.png", - ) - - # close the environment - env.close() - - -if __name__ == "__main__": - run_tests() +@pytest.mark.skip(reason="Currently takes too long to run") +def test_tiled_resolutions_tiny(): + """Define settings for resolution and number of environments""" + num_envs = 1024 + tile_widths = range(32, 48) + tile_heights = range(32, 48) + _launch_tests(tile_widths, tile_heights, num_envs) + + +@pytest.mark.skip(reason="Currently takes too long to run") +def test_tiled_resolutions_small(): + """Define settings for resolution and number of environments""" + num_envs = 300 + tile_widths = range(128, 156) + tile_heights = range(128, 156) + _launch_tests(tile_widths, tile_heights, num_envs) + + +@pytest.mark.skip(reason="Currently takes too long to run") +def test_tiled_resolutions_medium(): + """Define settings for resolution and number of environments""" + num_envs = 64 + tile_widths = range(320, 400, 20) + tile_heights = range(320, 400, 20) + _launch_tests(tile_widths, tile_heights, num_envs) + + +@pytest.mark.skip(reason="Currently takes too long to run") +def test_tiled_resolutions_large(): + """Define settings for resolution and number of environments""" + num_envs = 4 + tile_widths = range(480, 640, 40) + tile_heights = range(480, 640, 40) + _launch_tests(tile_widths, tile_heights, num_envs) + + +@pytest.mark.skip(reason="Currently takes too long to run") +def test_tiled_resolutions_edge_cases(): + """Define settings for resolution and number of environments""" + num_envs = 1000 + tile_widths = [12, 67, 93, 147] + tile_heights = [12, 67, 93, 147] + _launch_tests(tile_widths, tile_heights, num_envs) + + +@pytest.mark.skip(reason="Currently takes too long to run") +def test_tiled_num_envs_edge_cases(): + """Define settings for resolution and number of environments""" + num_envs = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 53, 359, 733, 927] + tile_widths = [67, 93, 147] + tile_heights = [67, 93, 147] + for n_envs in num_envs: + _launch_tests(tile_widths, tile_heights, n_envs) + + +# Helper functions + + +def _launch_tests(tile_widths: range, tile_heights: range, num_envs: int): + """Run through different resolutions for tiled rendering""" + device = "cuda:0" + task_name = "Isaac-Cartpole-RGB-Camera-Direct-v0" + # iterate over all registered environments + for width in tile_widths: + for height in tile_heights: + # create a new stage + omni.usd.get_context().new_stage() + # parse configuration + env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + env_cfg.tiled_camera.width = width + env_cfg.tiled_camera.height = height + print(f">>> Running test for resolution: {width} x {height}") + # check environment + _run_environment(env_cfg) + # close the environment + print(f">>> Closing environment: {task_name}") + print("-" * 80) + + +def _run_environment(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg): + """Run environment and capture a rendered image.""" + # create environment + env: ManagerBasedRLEnv | DirectRLEnv = gym.make("Isaac-Cartpole-RGB-Camera-Direct-v0", cfg=env_cfg) + # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + env.sim.set_setting("/physics/cooking/ujitsoCollisionCooking", False) + + # reset environment + obs, _ = env.reset() + # save image + if args_cli.save_images: + save_images_to_file( + obs["policy"] + 0.93, + f"output_{env.num_envs}_{env_cfg.tiled_camera.width}x{env_cfg.tiled_camera.height}.png", + ) + + # close the environment + env.close() diff --git a/source/isaaclab/test/sim/check_meshes.py b/source/isaaclab/test/sim/check_meshes.py index 6788e652b1ba..705677281d3c 100644 --- a/source/isaaclab/test/sim/check_meshes.py +++ b/source/isaaclab/test/sim/check_meshes.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -35,13 +35,12 @@ """Rest everything follows.""" -import numpy as np import random + +import numpy as np import torch import tqdm -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils @@ -76,7 +75,7 @@ def design_scene(): # create new xform prims for all objects to be spawned under origins = define_origins(num_origins=4, spacing=5.5) for idx, origin in enumerate(origins): - prim_utils.create_prim(f"/World/Origin{idx:02d}", "Xform", translation=origin) + sim_utils.create_prim(f"/World/Origin{idx:02d}", "Xform", translation=origin) # spawn a red cone cfg_sphere = sim_utils.MeshSphereCfg( diff --git a/source/isaaclab/test/sim/test_build_simulation_context_headless.py b/source/isaaclab/test/sim/test_build_simulation_context_headless.py index 39fb0a0b6232..ebe059bed666 100644 --- a/source/isaaclab/test/sim/test_build_simulation_context_headless.py +++ b/source/isaaclab/test/sim/test_build_simulation_context_headless.py @@ -1,102 +1,93 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """ -This test has a lot of duplication with ``test_build_simulation_context_nonheadless.py``. This is intentional to ensure that the -tests are run in both headless and non-headless modes, and we currently can't re-build the simulation app in a script. +This test has a lot of duplication with ``test_build_simulation_context_nonheadless.py``. -If you need to make a change to this test, please make sure to also make the same change to ``test_build_simulation_context_nonheadless.py``. +This is intentional to ensure that the tests are run in both headless and non-headless modes, +and we currently can't re-build the simulation app in a script. +If you need to make a change to this test, please make sure to also make the same change to +``test_build_simulation_context_nonheadless.py``. """ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest - -from isaacsim.core.utils.prims import is_prim_path_valid +import pytest from isaaclab.sim.simulation_cfg import SimulationCfg from isaaclab.sim.simulation_context import build_simulation_context -class TestBuildSimulationContextHeadless(unittest.TestCase): - """Tests for simulation context builder with headless usecase.""" - - """ - Tests - """ - - def test_build_simulation_context_no_cfg(self): - """Test that the simulation context is built when no simulation cfg is passed in.""" - for gravity_enabled in (True, False): - for device in ("cuda:0", "cpu"): - for dt in (0.01, 0.1): - with self.subTest(gravity_enabled=gravity_enabled, device=device, dt=dt): - with build_simulation_context(gravity_enabled=gravity_enabled, device=device, dt=dt) as sim: - if gravity_enabled: - self.assertEqual(sim.cfg.gravity, (0.0, 0.0, -9.81)) - else: - self.assertEqual(sim.cfg.gravity, (0.0, 0.0, 0.0)) - - if device == "cuda:0": - self.assertEqual(sim.cfg.device, "cuda:0") - else: - self.assertEqual(sim.cfg.device, "cpu") - - self.assertEqual(sim.cfg.dt, dt) - - # Ensure that dome light didn't get added automatically as we are headless - self.assertFalse(is_prim_path_valid("/World/defaultDomeLight")) - - def test_build_simulation_context_ground_plane(self): - """Test that the simulation context is built with the correct ground plane.""" - for add_ground_plane in (True, False): - with self.subTest(add_ground_plane=add_ground_plane): - with build_simulation_context(add_ground_plane=add_ground_plane) as _: - # Ensure that ground plane got added - self.assertEqual(is_prim_path_valid("/World/defaultGroundPlane"), add_ground_plane) - - def test_build_simulation_context_auto_add_lighting(self): - """Test that the simulation context is built with the correct lighting.""" - for add_lighting in (True, False): - for auto_add_lighting in (True, False): - with self.subTest(add_lighting=add_lighting, auto_add_lighting=auto_add_lighting): - with build_simulation_context(add_lighting=add_lighting, auto_add_lighting=auto_add_lighting) as _: - if add_lighting: - # Ensure that dome light got added - self.assertTrue(is_prim_path_valid("/World/defaultDomeLight")) - else: - # Ensure that dome light didn't get added as there's no GUI - self.assertFalse(is_prim_path_valid("/World/defaultDomeLight")) - - def test_build_simulation_context_cfg(self): - """Test that the simulation context is built with the correct cfg and values don't get overridden.""" - dt = 0.001 - # Non-standard gravity - gravity = (0.0, 0.0, -1.81) - device = "cuda:0" - - cfg = SimulationCfg( - gravity=gravity, - device=device, - dt=dt, - ) - - with build_simulation_context(sim_cfg=cfg, gravity_enabled=False, dt=0.01, device="cpu") as sim: - self.assertEqual(sim.cfg.gravity, gravity) - self.assertEqual(sim.cfg.device, device) - self.assertEqual(sim.cfg.dt, dt) - - -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("gravity_enabled", [True, False]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("dt", [0.01, 0.1]) +@pytest.mark.isaacsim_ci +def test_build_simulation_context_no_cfg(gravity_enabled, device, dt): + """Test that the simulation context is built when no simulation cfg is passed in.""" + with build_simulation_context(gravity_enabled=gravity_enabled, device=device, dt=dt) as sim: + if gravity_enabled: + assert sim.cfg.gravity == (0.0, 0.0, -9.81) + else: + assert sim.cfg.gravity == (0.0, 0.0, 0.0) + + assert sim.cfg.device == device + assert sim.cfg.dt == dt + + # Ensure that dome light didn't get added automatically as we are headless + assert not sim.stage.GetPrimAtPath("/World/defaultDomeLight").IsValid() + + +@pytest.mark.parametrize("add_ground_plane", [True, False]) +@pytest.mark.isaacsim_ci +def test_build_simulation_context_ground_plane(add_ground_plane): + """Test that the simulation context is built with the correct ground plane.""" + with build_simulation_context(add_ground_plane=add_ground_plane) as sim: + # Ensure that ground plane got added + if add_ground_plane: + assert sim.stage.GetPrimAtPath("/World/defaultGroundPlane").IsValid() + else: + assert not sim.stage.GetPrimAtPath("/World/defaultGroundPlane").IsValid() + + +@pytest.mark.parametrize("add_lighting", [True, False]) +@pytest.mark.parametrize("auto_add_lighting", [True, False]) +@pytest.mark.isaacsim_ci +def test_build_simulation_context_auto_add_lighting(add_lighting, auto_add_lighting): + """Test that the simulation context is built with the correct lighting.""" + with build_simulation_context(add_lighting=add_lighting, auto_add_lighting=auto_add_lighting) as sim: + if add_lighting: + # Ensure that dome light got added + assert sim.stage.GetPrimAtPath("/World/defaultDomeLight").IsValid() + else: + # Ensure that dome light didn't get added as there's no GUI + assert not sim.stage.GetPrimAtPath("/World/defaultDomeLight").IsValid() + + +@pytest.mark.isaacsim_ci +def test_build_simulation_context_cfg(): + """Test that the simulation context is built with the correct cfg and values don't get overridden.""" + dt = 0.001 + # Non-standard gravity + gravity = (0.0, 0.0, -1.81) + device = "cuda:0" + + cfg = SimulationCfg( + gravity=gravity, + device=device, + dt=dt, + ) + + with build_simulation_context(sim_cfg=cfg, gravity_enabled=False, dt=0.01, device="cpu") as sim: + assert sim.cfg.gravity == gravity + assert sim.cfg.device == device + assert sim.cfg.dt == dt diff --git a/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py b/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py index c53bde2fd956..ae2203c43b70 100644 --- a/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py +++ b/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py @@ -1,102 +1,85 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""This test has a lot of duplication with ``test_build_simulation_context_headless.py``. This is intentional to ensure that the -tests are run in both headless and non-headless modes, and we currently can't re-build the simulation app in a script. +"""This test has a lot of duplication with ``test_build_simulation_context_headless.py``. -If you need to make a change to this test, please make sure to also make the same change to ``test_build_simulation_context_headless.py``. +This is intentional to ensure that the tests are run in both headless and non-headless modes, +and we currently can't re-build the simulation app in a script. +If you need to make a change to this test, please make sure to also make the same change to +``test_build_simulation_context_headless.py``. """ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=False) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest - -from isaacsim.core.utils.prims import is_prim_path_valid +import pytest from isaaclab.sim.simulation_cfg import SimulationCfg from isaaclab.sim.simulation_context import build_simulation_context -class TestBuildSimulationContextNonheadless(unittest.TestCase): - """Tests for simulation context builder with non-headless usecase.""" - - """ - Tests - """ - - def test_build_simulation_context_no_cfg(self): - """Test that the simulation context is built when no simulation cfg is passed in.""" - for gravity_enabled in (True, False): - for device in ("cuda:0", "cpu"): - for dt in (0.01, 0.1): - with self.subTest(gravity_enabled=gravity_enabled, device=device, dt=dt): - with build_simulation_context( - gravity_enabled=gravity_enabled, - device=device, - dt=dt, - ) as sim: - if gravity_enabled: - self.assertEqual(sim.cfg.gravity, (0.0, 0.0, -9.81)) - else: - self.assertEqual(sim.cfg.gravity, (0.0, 0.0, 0.0)) - - if device == "cuda:0": - self.assertEqual(sim.cfg.device, "cuda:0") - else: - self.assertEqual(sim.cfg.device, "cpu") - - self.assertEqual(sim.cfg.dt, dt) - - def test_build_simulation_context_ground_plane(self): - """Test that the simulation context is built with the correct ground plane.""" - for add_ground_plane in (True, False): - with self.subTest(add_ground_plane=add_ground_plane): - with build_simulation_context(add_ground_plane=add_ground_plane) as _: - # Ensure that ground plane got added - self.assertEqual(is_prim_path_valid("/World/defaultGroundPlane"), add_ground_plane) - - def test_build_simulation_context_auto_add_lighting(self): - """Test that the simulation context is built with the correct lighting.""" - for add_lighting in (True, False): - for auto_add_lighting in (True, False): - with self.subTest(add_lighting=add_lighting, auto_add_lighting=auto_add_lighting): - with build_simulation_context(add_lighting=add_lighting, auto_add_lighting=auto_add_lighting) as _: - if auto_add_lighting or add_lighting: - # Ensure that dome light got added - self.assertTrue(is_prim_path_valid("/World/defaultDomeLight")) - else: - # Ensure that dome light didn't get added - self.assertFalse(is_prim_path_valid("/World/defaultDomeLight")) - - def test_build_simulation_context_cfg(self): - """Test that the simulation context is built with the correct cfg and values don't get overridden.""" - dt = 0.001 - # Non-standard gravity - gravity = (0.0, 0.0, -1.81) - device = "cuda:0" - - cfg = SimulationCfg( - gravity=gravity, - device=device, - dt=dt, - ) - - with build_simulation_context(sim_cfg=cfg, gravity_enabled=False, dt=0.01, device="cpu") as sim: - self.assertEqual(sim.cfg.gravity, gravity) - self.assertEqual(sim.cfg.device, device) - self.assertEqual(sim.cfg.dt, dt) - - -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("gravity_enabled", [True, False]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("dt", [0.01, 0.1]) +def test_build_simulation_context_no_cfg(gravity_enabled, device, dt): + """Test that the simulation context is built when no simulation cfg is passed in.""" + with build_simulation_context(gravity_enabled=gravity_enabled, device=device, dt=dt) as sim: + if gravity_enabled: + assert sim.cfg.gravity == (0.0, 0.0, -9.81) + else: + assert sim.cfg.gravity == (0.0, 0.0, 0.0) + + assert sim.cfg.device == device + assert sim.cfg.dt == dt + + +@pytest.mark.parametrize("add_ground_plane", [True, False]) +def test_build_simulation_context_ground_plane(add_ground_plane): + """Test that the simulation context is built with the correct ground plane.""" + with build_simulation_context(add_ground_plane=add_ground_plane) as sim: + # Ensure that ground plane got added + if add_ground_plane: + assert sim.stage.GetPrimAtPath("/World/defaultGroundPlane").IsValid() + else: + assert not sim.stage.GetPrimAtPath("/World/defaultGroundPlane").IsValid() + + +@pytest.mark.parametrize("add_lighting", [True, False]) +@pytest.mark.parametrize("auto_add_lighting", [True, False]) +def test_build_simulation_context_auto_add_lighting(add_lighting, auto_add_lighting): + """Test that the simulation context is built with the correct lighting.""" + with build_simulation_context(add_lighting=add_lighting, auto_add_lighting=auto_add_lighting) as sim: + if auto_add_lighting or add_lighting: + # Ensure that dome light got added + assert sim.stage.GetPrimAtPath("/World/defaultDomeLight").IsValid() + else: + # Ensure that dome light didn't get added + assert not sim.stage.GetPrimAtPath("/World/defaultDomeLight").IsValid() + + +def test_build_simulation_context_cfg(): + """Test that the simulation context is built with the correct cfg and values don't get overridden.""" + dt = 0.001 + # Non-standard gravity + gravity = (0.0, 0.0, -1.81) + device = "cuda:0" + + cfg = SimulationCfg( + gravity=gravity, + device=device, + dt=dt, + ) + + with build_simulation_context(sim_cfg=cfg, gravity_enabled=False, dt=0.01, device="cpu") as sim: + assert sim.cfg.gravity == gravity + assert sim.cfg.device == device + assert sim.cfg.dt == dt diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index 56ab9fce3d3b..ea4529d293cd 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app @@ -16,16 +16,16 @@ import os import random import tempfile -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils +import pytest + import omni -from isaacsim.core.api.simulation_context import SimulationContext from pxr import UsdGeom, UsdPhysics +import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.sim.converters import MeshConverter, MeshConverterCfg -from isaaclab.sim.schemas import schemas_cfg +from isaaclab.sim.schemas import MESH_APPROXIMATION_TOKENS, schemas_cfg from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path @@ -39,270 +39,334 @@ def random_quaternion(): return (w, x, y, z) -class TestMeshConverter(unittest.TestCase): - """Test fixture for the MeshConverter class.""" - - @classmethod - def setUpClass(cls): - """Load assets for tests.""" - assets_dir = f"{ISAACLAB_NUCLEUS_DIR}/Tests/MeshConverter/duck" - # Create mapping of file endings to file paths that can be used by tests - cls.assets = { - "obj": f"{assets_dir}/duck.obj", - "stl": f"{assets_dir}/duck.stl", - "fbx": f"{assets_dir}/duck.fbx", - "mtl": f"{assets_dir}/duck.mtl", - "png": f"{assets_dir}/duckCM.png", - } - # Download all these locally - download_dir = tempfile.mkdtemp(suffix="_mesh_converter_test_assets") - for key, value in cls.assets.items(): - cls.assets[key] = retrieve_file_path(value, download_dir=download_dir) - - def setUp(self): - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.01 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - # cleanup stage and context - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Test fixtures. - """ - - def test_no_change(self): - """Call conversion twice on the same input asset. This should not generate a new USD file if the hash is the same.""" - # create an initial USD file from asset - mesh_config = MeshConverterCfg(asset_path=self.assets["obj"]) - mesh_converter = MeshConverter(mesh_config) - time_usd_file_created = os.stat(mesh_converter.usd_path).st_mtime_ns - - # no change to config only define the usd directory - new_config = mesh_config - new_config.usd_dir = mesh_converter.usd_dir - # convert to usd but this time in the same directory as previous step - new_mesh_converter = MeshConverter(new_config) - new_time_usd_file_created = os.stat(new_mesh_converter.usd_path).st_mtime_ns - - self.assertEqual(time_usd_file_created, new_time_usd_file_created) - - def test_config_change(self): - """Call conversion twice but change the config in the second call. This should generate a new USD file.""" - # create an initial USD file from asset - mesh_config = MeshConverterCfg(asset_path=self.assets["obj"]) - mesh_converter = MeshConverter(mesh_config) - time_usd_file_created = os.stat(mesh_converter.usd_path).st_mtime_ns - - # change the config - new_config = mesh_config - new_config.make_instanceable = not mesh_config.make_instanceable - # define the usd directory - new_config.usd_dir = mesh_converter.usd_dir - # convert to usd but this time in the same directory as previous step - new_mesh_converter = MeshConverter(new_config) - new_time_usd_file_created = os.stat(new_mesh_converter.usd_path).st_mtime_ns - - self.assertNotEqual(time_usd_file_created, new_time_usd_file_created) - - def test_convert_obj(self): - """Convert an OBJ file""" - mesh_config = MeshConverterCfg( - asset_path=self.assets["obj"], - scale=(random.uniform(0.1, 2.0), random.uniform(0.1, 2.0), random.uniform(0.1, 2.0)), - translation=(random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0)), - rotation=random_quaternion(), - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_conversion(mesh_converter) - - def test_convert_stl(self): - """Convert an STL file""" - mesh_config = MeshConverterCfg( - asset_path=self.assets["stl"], - scale=(random.uniform(0.1, 2.0), random.uniform(0.1, 2.0), random.uniform(0.1, 2.0)), - translation=(random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0)), - rotation=random_quaternion(), - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_conversion(mesh_converter) - - def test_convert_fbx(self): - """Convert an FBX file""" - mesh_config = MeshConverterCfg( - asset_path=self.assets["fbx"], - scale=(random.uniform(0.1, 2.0), random.uniform(0.1, 2.0), random.uniform(0.1, 2.0)), - translation=(random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0)), - rotation=random_quaternion(), - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_conversion(mesh_converter) - - def test_convert_default_xform_transforms(self): - """Convert an OBJ file and check that default xform transforms are applied correctly""" - mesh_config = MeshConverterCfg(asset_path=self.assets["obj"]) - mesh_converter = MeshConverter(mesh_config) - # check that mesh conversion is successful - self._check_mesh_conversion(mesh_converter) - - def test_collider_no_approximation(self): - """Convert an OBJ file using no approximation""" - collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) - mesh_config = MeshConverterCfg( - asset_path=self.assets["obj"], - collision_approximation="none", - collision_props=collision_props, - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_collider_settings(mesh_converter) - - def test_collider_convex_hull(self): - """Convert an OBJ file using convex hull approximation""" - collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) - mesh_config = MeshConverterCfg( - asset_path=self.assets["obj"], - collision_approximation="convexHull", - collision_props=collision_props, - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_collider_settings(mesh_converter) - - def test_collider_mesh_simplification(self): - """Convert an OBJ file using mesh simplification approximation""" - collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) - mesh_config = MeshConverterCfg( - asset_path=self.assets["obj"], - collision_approximation="meshSimplification", - collision_props=collision_props, - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_collider_settings(mesh_converter) - - def test_collider_mesh_bounding_cube(self): - """Convert an OBJ file using bounding cube approximation""" - collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) - mesh_config = MeshConverterCfg( - asset_path=self.assets["obj"], - collision_approximation="boundingCube", - collision_props=collision_props, - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_collider_settings(mesh_converter) - - def test_collider_mesh_bounding_sphere(self): - """Convert an OBJ file using bounding sphere""" - collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) - mesh_config = MeshConverterCfg( - asset_path=self.assets["obj"], - collision_approximation="boundingSphere", - collision_props=collision_props, - ) - mesh_converter = MeshConverter(mesh_config) - - # check that mesh conversion is successful - self._check_mesh_collider_settings(mesh_converter) - - def test_collider_mesh_no_collision(self): - """Convert an OBJ file using bounding sphere with collision disabled""" - collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=False) - mesh_config = MeshConverterCfg( - asset_path=self.assets["obj"], - collision_approximation="boundingSphere", - collision_props=collision_props, - ) - mesh_converter = MeshConverter(mesh_config) - # check that mesh conversion is successful - self._check_mesh_collider_settings(mesh_converter) - - """ - Helper functions. - """ - - def _check_mesh_conversion(self, mesh_converter: MeshConverter): - """Check that mesh is loadable and stage is valid.""" - # Load the mesh - prim_path = "/World/Object" - prim_utils.create_prim(prim_path, usd_path=mesh_converter.usd_path) - # Check prim can be properly spawned - self.assertTrue(prim_utils.is_prim_path_valid(prim_path)) - # Load a second time - prim_path = "/World/Object2" - prim_utils.create_prim(prim_path, usd_path=mesh_converter.usd_path) - # Check prim can be properly spawned - self.assertTrue(prim_utils.is_prim_path_valid(prim_path)) - - stage = omni.usd.get_context().get_stage() - # Check axis is z-up - axis = UsdGeom.GetStageUpAxis(stage) - self.assertEqual(axis, "Z") - # Check units is meters - units = UsdGeom.GetStageMetersPerUnit(stage) - self.assertEqual(units, 1.0) - - # Check mesh settings - pos = tuple(prim_utils.get_prim_at_path("/World/Object/geometry").GetAttribute("xformOp:translate").Get()) - self.assertEqual(pos, mesh_converter.cfg.translation) - quat = prim_utils.get_prim_at_path("/World/Object/geometry").GetAttribute("xformOp:orient").Get() - quat = (quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]) - self.assertEqual(quat, mesh_converter.cfg.rotation) - scale = tuple(prim_utils.get_prim_at_path("/World/Object/geometry").GetAttribute("xformOp:scale").Get()) - self.assertEqual(scale, mesh_converter.cfg.scale) - - def _check_mesh_collider_settings(self, mesh_converter: MeshConverter): - # Check prim can be properly spawned - prim_path = "/World/Object" - prim_utils.create_prim(prim_path, usd_path=mesh_converter.usd_path) - self.assertTrue(prim_utils.is_prim_path_valid(prim_path)) - - # Make uninstanceable to check collision settings - geom_prim = prim_utils.get_prim_at_path(prim_path + "/geometry") - # Check that instancing worked! - self.assertEqual(geom_prim.IsInstanceable(), mesh_converter.cfg.make_instanceable) - # Obtain mesh settings - geom_prim.SetInstanceable(False) - mesh_prim = prim_utils.get_prim_at_path(prim_path + "/geometry/mesh") - - # Check collision settings - # -- if collision is enabled, check that API is present - exp_collision_enabled = ( - mesh_converter.cfg.collision_props is not None and mesh_converter.cfg.collision_props.collision_enabled - ) - collision_api = UsdPhysics.CollisionAPI(mesh_prim) - collision_enabled = collision_api.GetCollisionEnabledAttr().Get() - self.assertEqual(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 +@pytest.fixture(scope="session") +def assets(): + """Load assets for tests.""" + assets_dir = f"{ISAACLAB_NUCLEUS_DIR}/Tests/MeshConverter/duck" + # Create mapping of file endings to file paths that can be used by tests + assets = { + "obj": f"{assets_dir}/duck.obj", + "stl": f"{assets_dir}/duck.stl", + "fbx": f"{assets_dir}/duck.fbx", + "mtl": f"{assets_dir}/duck.mtl", + "png": f"{assets_dir}/duckCM.png", + } + # Download all these locally + download_dir = tempfile.mkdtemp(suffix="_mesh_converter_test_assets") + for key, value in assets.items(): + assets[key] = retrieve_file_path(value, download_dir=download_dir) + return assets + + +@pytest.fixture(autouse=True) +def sim(): + """Create a blank new stage for each test.""" + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.01 + # Load kit helper + sim = SimulationContext(SimulationCfg(dt=dt)) + yield sim + # stop simulation + sim.stop() + # cleanup stage and context + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +def check_mesh_conversion(mesh_converter: MeshConverter): + """Check that mesh is loadable and stage is valid.""" + # Obtain stage handle + stage = sim_utils.get_current_stage() + + # Load the mesh + prim_path = "/World/Object" + sim_utils.create_prim(prim_path, usd_path=mesh_converter.usd_path) + # Check prim can be properly spawned + assert stage.GetPrimAtPath(prim_path).IsValid() + # Load a second time + prim_path = "/World/Object2" + sim_utils.create_prim(prim_path, usd_path=mesh_converter.usd_path) + # Check prim can be properly spawned + assert stage.GetPrimAtPath(prim_path).IsValid() + + stage = omni.usd.get_context().get_stage() + # Check axis is z-up + axis = UsdGeom.GetStageUpAxis(stage) + assert axis == "Z" + # Check units is meters + units = UsdGeom.GetStageMetersPerUnit(stage) + assert units == 1.0 + + # Obtain prim handle + prim = stage.GetPrimAtPath("/World/Object/geometry") + # Check mesh settings + pos = tuple(prim.GetAttribute("xformOp:translate").Get()) + assert pos == mesh_converter.cfg.translation + quat = prim.GetAttribute("xformOp:orient").Get() + quat = (quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]) + assert quat == mesh_converter.cfg.rotation + scale = tuple(prim.GetAttribute("xformOp:scale").Get()) + assert scale == mesh_converter.cfg.scale + + +def check_mesh_collider_settings(mesh_converter: MeshConverter): + """Check that mesh collider settings are correct.""" + # Obtain stage handle + stage = sim_utils.get_current_stage() + + # Check prim can be properly spawned + prim_path = "/World/Object" + sim_utils.create_prim(prim_path, usd_path=mesh_converter.usd_path) + assert stage.GetPrimAtPath(prim_path).IsValid() + + # Make uninstanceable to check collision settings + geom_prim = stage.GetPrimAtPath(prim_path + "/geometry") + # Check that instancing worked! + assert geom_prim.IsInstanceable() == mesh_converter.cfg.make_instanceable + # Obtain mesh settings + geom_prim.SetInstanceable(False) + mesh_prim = stage.GetPrimAtPath(prim_path + "/geometry/mesh") + + # Check collision settings + # -- if collision is enabled, check that API is present + exp_collision_enabled = ( + mesh_converter.cfg.collision_props is not None and mesh_converter.cfg.collision_props.collision_enabled + ) + collision_api = UsdPhysics.CollisionAPI(mesh_prim) + collision_enabled = collision_api.GetCollisionEnabledAttr().Get() + 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: + if mesh_converter.cfg.mesh_collision_props is not None: + exp_collision_approximation_str = mesh_converter.cfg.mesh_collision_props.mesh_approximation_name + exp_collision_approximation_token = MESH_APPROXIMATION_TOKENS[exp_collision_approximation_str] mesh_collision_api = UsdPhysics.MeshCollisionAPI(mesh_prim) collision_approximation = mesh_collision_api.GetApproximationAttr().Get() - self.assertEqual( - collision_approximation, exp_collision_approximation, "Collision approximation is not the same!" + # Convert token to string for comparison + assert collision_approximation == exp_collision_approximation_token, ( + "Collision approximation is not the same!" ) -if __name__ == "__main__": - run_tests() +def test_no_change(assets): + """Call conversion twice on the same input asset. + + This should not generate a new USD file if the hash is the same. + """ + # create an initial USD file from asset + mesh_config = MeshConverterCfg(asset_path=assets["obj"]) + mesh_converter = MeshConverter(mesh_config) + time_usd_file_created = os.stat(mesh_converter.usd_path).st_mtime_ns + + # no change to config only define the usd directory + new_config = mesh_config + new_config.usd_dir = mesh_converter.usd_dir + # convert to usd but this time in the same directory as previous step + new_mesh_converter = MeshConverter(new_config) + new_time_usd_file_created = os.stat(new_mesh_converter.usd_path).st_mtime_ns + + assert time_usd_file_created == new_time_usd_file_created + + +def test_config_change(assets): + """Call conversion twice but change the config in the second call. This should generate a new USD file.""" + # create an initial USD file from asset + mesh_config = MeshConverterCfg(asset_path=assets["obj"]) + mesh_converter = MeshConverter(mesh_config) + time_usd_file_created = os.stat(mesh_converter.usd_path).st_mtime_ns + + # change the config + new_config = mesh_config + new_config.make_instanceable = not mesh_config.make_instanceable + # define the usd directory + new_config.usd_dir = mesh_converter.usd_dir + # convert to usd but this time in the same directory as previous step + new_mesh_converter = MeshConverter(new_config) + new_time_usd_file_created = os.stat(new_mesh_converter.usd_path).st_mtime_ns + + assert time_usd_file_created != new_time_usd_file_created + + +def test_convert_obj(assets): + """Convert an OBJ file""" + mesh_config = MeshConverterCfg( + asset_path=assets["obj"], + scale=(random.uniform(0.1, 2.0), random.uniform(0.1, 2.0), random.uniform(0.1, 2.0)), + translation=(random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0)), + rotation=random_quaternion(), + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_conversion(mesh_converter) + + +def test_convert_stl(assets): + """Convert an STL file""" + mesh_config = MeshConverterCfg( + asset_path=assets["stl"], + scale=(random.uniform(0.1, 2.0), random.uniform(0.1, 2.0), random.uniform(0.1, 2.0)), + translation=(random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0)), + rotation=random_quaternion(), + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_conversion(mesh_converter) + + +def test_convert_fbx(assets): + """Convert an FBX file""" + mesh_config = MeshConverterCfg( + asset_path=assets["fbx"], + scale=(random.uniform(0.1, 2.0), random.uniform(0.1, 2.0), random.uniform(0.1, 2.0)), + translation=(random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0)), + rotation=random_quaternion(), + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_conversion(mesh_converter) + + +def test_convert_default_xform_transforms(assets): + """Convert an OBJ file and check that default xform transforms are applied correctly""" + mesh_config = MeshConverterCfg(asset_path=assets["obj"]) + mesh_converter = MeshConverter(mesh_config) + # check that mesh conversion is successful + check_mesh_conversion(mesh_converter) + + +def test_collider_no_approximation(assets): + """Convert an OBJ file using no approximation""" + collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_config = MeshConverterCfg( + asset_path=assets["obj"], + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +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"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +def test_collider_convex_decomposition(assets): + """Convert an OBJ file using convex decomposition approximation""" + collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_collision_prop = schemas_cfg.ConvexDecompositionPropertiesCfg() + mesh_config = MeshConverterCfg( + asset_path=assets["obj"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +def test_collider_triangle_mesh(assets): + """Convert an OBJ file using triangle mesh approximation""" + collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_collision_prop = schemas_cfg.TriangleMeshPropertiesCfg() + mesh_config = MeshConverterCfg( + asset_path=assets["obj"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +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"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +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"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +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"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +def test_collider_mesh_sdf(assets): + """Convert an OBJ file using signed distance field approximation""" + collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_collision_prop = schemas_cfg.SDFMeshPropertiesCfg() + mesh_config = MeshConverterCfg( + asset_path=assets["obj"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) + + +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"], + mesh_collision_props=mesh_collision_prop, + collision_props=collision_props, + ) + mesh_converter = MeshConverter(mesh_config) + # check that mesh conversion is successful + check_mesh_collider_settings(mesh_converter) diff --git a/source/isaaclab/test/sim/test_mjcf_converter.py b/source/isaaclab/test/sim/test_mjcf_converter.py index 89ebcf3abcc9..8ce098b4a51b 100644 --- a/source/isaaclab/test/sim/test_mjcf_converter.py +++ b/source/isaaclab/test/sim/test_mjcf_converter.py @@ -1,104 +1,104 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -config = {"headless": True} -simulation_app = AppLauncher(config).app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" - import os -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import pytest + from isaacsim.core.utils.extensions import enable_extension, get_extension_path_from_name +import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.sim.converters import MjcfConverter, MjcfConverterCfg -class TestMjcfConverter(unittest.TestCase): - """Test fixture for the MjcfConverter class.""" +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Setup and teardown for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + + # Setup: Create simulation context + dt = 0.01 + sim = SimulationContext(SimulationCfg(dt=dt)) + + # Setup: Create MJCF config + enable_extension("isaacsim.asset.importer.mjcf") + extension_path = get_extension_path_from_name("isaacsim.asset.importer.mjcf") + config = MjcfConverterCfg( + asset_path=f"{extension_path}/data/mjcf/nv_ant.xml", + import_sites=True, + fix_base=False, + make_instanceable=True, + ) - def setUp(self): - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # retrieve path to mjcf importer extension - enable_extension("isaacsim.asset.importer.mjcf") - extension_path = get_extension_path_from_name("isaacsim.asset.importer.mjcf") - # default configuration - self.config = MjcfConverterCfg( - asset_path=f"{extension_path}/data/mjcf/nv_ant.xml", - import_sites=True, - fix_base=False, - make_instanceable=True, - ) + # Yield the resources for the test + yield sim, config - # Simulation time-step - self.dt = 0.01 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") + # Teardown: Cleanup simulation + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - # cleanup stage and context - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - def test_no_change(self): - """Call conversion twice. This should not generate a new USD file.""" +@pytest.mark.isaacsim_ci +def test_no_change(test_setup_teardown): + """Call conversion twice. This should not generate a new USD file.""" + sim, mjcf_config = test_setup_teardown - mjcf_converter = MjcfConverter(self.config) - time_usd_file_created = os.stat(mjcf_converter.usd_path).st_mtime_ns + mjcf_converter = MjcfConverter(mjcf_config) + time_usd_file_created = os.stat(mjcf_converter.usd_path).st_mtime_ns - # no change to config only define the usd directory - new_config = self.config - new_config.usd_dir = mjcf_converter.usd_dir - # convert to usd but this time in the same directory as previous step - new_mjcf_converter = MjcfConverter(new_config) - new_time_usd_file_created = os.stat(new_mjcf_converter.usd_path).st_mtime_ns + # no change to config only define the usd directory + new_config = mjcf_config + new_config.usd_dir = mjcf_converter.usd_dir + # convert to usd but this time in the same directory as previous step + new_mjcf_converter = MjcfConverter(new_config) + new_time_usd_file_created = os.stat(new_mjcf_converter.usd_path).st_mtime_ns - self.assertEqual(time_usd_file_created, new_time_usd_file_created) + assert time_usd_file_created == new_time_usd_file_created - def test_config_change(self): - """Call conversion twice but change the config in the second call. This should generate a new USD file.""" - mjcf_converter = MjcfConverter(self.config) - time_usd_file_created = os.stat(mjcf_converter.usd_path).st_mtime_ns +@pytest.mark.isaacsim_ci +def test_config_change(test_setup_teardown): + """Call conversion twice but change the config in the second call. This should generate a new USD file.""" + sim, mjcf_config = test_setup_teardown - # change the config - new_config = self.config - new_config.fix_base = not self.config.fix_base - # define the usd directory - new_config.usd_dir = mjcf_converter.usd_dir - # convert to usd but this time in the same directory as previous step - new_mjcf_converter = MjcfConverter(new_config) - new_time_usd_file_created = os.stat(new_mjcf_converter.usd_path).st_mtime_ns + mjcf_converter = MjcfConverter(mjcf_config) + time_usd_file_created = os.stat(mjcf_converter.usd_path).st_mtime_ns - self.assertNotEqual(time_usd_file_created, new_time_usd_file_created) + # change the config + new_config = mjcf_config + new_config.fix_base = not mjcf_config.fix_base + # define the usd directory + new_config.usd_dir = mjcf_converter.usd_dir + # convert to usd but this time in the same directory as previous step + new_mjcf_converter = MjcfConverter(new_config) + new_time_usd_file_created = os.stat(new_mjcf_converter.usd_path).st_mtime_ns - def test_create_prim_from_usd(self): - """Call conversion and create a prim from it.""" + assert time_usd_file_created != new_time_usd_file_created - urdf_converter = MjcfConverter(self.config) - prim_path = "/World/Robot" - prim_utils.create_prim(prim_path, usd_path=urdf_converter.usd_path) +@pytest.mark.isaacsim_ci +def test_create_prim_from_usd(test_setup_teardown): + """Call conversion and create a prim from it.""" + sim, mjcf_config = test_setup_teardown - self.assertTrue(prim_utils.is_prim_path_valid(prim_path)) + urdf_converter = MjcfConverter(mjcf_config) + prim_path = "/World/Robot" + sim_utils.create_prim(prim_path, usd_path=urdf_converter.usd_path) -if __name__ == "__main__": - run_tests() + assert sim.stage.GetPrimAtPath(prim_path).IsValid() diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 0ab8e49b24b0..05710bd9228e 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app @@ -13,384 +13,397 @@ """Rest everything follows.""" import math -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import pytest + from pxr import UsdPhysics +import isaaclab.sim as sim_utils import isaaclab.sim.schemas as schemas -from isaaclab.sim.utils import find_global_fixed_joint_prim +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.string import to_camel_case -class TestPhysicsSchema(unittest.TestCase): - """Test fixture for checking schemas modifications through Isaac Lab.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - # Set some default values for test - self.arti_cfg = schemas.ArticulationRootPropertiesCfg( - enabled_self_collisions=False, - articulation_enabled=True, - solver_position_iteration_count=4, - solver_velocity_iteration_count=1, - sleep_threshold=1.0, - stabilization_threshold=5.0, - fix_root_link=False, - ) - self.rigid_cfg = schemas.RigidBodyPropertiesCfg( - rigid_body_enabled=True, - kinematic_enabled=False, - disable_gravity=False, - linear_damping=0.1, - angular_damping=0.5, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=10.0, - max_contact_impulse=10.0, - enable_gyroscopic_forces=True, - retain_accelerations=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=1, - sleep_threshold=1.0, - stabilization_threshold=6.0, - ) - self.collision_cfg = schemas.CollisionPropertiesCfg( - collision_enabled=True, - contact_offset=0.05, - rest_offset=0.001, - min_torsional_patch_radius=0.1, - torsional_patch_radius=1.0, - ) - self.mass_cfg = schemas.MassPropertiesCfg(mass=1.0, density=100.0) - self.joint_cfg = schemas.JointDrivePropertiesCfg( - drive_type="acceleration", max_effort=80.0, max_velocity=10.0, stiffness=10.0, damping=0.1 - ) +@pytest.fixture +def setup_simulation(): + """Fixture to set up and tear down the simulation context.""" + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.1 + # Load kit helper + sim = SimulationContext(SimulationCfg(dt=dt)) + # Set some default values for test + arti_cfg = schemas.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + articulation_enabled=True, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + sleep_threshold=1.0, + stabilization_threshold=5.0, + fix_root_link=False, + ) + rigid_cfg = schemas.RigidBodyPropertiesCfg( + rigid_body_enabled=True, + kinematic_enabled=False, + disable_gravity=False, + linear_damping=0.1, + angular_damping=0.5, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=10.0, + max_contact_impulse=10.0, + enable_gyroscopic_forces=True, + retain_accelerations=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=1, + sleep_threshold=1.0, + stabilization_threshold=6.0, + ) + collision_cfg = schemas.CollisionPropertiesCfg( + collision_enabled=True, + contact_offset=0.05, + rest_offset=0.001, + min_torsional_patch_radius=0.1, + torsional_patch_radius=1.0, + ) + mass_cfg = schemas.MassPropertiesCfg(mass=1.0, density=100.0) + joint_cfg = schemas.JointDrivePropertiesCfg( + drive_type="acceleration", max_effort=80.0, max_velocity=10.0, stiffness=10.0, damping=0.1 + ) + yield sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg + # Teardown + sim._disable_app_control_on_stop_handle = True # prevent timeout + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_valid_properties_cfg(setup_simulation): + """Test that all the config instances have non-None values. + + This is to ensure that we check that all the properties of the schema are set. + """ + sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg = setup_simulation + for cfg in [arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg]: + # check nothing is none + for k, v in cfg.__dict__.items(): + assert v is not None, f"{cfg.__class__.__name__}:{k} is None. Please make sure schemas are valid." + + +@pytest.mark.isaacsim_ci +def test_modify_properties_on_invalid_prim(setup_simulation): + """Test modifying properties on a prim that does not exist.""" + sim, _, rigid_cfg, _, _, _ = setup_simulation + # set properties + with pytest.raises(ValueError): + schemas.modify_rigid_body_properties("/World/asset_xyz", rigid_cfg) - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - def test_valid_properties_cfg(self): - """Test that all the config instances have non-None values. - - This is to ensure that we check that all the properties of the schema are set. - """ - for cfg in [self.arti_cfg, self.rigid_cfg, self.collision_cfg, self.mass_cfg, self.joint_cfg]: - # check nothing is none - for k, v in cfg.__dict__.items(): - self.assertIsNotNone(v, f"{cfg.__class__.__name__}:{k} is None. Please make sure schemas are valid.") - - def test_modify_properties_on_invalid_prim(self): - """Test modifying properties on a prim that does not exist.""" - # set properties - with self.assertRaises(ValueError): - schemas.modify_rigid_body_properties("/World/asset_xyz", self.rigid_cfg) - - def test_modify_properties_on_articulation_instanced_usd(self): - """Test modifying properties on articulation instanced usd. - - In this case, modifying collision properties on the articulation instanced usd will fail. - """ - # spawn asset to the stage - asset_usd_file = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_instanceable.usd" - prim_utils.create_prim("/World/asset_instanced", usd_path=asset_usd_file, translation=(0.0, 0.0, 0.62)) - - # set properties on the asset and check all properties are set - schemas.modify_articulation_root_properties("/World/asset_instanced", self.arti_cfg) - schemas.modify_rigid_body_properties("/World/asset_instanced", self.rigid_cfg) - schemas.modify_mass_properties("/World/asset_instanced", self.mass_cfg) - schemas.modify_joint_drive_properties("/World/asset_instanced", self.joint_cfg) - # validate the properties - self._validate_articulation_properties_on_prim("/World/asset_instanced", has_default_fixed_root=False) - self._validate_rigid_body_properties_on_prim("/World/asset_instanced") - self._validate_mass_properties_on_prim("/World/asset_instanced") - self._validate_joint_drive_properties_on_prim("/World/asset_instanced") - - # make a fixed joint - # note: for this asset, it doesn't work because the root is not a rigid body - self.arti_cfg.fix_root_link = True - with self.assertRaises(NotImplementedError): - schemas.modify_articulation_root_properties("/World/asset_instanced", self.arti_cfg) - - def test_modify_properties_on_articulation_usd(self): - """Test setting properties on articulation usd.""" - # spawn asset to the stage - asset_usd_file = f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka.usd" - prim_utils.create_prim("/World/asset", usd_path=asset_usd_file, translation=(0.0, 0.0, 0.62)) - - # set properties on the asset and check all properties are set - schemas.modify_articulation_root_properties("/World/asset", self.arti_cfg) - schemas.modify_rigid_body_properties("/World/asset", self.rigid_cfg) - schemas.modify_collision_properties("/World/asset", self.collision_cfg) - schemas.modify_mass_properties("/World/asset", self.mass_cfg) - schemas.modify_joint_drive_properties("/World/asset", self.joint_cfg) - # validate the properties - self._validate_articulation_properties_on_prim("/World/asset", has_default_fixed_root=True) - self._validate_rigid_body_properties_on_prim("/World/asset") - self._validate_collision_properties_on_prim("/World/asset") - self._validate_mass_properties_on_prim("/World/asset") - self._validate_joint_drive_properties_on_prim("/World/asset") - - # make a fixed joint - self.arti_cfg.fix_root_link = True - schemas.modify_articulation_root_properties("/World/asset", self.arti_cfg) - # validate the properties - self._validate_articulation_properties_on_prim("/World/asset", has_default_fixed_root=True) - - def test_defining_rigid_body_properties_on_prim(self): - """Test defining rigid body properties on a prim.""" - # create a prim - prim_utils.create_prim("/World/parent", prim_type="XForm") - # spawn a prim - prim_utils.create_prim("/World/cube1", prim_type="Cube", translation=(0.0, 0.0, 0.62)) - # set properties on the asset and check all properties are set - schemas.define_rigid_body_properties("/World/cube1", self.rigid_cfg) - schemas.define_collision_properties("/World/cube1", self.collision_cfg) - schemas.define_mass_properties("/World/cube1", self.mass_cfg) - # validate the properties - self._validate_rigid_body_properties_on_prim("/World/cube1") - self._validate_collision_properties_on_prim("/World/cube1") - self._validate_mass_properties_on_prim("/World/cube1") - - # spawn another prim - prim_utils.create_prim("/World/cube2", prim_type="Cube", translation=(1.0, 1.0, 0.62)) - # set properties on the asset and check all properties are set - schemas.define_rigid_body_properties("/World/cube2", self.rigid_cfg) - schemas.define_collision_properties("/World/cube2", self.collision_cfg) - # validate the properties - self._validate_rigid_body_properties_on_prim("/World/cube2") - self._validate_collision_properties_on_prim("/World/cube2") - - # check if we can play - self.sim.reset() - for _ in range(100): - self.sim.step() - - def test_defining_articulation_properties_on_prim(self): - """Test defining articulation properties on a prim.""" - # create a parent articulation - prim_utils.create_prim("/World/parent", prim_type="Xform") - schemas.define_articulation_root_properties("/World/parent", self.arti_cfg) - # validate the properties - self._validate_articulation_properties_on_prim("/World/parent", has_default_fixed_root=False) - - # create a child articulation - prim_utils.create_prim("/World/parent/child", prim_type="Cube", translation=(0.0, 0.0, 0.62)) - schemas.define_rigid_body_properties("/World/parent/child", self.rigid_cfg) - schemas.define_mass_properties("/World/parent/child", self.mass_cfg) - - # check if we can play - self.sim.reset() - for _ in range(100): - self.sim.step() +@pytest.mark.isaacsim_ci +def test_modify_properties_on_articulation_instanced_usd(setup_simulation): + """Test modifying properties on articulation instanced usd. + + In this case, modifying collision properties on the articulation instanced usd will fail. """ - Helper functions. + sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg = setup_simulation + # spawn asset to the stage + asset_usd_file = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_c/anymal_c.usd" + if "4.5" in ISAAC_NUCLEUS_DIR: + asset_usd_file = asset_usd_file.replace("http", "https").replace("4.5", "5.0") + sim_utils.create_prim("/World/asset_instanced", usd_path=asset_usd_file, translation=(0.0, 0.0, 0.62)) + + # set properties on the asset and check all properties are set + schemas.modify_articulation_root_properties("/World/asset_instanced", arti_cfg) + schemas.modify_rigid_body_properties("/World/asset_instanced", rigid_cfg) + schemas.modify_mass_properties("/World/asset_instanced", mass_cfg) + schemas.modify_joint_drive_properties("/World/asset_instanced", joint_cfg) + # validate the properties + _validate_articulation_properties_on_prim("/World/asset_instanced/base", arti_cfg, False) + _validate_rigid_body_properties_on_prim("/World/asset_instanced", rigid_cfg) + _validate_mass_properties_on_prim("/World/asset_instanced", mass_cfg) + _validate_joint_drive_properties_on_prim("/World/asset_instanced", joint_cfg) + + # make a fixed joint + arti_cfg.fix_root_link = True + schemas.modify_articulation_root_properties("/World/asset_instanced", arti_cfg) + + +@pytest.mark.isaacsim_ci +def test_modify_properties_on_articulation_usd(setup_simulation): + """Test setting properties on articulation usd.""" + sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg = setup_simulation + # spawn asset to the stage + asset_usd_file = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" + if "4.5" in ISAAC_NUCLEUS_DIR: + asset_usd_file = asset_usd_file.replace("http", "https").replace("4.5", "5.0") + sim_utils.create_prim("/World/asset", usd_path=asset_usd_file, translation=(0.0, 0.0, 0.62)) + + # set properties on the asset and check all properties are set + schemas.modify_articulation_root_properties("/World/asset", arti_cfg) + schemas.modify_rigid_body_properties("/World/asset", rigid_cfg) + schemas.modify_collision_properties("/World/asset", collision_cfg) + schemas.modify_mass_properties("/World/asset", mass_cfg) + schemas.modify_joint_drive_properties("/World/asset", joint_cfg) + # validate the properties + _validate_articulation_properties_on_prim("/World/asset", arti_cfg, True) + _validate_rigid_body_properties_on_prim("/World/asset", rigid_cfg) + _validate_collision_properties_on_prim("/World/asset", collision_cfg) + _validate_mass_properties_on_prim("/World/asset", mass_cfg) + _validate_joint_drive_properties_on_prim("/World/asset", joint_cfg) + + # make a fixed joint + arti_cfg.fix_root_link = True + schemas.modify_articulation_root_properties("/World/asset", arti_cfg) + # validate the properties + _validate_articulation_properties_on_prim("/World/asset", arti_cfg, True) + + +@pytest.mark.isaacsim_ci +def test_defining_rigid_body_properties_on_prim(setup_simulation): + """Test defining rigid body properties on a prim.""" + sim, _, rigid_cfg, collision_cfg, mass_cfg, _ = setup_simulation + # create a prim + sim_utils.create_prim("/World/parent", prim_type="XForm") + # spawn a prim + sim_utils.create_prim("/World/cube1", prim_type="Cube", translation=(0.0, 0.0, 0.62)) + # set properties on the asset and check all properties are set + schemas.define_rigid_body_properties("/World/cube1", rigid_cfg) + schemas.define_collision_properties("/World/cube1", collision_cfg) + schemas.define_mass_properties("/World/cube1", mass_cfg) + # validate the properties + _validate_rigid_body_properties_on_prim("/World/cube1", rigid_cfg) + _validate_collision_properties_on_prim("/World/cube1", collision_cfg) + _validate_mass_properties_on_prim("/World/cube1", mass_cfg) + + # spawn another prim + sim_utils.create_prim("/World/cube2", prim_type="Cube", translation=(1.0, 1.0, 0.62)) + # set properties on the asset and check all properties are set + schemas.define_rigid_body_properties("/World/cube2", rigid_cfg) + schemas.define_collision_properties("/World/cube2", collision_cfg) + # validate the properties + _validate_rigid_body_properties_on_prim("/World/cube2", rigid_cfg) + _validate_collision_properties_on_prim("/World/cube2", collision_cfg) + + # check if we can play + sim.reset() + for _ in range(100): + sim.step() + + +@pytest.mark.isaacsim_ci +def test_defining_articulation_properties_on_prim(setup_simulation): + """Test defining articulation properties on a prim.""" + sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, _ = setup_simulation + # create a parent articulation + sim_utils.create_prim("/World/parent", prim_type="Xform") + schemas.define_articulation_root_properties("/World/parent", arti_cfg) + # validate the properties + _validate_articulation_properties_on_prim("/World/parent", arti_cfg, False) + + # create a child articulation + sim_utils.create_prim("/World/parent/child", prim_type="Cube", translation=(0.0, 0.0, 0.62)) + schemas.define_rigid_body_properties("/World/parent/child", rigid_cfg) + schemas.define_mass_properties("/World/parent/child", mass_cfg) + + # check if we can play + sim.reset() + for _ in range(100): + sim.step() + + +""" +Helper functions. +""" + + +def _validate_articulation_properties_on_prim( + prim_path: str, arti_cfg, has_default_fixed_root: bool, verbose: bool = False +): + """Validate the articulation properties on the prim. + + If :attr:`has_default_fixed_root` is True, then the asset already has a fixed root link. This is used to check the + expected behavior of the fixed root link configuration. """ - - def _validate_articulation_properties_on_prim( - self, prim_path: str, has_default_fixed_root: False, verbose: bool = False - ): - """Validate the articulation properties on the prim. - - If :attr:`has_default_fixed_root` is True, then the asset already has a fixed root link. This is used to check the - expected behavior of the fixed root link configuration. - """ - # the root prim - root_prim = prim_utils.get_prim_at_path(prim_path) - # check articulation properties are set correctly - for attr_name, attr_value in self.arti_cfg.__dict__.items(): - # skip names we know are not present - if attr_name == "func": - continue - # handle fixed root link - if attr_name == "fix_root_link" and attr_value is not None: - # obtain the fixed joint prim - fixed_joint_prim = find_global_fixed_joint_prim(prim_path) - # if asset does not have a fixed root link then check if the joint is created - if not has_default_fixed_root: - if attr_value: - self.assertIsNotNone(fixed_joint_prim) - else: - self.assertIsNone(fixed_joint_prim) + # Obtain stage handle + stage = sim_utils.get_current_stage() + # the root prim + root_prim = stage.GetPrimAtPath(prim_path) + # check articulation properties are set correctly + for attr_name, attr_value in arti_cfg.__dict__.items(): + # skip names we know are not present + if attr_name == "func": + continue + # handle fixed root link + if attr_name == "fix_root_link" and attr_value is not None: + # obtain the fixed joint prim + fixed_joint_prim = sim_utils.find_global_fixed_joint_prim(prim_path) + # if asset does not have a fixed root link then check if the joint is created + if not has_default_fixed_root: + if attr_value: + assert fixed_joint_prim is not None else: - # check a joint exists - self.assertIsNotNone(fixed_joint_prim) - # check if the joint is enabled or disabled - is_enabled = fixed_joint_prim.GetJointEnabledAttr().Get() - self.assertEqual(is_enabled, attr_value) - # skip the rest of the checks - continue - # convert attribute name in prim to cfg name - prim_prop_name = f"physxArticulation:{to_camel_case(attr_name, to='cC')}" - # validate the values - self.assertAlmostEqual( - root_prim.GetAttribute(prim_prop_name).Get(), - attr_value, - places=5, - msg=f"Failed setting for {prim_prop_name}", - ) - - def _validate_rigid_body_properties_on_prim(self, prim_path: str, verbose: bool = False): - """Validate the rigid body properties on the prim. - - Note: - Right now this function exploits the hierarchy in the asset to check the properties. This is not a - fool-proof way of checking the properties. - """ - # the root prim - root_prim = prim_utils.get_prim_at_path(prim_path) - # check rigid body properties are set correctly - for link_prim in root_prim.GetChildren(): - if UsdPhysics.RigidBodyAPI(link_prim): - for attr_name, attr_value in self.rigid_cfg.__dict__.items(): + assert fixed_joint_prim is None + else: + # check a joint exists + assert fixed_joint_prim is not None + # check if the joint is enabled or disabled + is_enabled = fixed_joint_prim.GetJointEnabledAttr().Get() + assert is_enabled == attr_value + # skip the rest of the checks + continue + # convert attribute name in prim to cfg name + prim_prop_name = f"physxArticulation:{to_camel_case(attr_name, to='cC')}" + # validate the values + assert root_prim.GetAttribute(prim_prop_name).Get() == pytest.approx(attr_value, abs=1e-5), ( + f"Failed setting for {prim_prop_name}" + ) + + +def _validate_rigid_body_properties_on_prim(prim_path: str, rigid_cfg, verbose: bool = False): + """Validate the rigid body properties on the prim. + + Note: + Right now this function exploits the hierarchy in the asset to check the properties. This is not a + fool-proof way of checking the properties. + """ + # Obtain stage handle + stage = sim_utils.get_current_stage() + # the root prim + root_prim = stage.GetPrimAtPath(prim_path) + # check rigid body properties are set correctly + for link_prim in root_prim.GetChildren(): + if UsdPhysics.RigidBodyAPI(link_prim): + for attr_name, attr_value in rigid_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func", "rigid_body_enabled", "kinematic_enabled"]: + continue + # convert attribute name in prim to cfg name + prim_prop_name = f"physxRigidBody:{to_camel_case(attr_name, to='cC')}" + # validate the values + assert link_prim.GetAttribute(prim_prop_name).Get() == pytest.approx(attr_value, abs=1e-5), ( + f"Failed setting for {prim_prop_name}" + ) + elif verbose: + print(f"Skipping prim {link_prim.GetPrimPath()} as it is not a rigid body.") + + +def _validate_collision_properties_on_prim(prim_path: str, collision_cfg, verbose: bool = False): + """Validate the collision properties on the prim. + + Note: + Right now this function exploits the hierarchy in the asset to check the properties. This is not a + fool-proof way of checking the properties. + """ + # Obtain stage handle + stage = sim_utils.get_current_stage() + # the root prim + root_prim = stage.GetPrimAtPath(prim_path) + # check collision properties are set correctly + for link_prim in root_prim.GetChildren(): + for mesh_prim in link_prim.GetChildren(): + if UsdPhysics.CollisionAPI(mesh_prim): + for attr_name, attr_value in collision_cfg.__dict__.items(): # skip names we know are not present - if attr_name in ["func", "rigid_body_enabled", "kinematic_enabled"]: + if attr_name in ["func", "collision_enabled"]: continue # convert attribute name in prim to cfg name - prim_prop_name = f"physxRigidBody:{to_camel_case(attr_name, to='cC')}" + prim_prop_name = f"physxCollision:{to_camel_case(attr_name, to='cC')}" # validate the values - self.assertAlmostEqual( - link_prim.GetAttribute(prim_prop_name).Get(), - attr_value, - places=5, - msg=f"Failed setting for {prim_prop_name}", + assert mesh_prim.GetAttribute(prim_prop_name).Get() == pytest.approx(attr_value, abs=1e-5), ( + f"Failed setting for {prim_prop_name}" ) elif verbose: - print(f"Skipping prim {link_prim.GetPrimPath()} as it is not a rigid body.") - - def _validate_collision_properties_on_prim(self, prim_path: str, verbose: bool = False): - """Validate the collision properties on the prim. - - Note: - Right now this function exploits the hierarchy in the asset to check the properties. This is not a - fool-proof way of checking the properties. - """ - # the root prim - root_prim = prim_utils.get_prim_at_path(prim_path) - # check collision properties are set correctly - for link_prim in root_prim.GetChildren(): - for mesh_prim in link_prim.GetChildren(): - if UsdPhysics.CollisionAPI(mesh_prim): - for attr_name, attr_value in self.collision_cfg.__dict__.items(): - # skip names we know are not present - if attr_name in ["func", "collision_enabled"]: - continue - # convert attribute name in prim to cfg name - prim_prop_name = f"physxCollision:{to_camel_case(attr_name, to='cC')}" - # validate the values - self.assertAlmostEqual( - mesh_prim.GetAttribute(prim_prop_name).Get(), - attr_value, - places=5, - msg=f"Failed setting for {prim_prop_name}", - ) - elif verbose: - print(f"Skipping prim {mesh_prim.GetPrimPath()} as it is not a collision mesh.") - - def _validate_mass_properties_on_prim(self, prim_path: str, verbose: bool = False): - """Validate the mass properties on the prim. - - Note: - Right now this function exploits the hierarchy in the asset to check the properties. This is not a - fool-proof way of checking the properties. - """ - # the root prim - root_prim = prim_utils.get_prim_at_path(prim_path) - # check rigid body mass properties are set correctly - for link_prim in root_prim.GetChildren(): - if UsdPhysics.MassAPI(link_prim): - for attr_name, attr_value in self.mass_cfg.__dict__.items(): + print(f"Skipping prim {mesh_prim.GetPrimPath()} as it is not a collision mesh.") + + +def _validate_mass_properties_on_prim(prim_path: str, mass_cfg, verbose: bool = False): + """Validate the mass properties on the prim. + + Note: + Right now this function exploits the hierarchy in the asset to check the properties. This is not a + fool-proof way of checking the properties. + """ + # Obtain stage handle + stage = sim_utils.get_current_stage() + # the root prim + root_prim = stage.GetPrimAtPath(prim_path) + # check rigid body mass properties are set correctly + for link_prim in root_prim.GetChildren(): + if UsdPhysics.MassAPI(link_prim): + for attr_name, attr_value in mass_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func"]: + continue + # print(link_prim.GetProperties()) + prim_prop_name = f"physics:{to_camel_case(attr_name, to='cC')}" + # validate the values + assert link_prim.GetAttribute(prim_prop_name).Get() == pytest.approx(attr_value, abs=1e-5), ( + f"Failed setting for {prim_prop_name}" + ) + elif verbose: + print(f"Skipping prim {link_prim.GetPrimPath()} as it is not a mass api.") + + +def _validate_joint_drive_properties_on_prim(prim_path: str, joint_cfg, verbose: bool = False): + """Validate the mass properties on the prim. + + Note: + Right now this function exploits the hierarchy in the asset to check the properties. This is not a + fool-proof way of checking the properties. + """ + # Obtain stage handle + stage = sim_utils.get_current_stage() + # the root prim + root_prim = stage.GetPrimAtPath(prim_path) + # check joint drive properties are set correctly + for link_prim in root_prim.GetAllChildren(): + for joint_prim in link_prim.GetChildren(): + if joint_prim.IsA(UsdPhysics.PrismaticJoint) or joint_prim.IsA(UsdPhysics.RevoluteJoint): + # check it has drive API + assert joint_prim.HasAPI(UsdPhysics.DriveAPI) + # iterate over the joint properties + for attr_name, attr_value in joint_cfg.__dict__.items(): # skip names we know are not present - if attr_name in ["func"]: + if attr_name == "func": + continue + # resolve the drive (linear or angular) + drive_model = "linear" if joint_prim.IsA(UsdPhysics.PrismaticJoint) else "angular" + + # manually check joint type since it is a string type + if attr_name == "drive_type": + prim_attr_name = f"drive:{drive_model}:physics:type" + # check the value + assert attr_value == joint_prim.GetAttribute(prim_attr_name).Get() continue - # print(link_prim.GetProperties()) - prim_prop_name = f"physics:{to_camel_case(attr_name, to='cC')}" + + # non-string attributes + if attr_name == "max_velocity": + prim_attr_name = "physxJoint:maxJointVelocity" + elif attr_name == "max_effort": + prim_attr_name = f"drive:{drive_model}:physics:maxForce" + else: + prim_attr_name = f"drive:{drive_model}:physics:{to_camel_case(attr_name, to='cC')}" + + # obtain value from USD API (for angular, these follow degrees unit) + prim_attr_value = joint_prim.GetAttribute(prim_attr_name).Get() + + # for angular drives, we expect user to set in radians + # the values reported by USD are in degrees + if drive_model == "angular": + if attr_name == "max_velocity": + # deg / s --> rad / s + prim_attr_value = prim_attr_value * math.pi / 180.0 + elif attr_name in ["stiffness", "damping"]: + # N-m/deg or N-m-s/deg --> N-m/rad or N-m-s/rad + prim_attr_value = prim_attr_value * 180.0 / math.pi + # validate the values - self.assertAlmostEqual( - link_prim.GetAttribute(prim_prop_name).Get(), - attr_value, - places=5, - msg=f"Failed setting for {prim_prop_name}", + assert prim_attr_value == pytest.approx(attr_value, abs=1e-5), ( + f"Failed setting for {prim_attr_name}" ) elif verbose: - print(f"Skipping prim {link_prim.GetPrimPath()} as it is not a mass api.") - - def _validate_joint_drive_properties_on_prim(self, prim_path: str, verbose: bool = False): - """Validate the mass properties on the prim. - - Note: - Right now this function exploits the hierarchy in the asset to check the properties. This is not a - fool-proof way of checking the properties. - """ - # the root prim - root_prim = prim_utils.get_prim_at_path(prim_path) - # check joint drive properties are set correctly - for link_prim in root_prim.GetAllChildren(): - for joint_prim in link_prim.GetChildren(): - if joint_prim.IsA(UsdPhysics.PrismaticJoint) or joint_prim.IsA(UsdPhysics.RevoluteJoint): - # check it has drive API - self.assertTrue(joint_prim.HasAPI(UsdPhysics.DriveAPI)) - # iterate over the joint properties - for attr_name, attr_value in self.joint_cfg.__dict__.items(): - # skip names we know are not present - if attr_name == "func": - continue - # resolve the drive (linear or angular) - drive_model = "linear" if joint_prim.IsA(UsdPhysics.PrismaticJoint) else "angular" - - # manually check joint type since it is a string type - if attr_name == "drive_type": - prim_attr_name = f"drive:{drive_model}:physics:type" - # check the value - self.assertEqual(attr_value, joint_prim.GetAttribute(prim_attr_name).Get()) - continue - - # non-string attributes - if attr_name == "max_velocity": - prim_attr_name = "physxJoint:maxJointVelocity" - elif attr_name == "max_effort": - prim_attr_name = f"drive:{drive_model}:physics:maxForce" - else: - prim_attr_name = f"drive:{drive_model}:physics:{to_camel_case(attr_name, to='cC')}" - - # obtain value from USD API (for angular, these follow degrees unit) - prim_attr_value = joint_prim.GetAttribute(prim_attr_name).Get() - - # for angular drives, we expect user to set in radians - # the values reported by USD are in degrees - if drive_model == "angular": - if attr_name == "max_velocity": - # deg / s --> rad / s - prim_attr_value = prim_attr_value * math.pi / 180.0 - elif attr_name in ["stiffness", "damping"]: - # N-m/deg or N-m-s/deg --> N-m/rad or N-m-s/rad - prim_attr_value = prim_attr_value * 180.0 / math.pi - - # validate the values - self.assertAlmostEqual( - prim_attr_value, - attr_value, - places=5, - msg=f"Failed setting for {prim_attr_name}", - ) - elif verbose: - print(f"Skipping prim {joint_prim.GetPrimPath()} as it is not a joint drive api.") - - -if __name__ == "__main__": - run_tests() + print(f"Skipping prim {joint_prim.GetPrimPath()} as it is not a joint drive api.") diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 846171fe576e..4244b36ff8e1 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -1,136 +1,163 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" +from collections.abc import Generator + import numpy as np -import unittest +import pytest -import isaacsim.core.utils.prims as prim_utils -from isaacsim.core.api.simulation_context import SimulationContext as IsaacSimulationContext +import omni.physx +import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext -class TestSimulationContext(unittest.TestCase): - """Test fixture for wrapper around simulation context.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Load kit helper - SimulationContext.clear_instance() - - def test_singleton(self): - """Tests that the singleton is working.""" - sim1 = SimulationContext() - sim2 = SimulationContext() - sim3 = IsaacSimulationContext() - self.assertIs(sim1, sim2) - self.assertIs(sim1, sim3) - - # try to delete the singleton - sim2.clear_instance() - self.assertTrue(sim1.instance() is None) - # create new instance - sim4 = SimulationContext() - self.assertIsNot(sim1, sim4) - self.assertIsNot(sim3, sim4) - self.assertIs(sim1.instance(), sim4.instance()) - self.assertIs(sim3.instance(), sim4.instance()) - # clear instance - sim3.clear_instance() - - def test_initialization(self): - """Test the simulation config.""" - cfg = SimulationCfg(physics_prim_path="/Physics/PhysX", render_interval=5, gravity=(0.0, -0.5, -0.5)) - sim = SimulationContext(cfg) - # TODO: Figure out why keyword argument doesn't work. - # note: added a fix in Isaac Sim 2023.1 for this. - # sim = SimulationContext(cfg=cfg) - - # check valid settings - self.assertEqual(sim.get_physics_dt(), cfg.dt) - self.assertEqual(sim.get_rendering_dt(), cfg.dt * cfg.render_interval) - self.assertFalse(sim.has_rtx_sensors()) - # check valid paths - self.assertTrue(prim_utils.is_prim_path_valid("/Physics/PhysX")) - self.assertTrue(prim_utils.is_prim_path_valid("/Physics/PhysX/defaultMaterial")) - # check valid gravity - gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() - gravity = np.array(gravity_dir) * gravity_mag - np.testing.assert_almost_equal(gravity, cfg.gravity) - - def test_sim_version(self): - """Test obtaining the version.""" - sim = SimulationContext() - version = sim.get_version() - self.assertTrue(len(version) > 0) - self.assertTrue(version[0] >= 4) - - def test_carb_setting(self): - """Test setting carb settings.""" - sim = SimulationContext() - # known carb setting - sim.set_setting("/physics/physxDispatcher", False) - self.assertEqual(sim.get_setting("/physics/physxDispatcher"), False) - # unknown carb setting - sim.set_setting("/myExt/using_omniverse_version", sim.get_version()) - self.assertSequenceEqual(sim.get_setting("/myExt/using_omniverse_version"), sim.get_version()) - - def test_headless_mode(self): - """Test that render mode is headless since we are running in headless mode.""" - - sim = SimulationContext() - # check default render mode - self.assertEqual(sim.render_mode, sim.RenderMode.NO_GUI_OR_RENDERING) - - # def test_boundedness(self): - # """Test that the boundedness of the simulation context remains constant. - - # Note: This test fails right now because Isaac Sim does not handle boundedness correctly. On creation, - # it is registering itself to various callbacks and hence the boundedness is more than 1. This may not be - # critical for the simulation context since we usually call various clear functions before deleting the - # simulation context. - # """ - # sim = SimulationContext() - # # manually set the boundedness to 1? -- this is not possible because of Isaac Sim. - # sim.clear_all_callbacks() - # sim._stage_open_callback = None - # sim._physics_timer_callback = None - # sim._event_timer_callback = None - - # # check that boundedness of simulation context is correct - # sim_ref_count = ctypes.c_long.from_address(id(sim)).value - # # reset the simulation - # sim.reset() - # self.assertEqual(ctypes.c_long.from_address(id(sim)).value, sim_ref_count) - # # step the simulation - # for _ in range(10): - # sim.step() - # self.assertEqual(ctypes.c_long.from_address(id(sim)).value, sim_ref_count) - # # clear the simulation - # sim.clear_instance() - # self.assertEqual(ctypes.c_long.from_address(id(sim)).value, sim_ref_count - 1) - - def test_zero_gravity(self): - """Test that gravity can be properly disabled.""" - cfg = SimulationCfg(gravity=(0.0, 0.0, 0.0)) - - sim = SimulationContext(cfg) - - gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() - gravity = np.array(gravity_dir) * gravity_mag - np.testing.assert_almost_equal(gravity, cfg.gravity) - - -if __name__ == "__main__": - run_tests() +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Setup and teardown for each test.""" + # Setup: Clear any existing simulation context + SimulationContext.clear_instance() + + # Yield for the test + yield + + # Teardown: Clear the simulation context after each test + SimulationContext.clear_instance() + + +@pytest.fixture +def sim_with_stage_in_memory() -> Generator[SimulationContext, None, None]: + """Create a simulation context with stage in memory.""" + # create stage in memory + cfg = SimulationCfg(create_stage_in_memory=True) + sim = SimulationContext(cfg=cfg) + # update stage + sim_utils.update_stage() + # yield simulation context + yield sim + # stop simulation + omni.physx.get_physx_simulation_interface().detach_stage() + sim.stop() + # clear simulation context + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_singleton(): + """Tests that the singleton is working.""" + sim1 = SimulationContext() + sim2 = SimulationContext() + assert sim1 is sim2 + + # try to delete the singleton + sim2.clear_instance() + assert sim1.instance() is None + # create new instance + sim4 = SimulationContext() + assert sim1 is not sim4 + assert sim1.instance() is sim4.instance() + + +@pytest.mark.isaacsim_ci +def test_initialization(): + """Test the simulation config.""" + cfg = SimulationCfg(physics_prim_path="/Physics/PhysX", render_interval=5, gravity=(0.0, -0.5, -0.5)) + sim = SimulationContext(cfg) + # TODO: Figure out why keyword argument doesn't work. + # note: added a fix in Isaac Sim 2023.1 for this. + # sim = SimulationContext(cfg=cfg) + + # check valid settings + assert sim.get_physics_dt() == cfg.dt + assert sim.get_rendering_dt() == cfg.dt * cfg.render_interval + assert not sim.has_rtx_sensors() + # check valid paths + assert sim.stage.GetPrimAtPath("/Physics/PhysX").IsValid() + assert sim.stage.GetPrimAtPath("/Physics/PhysX/defaultMaterial").IsValid() + # check valid gravity + gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() + gravity = np.array(gravity_dir) * gravity_mag + np.testing.assert_almost_equal(gravity, cfg.gravity) + + +@pytest.mark.isaacsim_ci +def test_sim_version(): + """Test obtaining the version.""" + sim = SimulationContext() + version = sim.get_version() + assert len(version) > 0 + assert version[0] >= 4 + + +@pytest.mark.isaacsim_ci +def test_carb_setting(): + """Test setting carb settings.""" + sim = SimulationContext() + # known carb setting + sim.set_setting("/physics/physxDispatcher", False) + assert sim.get_setting("/physics/physxDispatcher") is False + # unknown carb setting + sim.set_setting("/myExt/using_omniverse_version", sim.get_version()) + assert tuple(sim.get_setting("/myExt/using_omniverse_version")) == tuple(sim.get_version()) + + +@pytest.mark.isaacsim_ci +def test_headless_mode(): + """Test that render mode is headless since we are running in headless mode.""" + sim = SimulationContext() + # check default render mode + assert sim.render_mode == sim.RenderMode.NO_GUI_OR_RENDERING + + +# def test_boundedness(): +# """Test that the boundedness of the simulation context remains constant. +# +# Note: This test fails right now because Isaac Sim does not handle boundedness correctly. On creation, +# it is registering itself to various callbacks and hence the boundedness is more than 1. This may not be +# critical for the simulation context since we usually call various clear functions before deleting the +# simulation context. +# """ +# sim = SimulationContext() +# # manually set the boundedness to 1? -- this is not possible because of Isaac Sim. +# sim.clear_all_callbacks() +# sim._stage_open_callback = None +# sim._physics_timer_callback = None +# sim._event_timer_callback = None +# +# # check that boundedness of simulation context is correct +# sim_ref_count = ctypes.c_long.from_address(id(sim)).value +# # reset the simulation +# sim.reset() +# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count +# # step the simulation +# for _ in range(10): +# sim.step() +# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count +# # clear the simulation +# sim.clear_instance() +# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count - 1 + + +@pytest.mark.isaacsim_ci +def test_zero_gravity(): + """Test that gravity can be properly disabled.""" + cfg = SimulationCfg(gravity=(0.0, 0.0, 0.0)) + + sim = SimulationContext(cfg) + + gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() + gravity = np.array(gravity_dir) * gravity_mag + np.testing.assert_almost_equal(gravity, cfg.gravity) diff --git a/source/isaaclab/test/sim/test_simulation_render_config.py b/source/isaaclab/test/sim/test_simulation_render_config.py index 17e4b5f145cc..1bab84d11d7e 100644 --- a/source/isaaclab/test/sim/test_simulation_render_config.py +++ b/source/isaaclab/test/sim/test_simulation_render_config.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,161 +6,204 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True, enable_cameras=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + """Rest everything follows.""" -import unittest +import os + +import flatdict +import pytest +import toml import carb from isaaclab.sim.simulation_cfg import RenderCfg, SimulationCfg from isaaclab.sim.simulation_context import SimulationContext - - -class TestSimulationRenderConfig(unittest.TestCase): - """Tests for simulation context render config.""" - - """ - Tests - """ - - def test_render_cfg(self): - """Test that the simulation context is created with the correct render cfg.""" - enable_translucency = True - enable_reflections = True - enable_global_illumination = True - antialiasing_mode = "DLAA" - enable_dlssg = True - enable_dl_denoiser = True - dlss_mode = 0 - enable_direct_lighting = True - samples_per_pixel = 4 - enable_shadows = True - enable_ambient_occlusion = True - - render_cfg = RenderCfg( - enable_translucency=enable_translucency, - enable_reflections=enable_reflections, - enable_global_illumination=enable_global_illumination, - antialiasing_mode=antialiasing_mode, - enable_dlssg=enable_dlssg, - dlss_mode=dlss_mode, - enable_dl_denoiser=enable_dl_denoiser, - enable_direct_lighting=enable_direct_lighting, - samples_per_pixel=samples_per_pixel, - enable_shadows=enable_shadows, - enable_ambient_occlusion=enable_ambient_occlusion, - ) - - cfg = SimulationCfg(render=render_cfg) - - sim = SimulationContext(cfg) - - self.assertEqual(sim.cfg.render.enable_translucency, enable_translucency) - self.assertEqual(sim.cfg.render.enable_reflections, enable_reflections) - self.assertEqual(sim.cfg.render.enable_global_illumination, enable_global_illumination) - self.assertEqual(sim.cfg.render.antialiasing_mode, antialiasing_mode) - self.assertEqual(sim.cfg.render.enable_dlssg, enable_dlssg) - self.assertEqual(sim.cfg.render.dlss_mode, dlss_mode) - self.assertEqual(sim.cfg.render.enable_dl_denoiser, enable_dl_denoiser) - self.assertEqual(sim.cfg.render.enable_direct_lighting, enable_direct_lighting) - self.assertEqual(sim.cfg.render.samples_per_pixel, samples_per_pixel) - self.assertEqual(sim.cfg.render.enable_shadows, enable_shadows) - self.assertEqual(sim.cfg.render.enable_ambient_occlusion, enable_ambient_occlusion) - - carb_settings_iface = carb.settings.get_settings() - self.assertEqual(carb_settings_iface.get("/rtx/translucency/enabled"), sim.cfg.render.enable_translucency) - self.assertEqual(carb_settings_iface.get("/rtx/reflections/enabled"), sim.cfg.render.enable_reflections) - self.assertEqual( - carb_settings_iface.get("/rtx/indirectDiffuse/enabled"), sim.cfg.render.enable_global_illumination - ) - self.assertEqual(carb_settings_iface.get("/rtx-transient/dlssg/enabled"), sim.cfg.render.enable_dlssg) - self.assertEqual( - carb_settings_iface.get("/rtx-transient/dldenoiser/enabled"), sim.cfg.render.enable_dl_denoiser - ) - self.assertEqual(carb_settings_iface.get("/rtx/post/dlss/execMode"), sim.cfg.render.dlss_mode) - self.assertEqual(carb_settings_iface.get("/rtx/directLighting/enabled"), sim.cfg.render.enable_direct_lighting) - self.assertEqual( - carb_settings_iface.get("/rtx/directLighting/sampledLighting/samplesPerPixel"), - sim.cfg.render.samples_per_pixel, - ) - self.assertEqual(carb_settings_iface.get("/rtx/shadows/enabled"), sim.cfg.render.enable_shadows) - self.assertEqual( - carb_settings_iface.get("/rtx/ambientOcclusion/enabled"), sim.cfg.render.enable_ambient_occlusion - ) - self.assertEqual(carb_settings_iface.get("/rtx/post/aa/op"), 4) # dlss = 3, dlaa=4 - - def test_render_cfg_defaults(self): - """Test that the simulation context is created with the correct render cfg.""" - enable_translucency = False - enable_reflections = False - enable_global_illumination = False - antialiasing_mode = "DLSS" - enable_dlssg = False - enable_dl_denoiser = False - dlss_mode = 2 - enable_direct_lighting = False - samples_per_pixel = 1 - enable_shadows = False - enable_ambient_occlusion = False +from isaaclab.utils.version import get_isaac_sim_version + + +@pytest.mark.skip(reason="Timeline not stopped") +@pytest.mark.isaacsim_ci +def test_render_cfg(): + """Test that the simulation context is created with the correct render cfg.""" + enable_translucency = True + enable_reflections = True + enable_global_illumination = True + antialiasing_mode = "DLAA" + enable_dlssg = True + enable_dl_denoiser = True + dlss_mode = 0 + enable_direct_lighting = True + samples_per_pixel = 4 + enable_shadows = True + enable_ambient_occlusion = True + + render_cfg = RenderCfg( + enable_translucency=enable_translucency, + enable_reflections=enable_reflections, + enable_global_illumination=enable_global_illumination, + antialiasing_mode=antialiasing_mode, + enable_dlssg=enable_dlssg, + dlss_mode=dlss_mode, + enable_dl_denoiser=enable_dl_denoiser, + enable_direct_lighting=enable_direct_lighting, + samples_per_pixel=samples_per_pixel, + enable_shadows=enable_shadows, + enable_ambient_occlusion=enable_ambient_occlusion, + ) + + cfg = SimulationCfg(render=render_cfg) + + # FIXME: when running all tests, the timeline is not stopped, force stop it here but also that does not the timeline + # omni.timeline.get_timeline_interface().stop() + + sim = SimulationContext(cfg) + + assert sim.cfg.render.enable_translucency == enable_translucency + assert sim.cfg.render.enable_reflections == enable_reflections + assert sim.cfg.render.enable_global_illumination == enable_global_illumination + assert sim.cfg.render.antialiasing_mode == antialiasing_mode + assert sim.cfg.render.enable_dlssg == enable_dlssg + assert sim.cfg.render.dlss_mode == dlss_mode + assert sim.cfg.render.enable_dl_denoiser == enable_dl_denoiser + assert sim.cfg.render.enable_direct_lighting == enable_direct_lighting + assert sim.cfg.render.samples_per_pixel == samples_per_pixel + assert sim.cfg.render.enable_shadows == enable_shadows + assert sim.cfg.render.enable_ambient_occlusion == enable_ambient_occlusion + + carb_settings_iface = carb.settings.get_settings() + assert carb_settings_iface.get("/rtx/translucency/enabled") == sim.cfg.render.enable_translucency + assert carb_settings_iface.get("/rtx/reflections/enabled") == sim.cfg.render.enable_reflections + assert carb_settings_iface.get("/rtx/indirectDiffuse/enabled") == sim.cfg.render.enable_global_illumination + assert carb_settings_iface.get("/rtx-transient/dlssg/enabled") == sim.cfg.render.enable_dlssg + assert carb_settings_iface.get("/rtx-transient/dldenoiser/enabled") == sim.cfg.render.enable_dl_denoiser + assert carb_settings_iface.get("/rtx/post/dlss/execMode") == sim.cfg.render.dlss_mode + assert carb_settings_iface.get("/rtx/directLighting/enabled") == sim.cfg.render.enable_direct_lighting + assert ( + carb_settings_iface.get("/rtx/directLighting/sampledLighting/samplesPerPixel") + == sim.cfg.render.samples_per_pixel + ) + assert carb_settings_iface.get("/rtx/shadows/enabled") == sim.cfg.render.enable_shadows + assert carb_settings_iface.get("/rtx/ambientOcclusion/enabled") == sim.cfg.render.enable_ambient_occlusion + assert carb_settings_iface.get("/rtx/post/aa/op") == 4 # dlss = 3, dlaa=4 + + +@pytest.mark.isaacsim_ci +def test_render_cfg_presets(): + """Test that the simulation context is created with the correct render cfg preset with overrides.""" + + # carb setting dictionary overrides + carb_settings = {"/rtx/raytracing/subpixel/mode": 3, "/rtx/pathtracing/maxSamplesPerLaunch": 999999} + # user-friendly setting overrides + dlss_mode = ("/rtx/post/dlss/execMode", 5) + + rendering_modes = ["performance", "balanced", "quality"] + + for rendering_mode in 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 get_isaac_sim_version().major < 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=".")) render_cfg = RenderCfg( - enable_translucency=enable_translucency, - enable_reflections=enable_reflections, - enable_global_illumination=enable_global_illumination, - antialiasing_mode=antialiasing_mode, - enable_dlssg=enable_dlssg, - enable_dl_denoiser=enable_dl_denoiser, - dlss_mode=dlss_mode, - enable_direct_lighting=enable_direct_lighting, - samples_per_pixel=samples_per_pixel, - enable_shadows=enable_shadows, - enable_ambient_occlusion=enable_ambient_occlusion, + rendering_mode=rendering_mode, + dlss_mode=dlss_mode[1], + carb_settings=carb_settings, ) cfg = SimulationCfg(render=render_cfg) - sim = SimulationContext(cfg) - - self.assertEqual(sim.cfg.render.enable_translucency, enable_translucency) - self.assertEqual(sim.cfg.render.enable_reflections, enable_reflections) - self.assertEqual(sim.cfg.render.enable_global_illumination, enable_global_illumination) - self.assertEqual(sim.cfg.render.antialiasing_mode, antialiasing_mode) - self.assertEqual(sim.cfg.render.enable_dlssg, enable_dlssg) - self.assertEqual(sim.cfg.render.enable_dl_denoiser, enable_dl_denoiser) - self.assertEqual(sim.cfg.render.dlss_mode, dlss_mode) - self.assertEqual(sim.cfg.render.enable_direct_lighting, enable_direct_lighting) - self.assertEqual(sim.cfg.render.samples_per_pixel, samples_per_pixel) - self.assertEqual(sim.cfg.render.enable_shadows, enable_shadows) - self.assertEqual(sim.cfg.render.enable_ambient_occlusion, enable_ambient_occlusion) + SimulationContext(cfg) carb_settings_iface = carb.settings.get_settings() - self.assertEqual(carb_settings_iface.get("/rtx/translucency/enabled"), sim.cfg.render.enable_translucency) - self.assertEqual(carb_settings_iface.get("/rtx/reflections/enabled"), sim.cfg.render.enable_reflections) - self.assertEqual( - carb_settings_iface.get("/rtx/indirectDiffuse/enabled"), sim.cfg.render.enable_global_illumination - ) - self.assertEqual(carb_settings_iface.get("/rtx-transient/dlssg/enabled"), sim.cfg.render.enable_dlssg) - self.assertEqual( - carb_settings_iface.get("/rtx-transient/dldenoiser/enabled"), sim.cfg.render.enable_dl_denoiser - ) - self.assertEqual(carb_settings_iface.get("/rtx/post/dlss/execMode"), sim.cfg.render.dlss_mode) - self.assertEqual(carb_settings_iface.get("/rtx/directLighting/enabled"), sim.cfg.render.enable_direct_lighting) - self.assertEqual( - carb_settings_iface.get("/rtx/directLighting/sampledLighting/samplesPerPixel"), - sim.cfg.render.samples_per_pixel, - ) - self.assertEqual(carb_settings_iface.get("/rtx/shadows/enabled"), sim.cfg.render.enable_shadows) - self.assertEqual( - carb_settings_iface.get("/rtx/ambientOcclusion/enabled"), sim.cfg.render.enable_ambient_occlusion - ) - self.assertEqual(carb_settings_iface.get("/rtx/post/aa/op"), 3) # dlss = 3, dlaa=4 - - -if __name__ == "__main__": - run_tests() + for key, val in preset_dict.items(): + setting_name = "/" + key.replace(".", "/") # convert to carb setting format + + if setting_name in carb_settings: + # grab groundtruth from carb setting dictionary overrides + setting_gt = carb_settings[setting_name] + elif setting_name == dlss_mode[0]: + # grab groundtruth from user-friendly setting overrides + setting_gt = dlss_mode[1] + else: + # grab groundtruth from preset + setting_gt = val + + setting_val = carb_settings_iface.get(setting_name) + + assert setting_gt == setting_val + + +@pytest.mark.skip(reason="Timeline not stopped") +@pytest.mark.isaacsim_ci +def test_render_cfg_defaults(): + """Test that the simulation context is created with the correct render cfg.""" + enable_translucency = False + enable_reflections = False + enable_global_illumination = False + antialiasing_mode = "DLSS" + enable_dlssg = False + enable_dl_denoiser = False + dlss_mode = 2 + enable_direct_lighting = False + samples_per_pixel = 1 + enable_shadows = False + enable_ambient_occlusion = False + + render_cfg = RenderCfg( + enable_translucency=enable_translucency, + enable_reflections=enable_reflections, + enable_global_illumination=enable_global_illumination, + antialiasing_mode=antialiasing_mode, + enable_dlssg=enable_dlssg, + enable_dl_denoiser=enable_dl_denoiser, + dlss_mode=dlss_mode, + enable_direct_lighting=enable_direct_lighting, + samples_per_pixel=samples_per_pixel, + enable_shadows=enable_shadows, + enable_ambient_occlusion=enable_ambient_occlusion, + ) + + cfg = SimulationCfg(render=render_cfg) + + sim = SimulationContext(cfg) + + assert sim.cfg.render.enable_translucency == enable_translucency + assert sim.cfg.render.enable_reflections == enable_reflections + assert sim.cfg.render.enable_global_illumination == enable_global_illumination + assert sim.cfg.render.antialiasing_mode == antialiasing_mode + assert sim.cfg.render.enable_dlssg == enable_dlssg + assert sim.cfg.render.enable_dl_denoiser == enable_dl_denoiser + assert sim.cfg.render.dlss_mode == dlss_mode + assert sim.cfg.render.enable_direct_lighting == enable_direct_lighting + assert sim.cfg.render.samples_per_pixel == samples_per_pixel + assert sim.cfg.render.enable_shadows == enable_shadows + assert sim.cfg.render.enable_ambient_occlusion == enable_ambient_occlusion + + carb_settings_iface = carb.settings.get_settings() + assert carb_settings_iface.get("/rtx/translucency/enabled") == sim.cfg.render.enable_translucency + assert carb_settings_iface.get("/rtx/reflections/enabled") == sim.cfg.render.enable_reflections + assert carb_settings_iface.get("/rtx/indirectDiffuse/enabled") == sim.cfg.render.enable_global_illumination + assert carb_settings_iface.get("/rtx-transient/dlssg/enabled") == sim.cfg.render.enable_dlssg + assert carb_settings_iface.get("/rtx-transient/dldenoiser/enabled") == sim.cfg.render.enable_dl_denoiser + assert carb_settings_iface.get("/rtx/post/dlss/execMode") == sim.cfg.render.dlss_mode + assert carb_settings_iface.get("/rtx/directLighting/enabled") == sim.cfg.render.enable_direct_lighting + assert ( + carb_settings_iface.get("/rtx/directLighting/sampledLighting/samplesPerPixel") + == sim.cfg.render.samples_per_pixel + ) + assert carb_settings_iface.get("/rtx/shadows/enabled") == sim.cfg.render.enable_shadows + assert carb_settings_iface.get("/rtx/ambientOcclusion/enabled") == sim.cfg.render.enable_ambient_occlusion + assert carb_settings_iface.get("/rtx/post/aa/op") == 3 # dlss = 3, dlaa=4 diff --git a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py new file mode 100644 index 000000000000..68d9d86c666e --- /dev/null +++ b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py @@ -0,0 +1,259 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Integration tests for simulation context with stage in memory.""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +# FIXME (mmittal): Stage in memory requires cameras to be enabled. +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +"""Rest everything follows.""" + + +import pytest + +import omni +import omni.physx +import omni.usd +import usdrt +from isaacsim.core.cloner import GridCloner + +import isaaclab.sim as sim_utils +from isaaclab.sim.simulation_context import SimulationCfg, SimulationContext +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.version import get_isaac_sim_version + + +@pytest.fixture +def sim(): + """Create a simulation context.""" + cfg = SimulationCfg(create_stage_in_memory=True) + sim = SimulationContext(cfg=cfg) + sim_utils.update_stage() + yield sim + omni.physx.get_physx_simulation_interface().detach_stage() + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +""" +Tests +""" + + +def test_stage_in_memory_with_shapes(sim): + """Test spawning of shapes with stage in memory.""" + + # skip test if stage in memory is not supported + if get_isaac_sim_version().major < 5: + pytest.skip("Stage in memory is not supported in this version of Isaac Sim") + + # define parameters + num_clones = 10 + + # grab stage in memory and set as current stage via the with statement + stage_in_memory = sim.get_initial_stage() + with sim_utils.use_stage(stage_in_memory): + # create cloned cone stage + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + + cfg = sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.ConeCfg( + radius=0.3, + height=0.6, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + ), + sim_utils.MeshCuboidCfg( + size=(0.3, 0.3, 0.3), + visual_material=sim_utils.MdlFileCfg( + mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl", + project_uvw=True, + texture_scale=(0.25, 0.25), + ), + ), + sim_utils.SphereCfg( + radius=0.3, + visual_material=sim_utils.MdlFileCfg( + mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl", + project_uvw=True, + texture_scale=(0.25, 0.25), + ), + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + ), + ], + random_choice=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ) + prim_path_regex = "/World/env_.*/Cone" + cfg.func(prim_path_regex, cfg) + + # verify stage is in memory + assert sim_utils.is_current_stage_in_memory() + + # verify prims exist in stage in memory + prims = sim_utils.find_matching_prim_paths(prim_path_regex) + assert len(prims) == num_clones + + # verify prims do not exist in context stage + context_stage = omni.usd.get_context().get_stage() + with sim_utils.use_stage(context_stage): + prims = sim_utils.find_matching_prim_paths(prim_path_regex) + assert len(prims) != num_clones + + # attach stage to context + sim_utils.attach_stage_to_usd_context() + + # verify stage is no longer in memory + assert not sim_utils.is_current_stage_in_memory() + + # verify prims now exist in context stage + prims = sim_utils.find_matching_prim_paths(prim_path_regex) + assert len(prims) == num_clones + + +def test_stage_in_memory_with_usds(sim): + """Test spawning of USDs with stage in memory.""" + + # skip test if stage in memory is not supported + if get_isaac_sim_version().major < 5: + pytest.skip("Stage in memory is not supported in this version of Isaac Sim") + + # define parameters + num_clones = 10 + usd_paths = [ + f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd", + f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd", + ] + + # grab stage in memory and set as current stage via the with statement + stage_in_memory = sim.get_initial_stage() + with sim_utils.use_stage(stage_in_memory): + # create cloned robot stage + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + + cfg = sim_utils.MultiUsdFileCfg( + usd_path=usd_paths, + random_choice=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 + ), + activate_contact_sensors=True, + ) + prim_path_regex = "/World/env_.*/Robot" + cfg.func(prim_path_regex, cfg) + + # verify stage is in memory + assert sim_utils.is_current_stage_in_memory() + + # verify prims exist in stage in memory + prims = sim_utils.find_matching_prim_paths(prim_path_regex) + assert len(prims) == num_clones + + # verify prims do not exist in context stage + context_stage = omni.usd.get_context().get_stage() + with sim_utils.use_stage(context_stage): + prims = sim_utils.find_matching_prim_paths(prim_path_regex) + assert len(prims) != num_clones + + # attach stage to context + sim_utils.attach_stage_to_usd_context() + + # verify stage is no longer in memory + assert not sim_utils.is_current_stage_in_memory() + + # verify prims now exist in context stage + prims = sim_utils.find_matching_prim_paths(prim_path_regex) + assert len(prims) == num_clones + + +def test_stage_in_memory_with_clone_in_fabric(sim): + """Test cloning in fabric with stage in memory.""" + + # skip test if stage in memory is not supported + if get_isaac_sim_version().major < 5: + pytest.skip("Stage in memory is not supported in this version of Isaac Sim") + + # define parameters + usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd" + num_clones = 100 + + # grab stage in memory and set as current stage via the with statement + stage_in_memory = sim.get_initial_stage() + with sim_utils.use_stage(stage_in_memory): + # set up paths + base_env_path = "/World/envs" + source_prim_path = f"{base_env_path}/env_0" + + # create cloner + cloner = GridCloner(spacing=3, stage=stage_in_memory) + cloner.define_base_env(base_env_path) + + # create source prim + sim_utils.create_prim(f"{source_prim_path}/Robot", "Xform", usd_path=usd_path) + + # generate target paths + target_paths = cloner.generate_paths("/World/envs/env", num_clones) + + # clone robots at target paths + cloner.clone( + source_prim_path=source_prim_path, + base_env_path=base_env_path, + prim_paths=target_paths, + replicate_physics=True, + clone_in_fabric=True, + ) + prim_path_regex = "/World/envs/env_.*" + + # verify prims do not exist in context stage + context_stage = omni.usd.get_context().get_stage() + with sim_utils.use_stage(context_stage): + prims = sim_utils.find_matching_prim_paths(prim_path_regex) + assert len(prims) != num_clones + + # attach stage to context + sim_utils.attach_stage_to_usd_context() + + # verify stage is no longer in memory + assert not sim_utils.is_current_stage_in_memory() + + # verify prims now exist in fabric stage using usdrt apis + stage_id = sim_utils.get_current_stage_id() + usdrt_stage = usdrt.Usd.Stage.Attach(stage_id) + for i in range(num_clones): + prim = usdrt_stage.GetPrimAtPath(f"/World/envs/env_{i}/Robot") + assert prim.IsValid() diff --git a/source/isaaclab/test/sim/test_spawn_from_files.py b/source/isaaclab/test/sim/test_spawn_from_files.py index b928701b34a4..9edf535ac91e 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -1,9 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher """Launch Isaac Sim Simulator first.""" @@ -12,90 +12,194 @@ """Rest everything follows.""" -import unittest +import pytest +from packaging.version import Version -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext -from isaacsim.core.utils.extensions import enable_extension, get_extension_path_from_name +import omni.kit.app import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR - - -class TestSpawningFromFiles(unittest.TestCase): - """Test fixture for checking spawning of USD references from files with different settings.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - # Wait for spawning - stage_utils.update_stage() - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Basic spawning. - """ - - def test_spawn_usd(self): - """Test loading prim from Usd file.""" - # Spawn cone - cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd") - prim = cfg.func("/World/Franka", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Franka")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - - def test_spawn_usd_fails(self): - """Test loading prim from Usd file fails when asset usd path is invalid.""" - # Spawn cone - cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda2_instanceable.usd") - - with self.assertRaises(FileNotFoundError): - cfg.func("/World/Franka", cfg) - - def test_spawn_urdf(self): - """Test loading prim from URDF file.""" - # retrieve path to urdf importer extension - enable_extension("isaacsim.asset.importer.urdf") - extension_path = get_extension_path_from_name("isaacsim.asset.importer.urdf") - # Spawn franka from URDF - cfg = sim_utils.UrdfFileCfg( - asset_path=f"{extension_path}/data/urdf/robots/franka_description/robots/panda_arm_hand.urdf", - fix_base=True, - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=None, damping=None) - ), - ) - prim = cfg.func("/World/Franka", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Franka")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - - def test_spawn_ground_plane(self): - """Test loading prim for the ground plane from grid world USD.""" - # Spawn ground plane - cfg = sim_utils.GroundPlaneCfg(color=(0.1, 0.1, 0.1), size=(10.0, 10.0)) - prim = cfg.func("/World/ground_plane", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/ground_plane")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - - -if __name__ == "__main__": - run_tests() +from isaaclab.utils.version import get_isaac_sim_version + + +@pytest.fixture +def sim(): + """Create a blank new stage for each test.""" + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.1 + # Load kit helper + sim = SimulationContext(SimulationCfg(dt=dt)) + # Wait for spawning + sim_utils.update_stage() + + yield sim + + # cleanup after test + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_spawn_usd(sim): + """Test loading prim from Usd file.""" + # Spawn cone + cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd") + prim = cfg.func("/World/Franka", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Franka").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + +@pytest.mark.isaacsim_ci +def test_spawn_usd_fails(sim): + """Test loading prim from Usd file fails when asset usd path is invalid.""" + # Spawn cone + cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda2_instanceable.usd") + + with pytest.raises(FileNotFoundError): + cfg.func("/World/Franka", cfg) + + +@pytest.mark.isaacsim_ci +def test_spawn_urdf(sim): + """Test loading prim from URDF file.""" + # pin the urdf importer extension to the older version + manager = omni.kit.app.get_app().get_extension_manager() + if get_isaac_sim_version() >= Version("5.1"): + pinned_urdf_extension_name = "isaacsim.asset.importer.urdf-2.4.31" + manager.set_extension_enabled_immediate(pinned_urdf_extension_name, True) + else: + pinned_urdf_extension_name = "isaacsim.asset.importer.urdf" + # retrieve path to urdf importer extension + extension_id = manager.get_enabled_extension_id(pinned_urdf_extension_name) + extension_path = manager.get_extension_path(extension_id) + # Spawn franka from URDF + cfg = sim_utils.UrdfFileCfg( + asset_path=f"{extension_path}/data/urdf/robots/franka_description/robots/panda_arm_hand.urdf", + fix_base=True, + joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( + gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=None, damping=None) + ), + ) + prim = cfg.func("/World/Franka", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Franka").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + +@pytest.mark.isaacsim_ci +def test_spawn_ground_plane(sim): + """Test loading prim for the ground plane from grid world USD.""" + # Spawn ground plane + cfg = sim_utils.GroundPlaneCfg(color=(0.1, 0.1, 0.1), size=(10.0, 10.0)) + prim = cfg.func("/World/ground_plane", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/ground_plane").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + +@pytest.mark.isaacsim_ci +def test_spawn_usd_with_compliant_contact_material(sim): + """Test loading prim from USD file with physics material applied to specific prim.""" + # Spawn gelsight finger with physics material on specific prim + usd_file_path = f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd" + + # Create spawn configuration + spawn_cfg = sim_utils.UsdFileWithCompliantContactCfg( + usd_path=usd_file_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + compliant_contact_stiffness=1000.0, + compliant_contact_damping=100.0, + physics_material_prim_path="elastomer", + ) + + # Spawn the prim + prim = spawn_cfg.func("/World/Robot", spawn_cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Robot").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + material_prim_path = "/World/Robot/elastomer/compliant_material" + # Check that the physics material was applied to the specified prim + assert sim.stage.GetPrimAtPath(material_prim_path).IsValid() + + # Check properties + material_prim = sim.stage.GetPrimAtPath(material_prim_path) + assert material_prim.IsValid() + assert material_prim.GetAttribute("physxMaterial:compliantContactStiffness").Get() == 1000.0 + assert material_prim.GetAttribute("physxMaterial:compliantContactDamping").Get() == 100.0 + + +@pytest.mark.isaacsim_ci +def test_spawn_usd_with_compliant_contact_material_on_multiple_prims(sim): + """Test loading prim from USD file with physics material applied to multiple prims.""" + # Spawn Panda robot with physics material on specific prims + usd_file_path = f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd" + + # Create spawn configuration + spawn_cfg = sim_utils.UsdFileWithCompliantContactCfg( + usd_path=usd_file_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + compliant_contact_stiffness=1000.0, + compliant_contact_damping=100.0, + physics_material_prim_path=["elastomer", "gelsight_finger"], + ) + + # Spawn the prim + prim = spawn_cfg.func("/World/Robot", spawn_cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Robot").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + # Check that the physics material was applied to the specified prims + for link_name in ["elastomer", "gelsight_finger"]: + material_prim_path = f"/World/Robot/{link_name}/compliant_material" + print("checking", material_prim_path) + assert sim.stage.GetPrimAtPath(material_prim_path).IsValid() + + # Check properties + material_prim = sim.stage.GetPrimAtPath(material_prim_path) + assert material_prim.IsValid() + assert material_prim.GetAttribute("physxMaterial:compliantContactStiffness").Get() == 1000.0 + assert material_prim.GetAttribute("physxMaterial:compliantContactDamping").Get() == 100.0 + + +@pytest.mark.isaacsim_ci +def test_spawn_usd_with_compliant_contact_material_no_prim_path(sim): + """Test loading prim from USD file with physics material but no prim path specified.""" + # Spawn gelsight finger without specifying prim path for physics material + usd_file_path = f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd" + + # Create spawn configuration without physics material prim path + spawn_cfg = sim_utils.UsdFileWithCompliantContactCfg( + usd_path=usd_file_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + compliant_contact_stiffness=1000.0, + compliant_contact_damping=100.0, + physics_material_prim_path=None, + ) + + # Spawn the prim + prim = spawn_cfg.func("/World/Robot", spawn_cfg) + + # Check validity - should still spawn successfully but without physics material + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Robot").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + material_prim_path = "/World/Robot/elastomer/compliant_material" + material_prim = sim.stage.GetPrimAtPath(material_prim_path) + assert material_prim is not None + assert not material_prim.IsValid() diff --git a/source/isaaclab/test/sim/test_spawn_lights.py b/source/isaaclab/test/sim/test_spawn_lights.py index 440e04462dd5..9dbbd98cf7a2 100644 --- a/source/isaaclab/test/sim/test_spawn_lights.py +++ b/source/isaaclab/test/sim/test_spawn_lights.py @@ -1,163 +1,159 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext -from pxr import UsdLux +import pytest + +from pxr import Usd, UsdLux import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils.string import to_camel_case -class TestSpawningLights(unittest.TestCase): - """Test fixture for checking spawning of USD lights with different settings.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - # Wait for spawning - stage_utils.update_stage() - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Basic spawning. +@pytest.fixture(autouse=True) +def sim(): + """Setup and teardown for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.1 + # Load kit helper + sim = SimulationContext(SimulationCfg(dt=dt)) + # Wait for spawning + sim_utils.update_stage() + + # Yield the simulation context for the test + yield sim + + # Teardown: Stop simulation + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +def test_spawn_disk_light(sim): + """Test spawning a disk light source.""" + cfg = sim_utils.DiskLightCfg( + color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, radius=20.0 + ) + prim = cfg.func("/World/disk_light", cfg) + + # check if the light is spawned + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/disk_light").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "DiskLight" + # validate properties on the prim + _validate_properties_on_prim(prim, cfg) + + +def test_spawn_distant_light(sim): + """Test spawning a distant light.""" + cfg = sim_utils.DistantLightCfg( + color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, angle=20 + ) + prim = cfg.func("/World/distant_light", cfg) + + # check if the light is spawned + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/distant_light").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "DistantLight" + # validate properties on the prim + _validate_properties_on_prim(prim, cfg) + + +def test_spawn_dome_light(sim): + """Test spawning a dome light source.""" + cfg = sim_utils.DomeLightCfg( + color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100 + ) + prim = cfg.func("/World/dome_light", cfg) + + # check if the light is spawned + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/dome_light").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "DomeLight" + # validate properties on the prim + _validate_properties_on_prim(prim, cfg) + + +def test_spawn_cylinder_light(sim): + """Test spawning a cylinder light source.""" + cfg = sim_utils.CylinderLightCfg( + color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, radius=20.0 + ) + prim = cfg.func("/World/cylinder_light", cfg) + + # check if the light is spawned + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/cylinder_light").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "CylinderLight" + # validate properties on the prim + _validate_properties_on_prim(prim, cfg) + + +def test_spawn_sphere_light(sim): + """Test spawning a sphere light source.""" + cfg = sim_utils.SphereLightCfg( + color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, radius=20.0 + ) + prim = cfg.func("/World/sphere_light", cfg) + + # check if the light is spawned + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/sphere_light").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "SphereLight" + # validate properties on the prim + _validate_properties_on_prim(prim, cfg) + + +""" +Helper functions. +""" + + +def _validate_properties_on_prim(prim: Usd.Prim, cfg: sim_utils.LightCfg): + """Validate the properties on the prim. + + Args: + prim: The prim. + cfg: The configuration for the light source. """ - - def test_spawn_disk_light(self): - """Test spawning a disk light source.""" - cfg = sim_utils.DiskLightCfg( - color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, radius=20.0 - ) - prim = cfg.func("/World/disk_light", cfg) - - # check if the light is spawned - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/disk_light")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "DiskLight") - # validate properties on the prim - self._validate_properties_on_prim("/World/disk_light", cfg) - - def test_spawn_distant_light(self): - """Test spawning a distant light.""" - cfg = sim_utils.DistantLightCfg( - color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, angle=20 - ) - prim = cfg.func("/World/distant_light", cfg) - - # check if the light is spawned - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/distant_light")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "DistantLight") - # validate properties on the prim - self._validate_properties_on_prim("/World/distant_light", cfg) - - def test_spawn_dome_light(self): - """Test spawning a dome light source.""" - cfg = sim_utils.DomeLightCfg( - color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100 - ) - prim = cfg.func("/World/dome_light", cfg) - - # check if the light is spawned - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/dome_light")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "DomeLight") - # validate properties on the prim - self._validate_properties_on_prim("/World/dome_light", cfg) - - def test_spawn_cylinder_light(self): - """Test spawning a cylinder light source.""" - cfg = sim_utils.CylinderLightCfg( - color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, radius=20.0 - ) - prim = cfg.func("/World/cylinder_light", cfg) - - # check if the light is spawned - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/cylinder_light")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "CylinderLight") - # validate properties on the prim - self._validate_properties_on_prim("/World/cylinder_light", cfg) - - def test_spawn_sphere_light(self): - """Test spawning a sphere light source.""" - cfg = sim_utils.SphereLightCfg( - color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=100, radius=20.0 - ) - prim = cfg.func("/World/sphere_light", cfg) - - # check if the light is spawned - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/sphere_light")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "SphereLight") - # validate properties on the prim - self._validate_properties_on_prim("/World/sphere_light", cfg) - - """ - Helper functions. - """ - - def _validate_properties_on_prim(self, prim_path: str, cfg: sim_utils.LightCfg): - """Validate the properties on the prim. - - Args: - prim_path: The prim name. - cfg: The configuration for the light source. - """ - # default list of params to skip - non_usd_params = ["func", "prim_type", "visible", "semantic_tags", "copy_from_source"] - # obtain prim - prim = prim_utils.get_prim_at_path(prim_path) - for attr_name, attr_value in cfg.__dict__.items(): - # skip names we know are not present - if attr_name in non_usd_params or attr_value is None: - continue - # deal with texture input names - if "texture" in attr_name: - light_prim = UsdLux.DomeLight(prim) - if attr_name == "texture_file": - configured_value = light_prim.GetTextureFileAttr().Get() - elif attr_name == "texture_format": - configured_value = light_prim.GetTextureFormatAttr().Get() - else: - raise ValueError(f"Unknown texture attribute: '{attr_name}'") + # default list of params to skip + non_usd_params = ["func", "prim_type", "visible", "semantic_tags", "copy_from_source"] + # validate the properties + for attr_name, attr_value in cfg.__dict__.items(): + # skip names we know are not present + if attr_name in non_usd_params or attr_value is None: + continue + # deal with texture input names + if "texture" in attr_name: + light_prim = UsdLux.DomeLight(prim) + if attr_name == "texture_file": + configured_value = light_prim.GetTextureFileAttr().Get() + elif attr_name == "texture_format": + configured_value = light_prim.GetTextureFormatAttr().Get() + else: + raise ValueError(f"Unknown texture attribute: '{attr_name}'") + else: + # convert attribute name in prim to cfg name + if attr_name == "visible_in_primary_ray": + prim_prop_name = f"{to_camel_case(attr_name, to='cC')}" else: - # convert attribute name in prim to cfg name - if attr_name == "visible_in_primary_ray": - prim_prop_name = f"{to_camel_case(attr_name, to='cC')}" - else: - prim_prop_name = f"inputs:{to_camel_case(attr_name, to='cC')}" - # configured value - configured_value = prim.GetAttribute(prim_prop_name).Get() - # validate the values - self.assertEqual(configured_value, attr_value, msg=f"Failed for attribute: '{attr_name}'") - - -if __name__ == "__main__": - run_tests() + prim_prop_name = f"inputs:{to_camel_case(attr_name, to='cC')}" + # configured value + configured_value = prim.GetAttribute(prim_prop_name).Get() + # validate the values + assert configured_value == attr_value, f"Failed for attribute: '{attr_name}'" diff --git a/source/isaaclab/test/sim/test_spawn_materials.py b/source/isaaclab/test/sim/test_spawn_materials.py index af62ebfbc7fc..e5c3b14f50d7 100644 --- a/source/isaaclab/test/sim/test_spawn_materials.py +++ b/source/isaaclab/test/sim/test_spawn_materials.py @@ -1,197 +1,176 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import pytest + from pxr import UsdPhysics, UsdShade import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR -class TestSpawningMaterials(unittest.TestCase): - """Test fixture for checking spawning of materials.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - # Wait for spawning - stage_utils.update_stage() - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - def test_spawn_preview_surface(self): - """Test spawning preview surface.""" - # Spawn preview surface - cfg = sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)) - prim = cfg.func("/Looks/PreviewSurface", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/Looks/PreviewSurface")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Shader") - # Check properties - self.assertEqual(prim.GetAttribute("inputs:diffuseColor").Get(), cfg.diffuse_color) - - def test_spawn_mdl_material(self): - """Test spawning mdl material.""" - # Spawn mdl material - cfg = sim_utils.materials.MdlFileCfg( - mdl_path=f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Aluminum_Anodized.mdl", - project_uvw=True, - albedo_brightness=0.5, - ) - prim = cfg.func("/Looks/MdlMaterial", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/Looks/MdlMaterial")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Shader") - # Check properties - self.assertEqual(prim.GetAttribute("inputs:project_uvw").Get(), cfg.project_uvw) - self.assertEqual(prim.GetAttribute("inputs:albedo_brightness").Get(), cfg.albedo_brightness) - - def test_spawn_glass_mdl_material(self): - """Test spawning a glass mdl material.""" - # Spawn mdl material - cfg = sim_utils.materials.GlassMdlCfg(thin_walled=False, glass_ior=1.0, glass_color=(0.0, 1.0, 0.0)) - prim = cfg.func("/Looks/GlassMaterial", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/Looks/GlassMaterial")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Shader") - # Check properties - self.assertEqual(prim.GetAttribute("inputs:thin_walled").Get(), cfg.thin_walled) - self.assertEqual(prim.GetAttribute("inputs:glass_ior").Get(), cfg.glass_ior) - self.assertEqual(prim.GetAttribute("inputs:glass_color").Get(), cfg.glass_color) - - def test_spawn_rigid_body_material(self): - """Test spawning a rigid body material.""" - # spawn physics material - cfg = sim_utils.materials.RigidBodyMaterialCfg( - dynamic_friction=1.5, - restitution=1.5, - static_friction=0.5, - restitution_combine_mode="max", - friction_combine_mode="max", - improve_patch_friction=True, - ) - prim = cfg.func("/Looks/RigidBodyMaterial", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/Looks/RigidBodyMaterial")) - # Check properties - self.assertEqual(prim.GetAttribute("physics:staticFriction").Get(), cfg.static_friction) - self.assertEqual(prim.GetAttribute("physics:dynamicFriction").Get(), cfg.dynamic_friction) - self.assertEqual(prim.GetAttribute("physics:restitution").Get(), cfg.restitution) - self.assertEqual(prim.GetAttribute("physxMaterial:improvePatchFriction").Get(), cfg.improve_patch_friction) - self.assertEqual(prim.GetAttribute("physxMaterial:restitutionCombineMode").Get(), cfg.restitution_combine_mode) - self.assertEqual(prim.GetAttribute("physxMaterial:frictionCombineMode").Get(), cfg.friction_combine_mode) - - def test_spawn_deformable_body_material(self): - """Test spawning a deformable body material.""" - # spawn deformable body material - cfg = sim_utils.materials.DeformableBodyMaterialCfg( - density=1.0, - dynamic_friction=0.25, - youngs_modulus=50000000.0, - poissons_ratio=0.5, - elasticity_damping=0.005, - damping_scale=1.0, - ) - prim = cfg.func("/Looks/DeformableBodyMaterial", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/Looks/DeformableBodyMaterial")) - # Check properties - self.assertEqual(prim.GetAttribute("physxDeformableBodyMaterial:density").Get(), cfg.density) - self.assertEqual(prim.GetAttribute("physxDeformableBodyMaterial:dynamicFriction").Get(), cfg.dynamic_friction) - self.assertEqual(prim.GetAttribute("physxDeformableBodyMaterial:youngsModulus").Get(), cfg.youngs_modulus) - self.assertEqual(prim.GetAttribute("physxDeformableBodyMaterial:poissonsRatio").Get(), cfg.poissons_ratio) - self.assertAlmostEqual( - prim.GetAttribute("physxDeformableBodyMaterial:elasticityDamping").Get(), cfg.elasticity_damping - ) - self.assertEqual(prim.GetAttribute("physxDeformableBodyMaterial:dampingScale").Get(), cfg.damping_scale) - - def test_apply_rigid_body_material_on_visual_material(self): - """Test applying a rigid body material on a visual material.""" - # Spawn mdl material - cfg = sim_utils.materials.GlassMdlCfg(thin_walled=False, glass_ior=1.0, glass_color=(0.0, 1.0, 0.0)) - prim = cfg.func("/Looks/Material", cfg) - # spawn physics material - cfg = sim_utils.materials.RigidBodyMaterialCfg( - dynamic_friction=1.5, - restitution=1.5, - static_friction=0.5, - restitution_combine_mode="max", - friction_combine_mode="max", - improve_patch_friction=True, - ) - prim = cfg.func("/Looks/Material", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/Looks/Material")) - # Check properties - self.assertEqual(prim.GetAttribute("physics:staticFriction").Get(), cfg.static_friction) - self.assertEqual(prim.GetAttribute("physics:dynamicFriction").Get(), cfg.dynamic_friction) - self.assertEqual(prim.GetAttribute("physics:restitution").Get(), cfg.restitution) - self.assertEqual(prim.GetAttribute("physxMaterial:improvePatchFriction").Get(), cfg.improve_patch_friction) - self.assertEqual(prim.GetAttribute("physxMaterial:restitutionCombineMode").Get(), cfg.restitution_combine_mode) - self.assertEqual(prim.GetAttribute("physxMaterial:frictionCombineMode").Get(), cfg.friction_combine_mode) - - def test_bind_prim_to_material(self): - """Test binding a rigid body material on a mesh prim.""" - - # create a mesh prim - object_prim = prim_utils.create_prim("/World/Geometry/box", "Cube") - UsdPhysics.CollisionAPI.Apply(object_prim) - - # 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 physics material - physics_material_cfg = sim_utils.RigidBodyMaterialCfg( - static_friction=0.5, dynamic_friction=1.5, restitution=1.5 - ) - physics_material_cfg.func("/World/Physics/rubberMaterial", physics_material_cfg) - # bind the visual material to the mesh prim - sim_utils.bind_visual_material("/World/Geometry/box", "/World/Looks/glassMaterial") - sim_utils.bind_physics_material("/World/Geometry/box", "/World/Physics/rubberMaterial") - - # check the main material binding - material_binding_api = UsdShade.MaterialBindingAPI(object_prim) - # -- visual - material_direct_binding = material_binding_api.GetDirectBinding() - self.assertEqual(material_direct_binding.GetMaterialPath(), "/World/Looks/glassMaterial") - self.assertEqual(material_direct_binding.GetMaterialPurpose(), "") - # -- physics - material_direct_binding = material_binding_api.GetDirectBinding("physics") - self.assertEqual(material_direct_binding.GetMaterialPath(), "/World/Physics/rubberMaterial") - self.assertEqual(material_direct_binding.GetMaterialPurpose(), "physics") - - -if __name__ == "__main__": - run_tests() +@pytest.fixture +def sim(): + """Create a simulation context.""" + sim_utils.create_new_stage() + dt = 0.1 + sim = SimulationContext(SimulationCfg(dt=dt)) + sim_utils.update_stage() + yield sim + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +def test_spawn_preview_surface(sim): + """Test spawning preview surface.""" + cfg = sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)) + prim = cfg.func("/Looks/PreviewSurface", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/Looks/PreviewSurface").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Shader" + # Check properties + assert prim.GetAttribute("inputs:diffuseColor").Get() == cfg.diffuse_color + + +def test_spawn_mdl_material(sim): + """Test spawning mdl material.""" + cfg = sim_utils.materials.MdlFileCfg( + mdl_path=f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Aluminum_Anodized.mdl", + project_uvw=True, + albedo_brightness=0.5, + ) + prim = cfg.func("/Looks/MdlMaterial", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/Looks/MdlMaterial").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Shader" + # Check properties + assert prim.GetAttribute("inputs:project_uvw").Get() == cfg.project_uvw + assert prim.GetAttribute("inputs:albedo_brightness").Get() == cfg.albedo_brightness + + +def test_spawn_glass_mdl_material(sim): + """Test spawning a glass mdl material.""" + cfg = sim_utils.materials.GlassMdlCfg(thin_walled=False, glass_ior=1.0, glass_color=(0.0, 1.0, 0.0)) + prim = cfg.func("/Looks/GlassMaterial", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/Looks/GlassMaterial").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Shader" + # Check properties + assert prim.GetAttribute("inputs:thin_walled").Get() == cfg.thin_walled + assert prim.GetAttribute("inputs:glass_ior").Get() == cfg.glass_ior + assert prim.GetAttribute("inputs:glass_color").Get() == cfg.glass_color + + +def test_spawn_rigid_body_material(sim): + """Test spawning a rigid body material.""" + cfg = sim_utils.materials.RigidBodyMaterialCfg( + dynamic_friction=1.5, + restitution=1.5, + static_friction=0.5, + restitution_combine_mode="max", + friction_combine_mode="max", + ) + prim = cfg.func("/Looks/RigidBodyMaterial", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/Looks/RigidBodyMaterial").IsValid() + # Check properties + assert prim.GetAttribute("physics:staticFriction").Get() == cfg.static_friction + assert prim.GetAttribute("physics:dynamicFriction").Get() == cfg.dynamic_friction + assert prim.GetAttribute("physics:restitution").Get() == cfg.restitution + assert prim.GetAttribute("physxMaterial:restitutionCombineMode").Get() == cfg.restitution_combine_mode + assert prim.GetAttribute("physxMaterial:frictionCombineMode").Get() == cfg.friction_combine_mode + + +def test_spawn_deformable_body_material(sim): + """Test spawning a deformable body material.""" + cfg = sim_utils.materials.DeformableBodyMaterialCfg( + density=1.0, + dynamic_friction=0.25, + youngs_modulus=50000000.0, + poissons_ratio=0.5, + elasticity_damping=0.005, + damping_scale=1.0, + ) + prim = cfg.func("/Looks/DeformableBodyMaterial", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/Looks/DeformableBodyMaterial").IsValid() + # Check properties + assert prim.GetAttribute("physxDeformableBodyMaterial:density").Get() == cfg.density + assert prim.GetAttribute("physxDeformableBodyMaterial:dynamicFriction").Get() == cfg.dynamic_friction + assert prim.GetAttribute("physxDeformableBodyMaterial:youngsModulus").Get() == cfg.youngs_modulus + assert prim.GetAttribute("physxDeformableBodyMaterial:poissonsRatio").Get() == cfg.poissons_ratio + assert prim.GetAttribute("physxDeformableBodyMaterial:elasticityDamping").Get() == pytest.approx( + cfg.elasticity_damping + ) + assert prim.GetAttribute("physxDeformableBodyMaterial:dampingScale").Get() == cfg.damping_scale + + +def test_apply_rigid_body_material_on_visual_material(sim): + """Test applying a rigid body material on a visual material.""" + cfg = sim_utils.materials.GlassMdlCfg(thin_walled=False, glass_ior=1.0, glass_color=(0.0, 1.0, 0.0)) + prim = cfg.func("/Looks/Material", cfg) + cfg = sim_utils.materials.RigidBodyMaterialCfg( + dynamic_friction=1.5, + restitution=1.5, + static_friction=0.5, + restitution_combine_mode="max", + friction_combine_mode="max", + ) + prim = cfg.func("/Looks/Material", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/Looks/Material").IsValid() + # Check properties + assert prim.GetAttribute("physics:staticFriction").Get() == cfg.static_friction + assert prim.GetAttribute("physics:dynamicFriction").Get() == cfg.dynamic_friction + assert prim.GetAttribute("physics:restitution").Get() == cfg.restitution + assert prim.GetAttribute("physxMaterial:restitutionCombineMode").Get() == cfg.restitution_combine_mode + assert prim.GetAttribute("physxMaterial:frictionCombineMode").Get() == cfg.friction_combine_mode + + +def test_bind_prim_to_material(sim): + """Test binding a rigid body material on a mesh prim.""" + + # create a mesh prim + object_prim = sim_utils.create_prim("/World/Geometry/box", "Cube") + UsdPhysics.CollisionAPI.Apply(object_prim) + + # 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 physics material + physics_material_cfg = sim_utils.RigidBodyMaterialCfg(static_friction=0.5, dynamic_friction=1.5, restitution=1.5) + physics_material_cfg.func("/World/Physics/rubberMaterial", physics_material_cfg) + sim_utils.bind_visual_material("/World/Geometry/box", "/World/Looks/glassMaterial") + sim_utils.bind_physics_material("/World/Geometry/box", "/World/Physics/rubberMaterial") + + # check the material binding + material_binding_api = UsdShade.MaterialBindingAPI(object_prim) + # -- visual material + material_direct_binding = material_binding_api.GetDirectBinding() + assert material_direct_binding.GetMaterialPath() == "/World/Looks/glassMaterial" + assert material_direct_binding.GetMaterialPurpose() == "" + # -- physics material + material_direct_binding = material_binding_api.GetDirectBinding("physics") + assert material_direct_binding.GetMaterialPath() == "/World/Physics/rubberMaterial" + assert material_direct_binding.GetMaterialPurpose() == "physics" diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index 69dbb61c7486..43fbc7852c2a 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -1,366 +1,266 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import pytest import isaaclab.sim as sim_utils - - -class TestSpawningMeshGeometries(unittest.TestCase): - """Test fixture for checking spawning of USD-Mesh prim with different settings.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, device="cuda:0") - # Wait for spawning - stage_utils.update_stage() - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Basic spawning. - """ - - def test_spawn_cone(self): - """Test spawning of UsdGeomMesh as a cone prim.""" - # Spawn cone - cfg = sim_utils.MeshConeCfg(radius=1.0, height=2.0, axis="Y") - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Mesh") - - def test_spawn_capsule(self): - """Test spawning of UsdGeomMesh as a capsule prim.""" - # Spawn capsule - cfg = sim_utils.MeshCapsuleCfg(radius=1.0, height=2.0, axis="Y") - prim = cfg.func("/World/Capsule", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Capsule")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Capsule/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Mesh") - - def test_spawn_cylinder(self): - """Test spawning of UsdGeomMesh as a cylinder prim.""" - # Spawn cylinder - cfg = sim_utils.MeshCylinderCfg(radius=1.0, height=2.0, axis="Y") - prim = cfg.func("/World/Cylinder", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cylinder")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cylinder/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Mesh") - - def test_spawn_cuboid(self): - """Test spawning of UsdGeomMesh as a cuboid prim.""" - # Spawn cuboid - cfg = sim_utils.MeshCuboidCfg(size=(1.0, 2.0, 3.0)) - prim = cfg.func("/World/Cube", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cube")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cube/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Mesh") - - def test_spawn_sphere(self): - """Test spawning of UsdGeomMesh as a sphere prim.""" - # Spawn sphere - cfg = sim_utils.MeshSphereCfg(radius=1.0) - prim = cfg.func("/World/Sphere", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Sphere")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Sphere/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Mesh") - +from isaaclab.sim import SimulationCfg, SimulationContext + + +@pytest.fixture +def sim(): + """Create a simulation context for testing.""" + # Create a new stage + sim_utils.create_new_stage() + # Simulation time-step + dt = 0.1 + # Load kit helper + sim = SimulationContext(SimulationCfg(dt=dt)) + # Wait for spawning + sim_utils.update_stage() + yield sim + # Cleanup + sim._disable_app_control_on_stop_handle = True # prevent timeout + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +""" +Basic spawning. +""" + + +def test_spawn_cone(sim): + """Test spawning of UsdGeomMesh as a cone prim.""" + # Spawn cone + cfg = sim_utils.MeshConeCfg(radius=1.0, height=2.0, axis="Y") + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Mesh" + + +def test_spawn_capsule(sim): + """Test spawning of UsdGeomMesh as a capsule prim.""" + # Spawn capsule + cfg = sim_utils.MeshCapsuleCfg(radius=1.0, height=2.0, axis="Y") + prim = cfg.func("/World/Capsule", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Capsule").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Capsule/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Mesh" + + +def test_spawn_cylinder(sim): + """Test spawning of UsdGeomMesh as a cylinder prim.""" + # Spawn cylinder + cfg = sim_utils.MeshCylinderCfg(radius=1.0, height=2.0, axis="Y") + prim = cfg.func("/World/Cylinder", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cylinder").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cylinder/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Mesh" + + +def test_spawn_cuboid(sim): + """Test spawning of UsdGeomMesh as a cuboid prim.""" + # Spawn cuboid + cfg = sim_utils.MeshCuboidCfg(size=(1.0, 2.0, 3.0)) + prim = cfg.func("/World/Cube", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cube").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cube/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Mesh" + + +def test_spawn_sphere(sim): + """Test spawning of UsdGeomMesh as a sphere prim.""" + # Spawn sphere + cfg = sim_utils.MeshSphereCfg(radius=1.0) + prim = cfg.func("/World/Sphere", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Sphere").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Sphere/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Mesh" + + +""" +Physics properties. +""" + + +def test_spawn_cone_with_deformable_props(sim): + """Test spawning of UsdGeomMesh prim for a cone with deformable body API.""" + # Spawn cone + cfg = sim_utils.MeshConeCfg( + radius=1.0, + height=2.0, + deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + + # Check properties + # Unlike rigid body, deformable body properties are on the mesh prim + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetAttribute("physxDeformable:deformableEnabled").Get() == cfg.deformable_props.deformable_enabled + + +def test_spawn_cone_with_deformable_and_mass_props(sim): + """Test spawning of UsdGeomMesh prim for a cone with deformable body and mass API.""" + # Spawn cone + cfg = sim_utils.MeshConeCfg( + radius=1.0, + height=2.0, + deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetAttribute("physics:mass").Get() == cfg.mass_props.mass + + # check sim playing + sim.play() + for _ in range(10): + sim.step() + + +def test_spawn_cone_with_deformable_and_density_props(sim): + """Test spawning of UsdGeomMesh prim for a cone with deformable body and mass API. + + Note: + In this case, we specify the density instead of the mass. In that case, physics need to know + the collision shape to compute the mass. Thus, we have to set the collider properties. In + order to not have a collision shape, we disable the collision. """ - Physics properties. - """ - - def test_spawn_cone_with_deformable_props(self): - """Test spawning of UsdGeomMesh prim for a cone with deformable body API.""" - # Spawn cone - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - - # Check properties - # Unlike rigid body, deformable body properties are on the mesh prim - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual( - prim.GetAttribute("physxDeformable:deformableEnabled").Get(), cfg.deformable_props.deformable_enabled - ) - - def test_spawn_cone_with_deformable_and_mass_props(self): - """Test spawning of UsdGeomMesh prim for a cone with deformable body and mass API.""" - # Spawn cone - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual(prim.GetAttribute("physics:mass").Get(), cfg.mass_props.mass) - - # check sim playing - self.sim.play() - for _ in range(10): - self.sim.step() - - def test_spawn_cone_with_deformable_and_density_props(self): - """Test spawning of UsdGeomMesh prim for a cone with deformable body and mass API. - - Note: - In this case, we specify the density instead of the mass. In that case, physics need to know - the collision shape to compute the mass. Thus, we have to set the collider properties. In - order to not have a collision shape, we disable the collision. - """ - # Spawn cone - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), - mass_props=sim_utils.MassPropertiesCfg(density=10.0), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual(prim.GetAttribute("physics:density").Get(), cfg.mass_props.density) - # check sim playing - self.sim.play() - for _ in range(10): - self.sim.step() - - def test_spawn_cone_with_all_deformable_props(self): - """Test spawning of UsdGeomMesh prim for a cone with all deformable properties.""" - # Spawn cone - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - mass_props=sim_utils.MassPropertiesCfg(mass=5.0), - deformable_props=sim_utils.DeformableBodyPropertiesCfg(), - visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), - physics_material=sim_utils.materials.DeformableBodyMaterialCfg(), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone/geometry/material")) - # Check properties - # -- deformable body - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual(prim.GetAttribute("physxDeformable:deformableEnabled").Get(), True) - - # check sim playing - self.sim.play() - for _ in range(10): - self.sim.step() - - def test_spawn_cone_with_all_rigid_props(self): - """Test spawning of UsdGeomMesh prim for a cone with all rigid properties.""" - # Spawn cone - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - mass_props=sim_utils.MassPropertiesCfg(mass=5.0), - rigid_props=sim_utils.RigidBodyPropertiesCfg( - rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 - ), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), - physics_material=sim_utils.materials.RigidBodyMaterialCfg(), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone/geometry/material")) - # Check properties - # -- rigid body - prim = prim_utils.get_prim_at_path("/World/Cone") - self.assertEqual(prim.GetAttribute("physics:rigidBodyEnabled").Get(), cfg.rigid_props.rigid_body_enabled) - self.assertEqual( - prim.GetAttribute("physxRigidBody:solverPositionIterationCount").Get(), - cfg.rigid_props.solver_position_iteration_count, - ) - self.assertAlmostEqual( - prim.GetAttribute("physxRigidBody:sleepThreshold").Get(), cfg.rigid_props.sleep_threshold - ) - # -- mass - self.assertEqual(prim.GetAttribute("physics:mass").Get(), cfg.mass_props.mass) - # -- collision shape - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual(prim.GetAttribute("physics:collisionEnabled").Get(), True) - - # check sim playing - self.sim.play() - for _ in range(10): - self.sim.step() - - def test_spawn_deformable_rigid_cone_invalid(self): - """Test specifying both rigid and deformable properties for a cone causes an error.""" - # Spawn cone - with self.assertRaises(ValueError): - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - mass_props=sim_utils.MassPropertiesCfg(mass=5.0), - rigid_props=sim_utils.RigidBodyPropertiesCfg(rigid_body_enabled=True), - deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), - ) - cfg.func("/World/Cone", cfg) - - def test_spawn_deformable_collider_cone_invalid(self): - """Test specifying both deformable and collider properties for a cone causes an error.""" - # Spawn cone - with self.assertRaises(ValueError): - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), - deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), - ) - cfg.func("/World/Cone", cfg) - - def test_spawn_deformable_incorrect_material(self): - """Test specifying incorrect material for deformable object causes an error.""" - # Spawn cone - with self.assertRaises(ValueError): - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), - visual_material=sim_utils.materials.PreviewSurfaceCfg(), - physics_material=sim_utils.materials.RigidBodyMaterialCfg(), - ) - cfg.func("/World/Cone", cfg) - - def test_spawn_rigid_incorrect_material(self): - """Test specifying incorrect material for rigid object causes an error.""" - # Spawn cone - with self.assertRaises(ValueError): - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - mass_props=sim_utils.MassPropertiesCfg(mass=5.0), - rigid_props=sim_utils.RigidBodyPropertiesCfg(rigid_body_enabled=True), - visual_material=sim_utils.materials.PreviewSurfaceCfg(), - physics_material=sim_utils.materials.DeformableBodyMaterialCfg(), - ) - cfg.func("/World/Cone", cfg) - - """ - Cloning. - """ - - def test_spawn_cone_clones_invalid_paths(self): - """Test spawning of cone clones on invalid cloning paths.""" - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - # Spawn cone - cfg = sim_utils.MeshConeCfg(radius=1.0, height=2.0, copy_from_source=True) - # Should raise error for invalid path - with self.assertRaises(RuntimeError): - cfg.func("/World/env/env_.*/Cone", cfg) - - def test_spawn_cone_clones(self): - """Test spawning of cone clones.""" - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - # Spawn cone - cfg = sim_utils.MeshConeCfg(radius=1.0, height=2.0, copy_from_source=True) - prim = cfg.func("/World/env_.*/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertEqual(prim_utils.get_prim_path(prim), "/World/env_0/Cone") - # Find matching prims - prims = prim_utils.find_matching_prim_paths("/World/env_*/Cone") - self.assertEqual(len(prims), num_clones) - - def test_spawn_cone_clone_with_all_props_global_material(self): - """Test spawning of cone clones with global material reference.""" - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - # Spawn cone - cfg = sim_utils.MeshConeCfg( - radius=1.0, - height=2.0, - mass_props=sim_utils.MassPropertiesCfg(mass=5.0), - deformable_props=sim_utils.DeformableBodyPropertiesCfg(), - visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), - physics_material=sim_utils.materials.DeformableBodyMaterialCfg(), - visual_material_path="/Looks/visualMaterial", - physics_material_path="/Looks/physicsMaterial", - ) - prim = cfg.func("/World/env_.*/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertEqual(prim_utils.get_prim_path(prim), "/World/env_0/Cone") - # Find matching prims - prims = prim_utils.find_matching_prim_paths("/World/env_*/Cone") - self.assertEqual(len(prims), num_clones) - # Find global materials - prims = prim_utils.find_matching_prim_paths("/Looks/visualMaterial.*") - self.assertEqual(len(prims), 1) - - -if __name__ == "__main__": - run_tests() + # Spawn cone + cfg = sim_utils.MeshConeCfg( + radius=1.0, + height=2.0, + deformable_props=sim_utils.DeformableBodyPropertiesCfg(deformable_enabled=True), + mass_props=sim_utils.MassPropertiesCfg(density=10.0), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetAttribute("physics:density").Get() == cfg.mass_props.density + # check sim playing + sim.play() + for _ in range(10): + sim.step() + + +def test_spawn_cone_with_all_deformable_props(sim): + """Test spawning of UsdGeomMesh prim for a cone with all deformable properties.""" + # Spawn cone + cfg = sim_utils.MeshConeCfg( + radius=1.0, + height=2.0, + mass_props=sim_utils.MassPropertiesCfg(mass=5.0), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(), + visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), + physics_material=sim_utils.materials.DeformableBodyMaterialCfg(), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone/geometry/material").IsValid() + # Check properties + # -- deformable body + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetAttribute("physxDeformable:deformableEnabled").Get() is True + + # check sim playing + sim.play() + for _ in range(10): + sim.step() + + +def test_spawn_cone_with_all_rigid_props(sim): + """Test spawning of UsdGeomMesh prim for a cone with all rigid properties.""" + # Spawn cone + cfg = sim_utils.MeshConeCfg( + radius=1.0, + height=2.0, + mass_props=sim_utils.MassPropertiesCfg(mass=5.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 + ), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), + physics_material=sim_utils.materials.RigidBodyMaterialCfg(), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone/geometry/material").IsValid() + # Check properties + # -- rigid body + prim = sim.stage.GetPrimAtPath("/World/Cone") + assert prim.GetAttribute("physics:rigidBodyEnabled").Get() == cfg.rigid_props.rigid_body_enabled + assert ( + prim.GetAttribute("physxRigidBody:solverPositionIterationCount").Get() + == cfg.rigid_props.solver_position_iteration_count + ) + assert prim.GetAttribute("physxRigidBody:sleepThreshold").Get() == pytest.approx(cfg.rigid_props.sleep_threshold) + # -- mass + assert prim.GetAttribute("physics:mass").Get() == cfg.mass_props.mass + # -- collision shape + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetAttribute("physics:collisionEnabled").Get() is True + + # check sim playing + sim.play() + for _ in range(10): + sim.step() diff --git a/source/isaaclab/test/sim/test_spawn_sensors.py b/source/isaaclab/test/sim/test_spawn_sensors.py index 157957146187..63f29af7830c 100644 --- a/source/isaaclab/test/sim/test_spawn_sensors.py +++ b/source/isaaclab/test/sim/test_spawn_sensors.py @@ -1,125 +1,114 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import pytest + +from pxr import Usd import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.sim.spawners.sensors.sensors import CUSTOM_FISHEYE_CAMERA_ATTRIBUTES, CUSTOM_PINHOLE_CAMERA_ATTRIBUTES from isaaclab.utils.string import to_camel_case -class TestSpawningSensors(unittest.TestCase): - """Test fixture for checking spawning of USD sensors with different settings.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - # Wait for spawning - stage_utils.update_stage() - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Basic spawning. - """ - - def test_spawn_pinhole_camera(self): - """Test spawning a pinhole camera.""" - cfg = sim_utils.PinholeCameraCfg( - focal_length=5.0, f_stop=10.0, clipping_range=(0.1, 1000.0), horizontal_aperture=10.0 - ) - prim = cfg.func("/World/pinhole_camera", cfg) - - # check if the light is spawned - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/pinhole_camera")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Camera") - # validate properties on the prim - self._validate_properties_on_prim("/World/pinhole_camera", cfg, CUSTOM_PINHOLE_CAMERA_ATTRIBUTES) - - def test_spawn_fisheye_camera(self): - """Test spawning a fisheye camera.""" - cfg = sim_utils.FisheyeCameraCfg( - projection_type="fisheyePolynomial", - focal_length=5.0, - f_stop=10.0, - clipping_range=(0.1, 1000.0), - horizontal_aperture=10.0, - ) - # FIXME: This throws a warning. Check with Replicator team if this is expected/known. - # [omni.hydra] Camera '/World/fisheye_camera': Unknown projection type, defaulting to pinhole - prim = cfg.func("/World/fisheye_camera", cfg) - - # check if the light is spawned - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/fisheye_camera")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Camera") - # validate properties on the prim - self._validate_properties_on_prim("/World/fisheye_camera", cfg, CUSTOM_FISHEYE_CAMERA_ATTRIBUTES) - +@pytest.fixture +def sim(): + """Create a simulation context.""" + sim_utils.create_new_stage() + dt = 0.1 + sim = SimulationContext(SimulationCfg(dt=dt)) + sim_utils.update_stage() + yield sim + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +""" +Basic spawning. +""" + + +def test_spawn_pinhole_camera(sim): + """Test spawning a pinhole camera.""" + cfg = sim_utils.PinholeCameraCfg( + focal_length=5.0, f_stop=10.0, clipping_range=(0.1, 1000.0), horizontal_aperture=10.0 + ) + prim = cfg.func("/World/pinhole_camera", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/pinhole_camera").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Camera" + # Check properties + _validate_properties_on_prim(prim, cfg, CUSTOM_PINHOLE_CAMERA_ATTRIBUTES) + + +def test_spawn_fisheye_camera(sim): + """Test spawning a fisheye camera.""" + cfg = sim_utils.FisheyeCameraCfg( + projection_type="fisheyePolynomial", + focal_length=5.0, + f_stop=10.0, + clipping_range=(0.1, 1000.0), + horizontal_aperture=10.0, + ) + # FIXME: This throws a warning. Check with Replicator team if this is expected/known. + # [omni.hydra] Camera '/World/fisheye_camera': Unknown projection type, defaulting to pinhole + prim = cfg.func("/World/fisheye_camera", cfg) + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/fisheye_camera").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Camera" + # Check properties + _validate_properties_on_prim(prim, cfg, CUSTOM_FISHEYE_CAMERA_ATTRIBUTES) + + +""" +Helper functions. +""" + + +def _validate_properties_on_prim(prim: Usd.Prim, cfg: object, custom_attr: dict): + """Validate the properties on the prim. + + Args: + prim: The prim. + cfg: The configuration object. + custom_attr: The custom attributes for sensor. """ - Helper functions. - """ - - def _validate_properties_on_prim(self, prim_path: str, cfg: object, custom_attr: dict): - """Validate the properties on the prim. - - Args: - prim_path: The prim name. - cfg: The configuration object. - custom_attr: The custom attributes for sensor. - """ - # delete custom attributes in the config that are not USD parameters - non_usd_cfg_param_names = [ - "func", - "copy_from_source", - "lock_camera", - "visible", - "semantic_tags", - "from_intrinsic_matrix", - ] - # get prim - prim = prim_utils.get_prim_at_path(prim_path) - for attr_name, attr_value in cfg.__dict__.items(): - # skip names we know are not present - if attr_name in non_usd_cfg_param_names or attr_value is None: - continue - # obtain prim property name - if attr_name in custom_attr: - # check custom attributes - prim_prop_name = custom_attr[attr_name][0] - else: - # convert attribute name in prim to cfg name - prim_prop_name = to_camel_case(attr_name, to="cC") - # validate the values - self.assertAlmostEqual(prim.GetAttribute(prim_prop_name).Get(), attr_value, places=5) - - -if __name__ == "__main__": - run_tests() + # delete custom attributes in the config that are not USD parameters + non_usd_cfg_param_names = [ + "func", + "copy_from_source", + "lock_camera", + "visible", + "semantic_tags", + "from_intrinsic_matrix", + ] + # validate the properties + for attr_name, attr_value in cfg.__dict__.items(): + # skip names we know are not present + if attr_name in non_usd_cfg_param_names or attr_value is None: + continue + # obtain prim property name + if attr_name in custom_attr: + # check custom attributes + prim_prop_name = custom_attr[attr_name][0] + else: + # convert attribute name in prim to cfg name + prim_prop_name = to_camel_case(attr_name, to="cC") + # validate the values + assert prim.GetAttribute(prim_prop_name).Get() == pytest.approx(attr_value, rel=1e-5) diff --git a/source/isaaclab/test/sim/test_spawn_shapes.py b/source/isaaclab/test/sim/test_spawn_shapes.py index fcd481ac54db..4c18753d52ea 100644 --- a/source/isaaclab/test/sim/test_spawn_shapes.py +++ b/source/isaaclab/test/sim/test_spawn_shapes.py @@ -1,304 +1,303 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest - -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import pytest import isaaclab.sim as sim_utils - - -class TestSpawningUsdGeometries(unittest.TestCase): - """Test fixture for checking spawning of USDGeom prim with different settings.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - # Wait for spawning - stage_utils.update_stage() - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - +from isaaclab.sim import SimulationCfg, SimulationContext + + +@pytest.fixture +def sim(): + """Create a simulation context.""" + sim_utils.create_new_stage() + dt = 0.1 + sim = SimulationContext(SimulationCfg(dt=dt)) + sim_utils.update_stage() + yield sim + sim._disable_app_control_on_stop_handle = True # prevent timeout + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +""" +Basic spawning. +""" + + +def test_spawn_cone(sim): + """Test spawning of UsdGeom.Cone prim.""" + cfg = sim_utils.ConeCfg(radius=1.0, height=2.0, axis="Y") + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Cone" + assert prim.GetAttribute("radius").Get() == cfg.radius + assert prim.GetAttribute("height").Get() == cfg.height + assert prim.GetAttribute("axis").Get() == cfg.axis + + +def test_spawn_capsule(sim): + """Test spawning of UsdGeom.Capsule prim.""" + cfg = sim_utils.CapsuleCfg(radius=1.0, height=2.0, axis="Y") + prim = cfg.func("/World/Capsule", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Capsule").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Capsule/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Capsule" + assert prim.GetAttribute("radius").Get() == cfg.radius + assert prim.GetAttribute("height").Get() == cfg.height + assert prim.GetAttribute("axis").Get() == cfg.axis + + +def test_spawn_cylinder(sim): + """Test spawning of UsdGeom.Cylinder prim.""" + cfg = sim_utils.CylinderCfg(radius=1.0, height=2.0, axis="Y") + prim = cfg.func("/World/Cylinder", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cylinder").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cylinder/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Cylinder" + assert prim.GetAttribute("radius").Get() == cfg.radius + assert prim.GetAttribute("height").Get() == cfg.height + assert prim.GetAttribute("axis").Get() == cfg.axis + + +def test_spawn_cuboid(sim): + """Test spawning of UsdGeom.Cube prim.""" + cfg = sim_utils.CuboidCfg(size=(1.0, 2.0, 3.0)) + prim = cfg.func("/World/Cube", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cube").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cube/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Cube" + assert prim.GetAttribute("size").Get() == min(cfg.size) + + +def test_spawn_sphere(sim): + """Test spawning of UsdGeom.Sphere prim.""" + cfg = sim_utils.SphereCfg(radius=1.0) + prim = cfg.func("/World/Sphere", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Sphere").IsValid() + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Sphere/geometry/mesh") + assert prim.GetPrimTypeInfo().GetTypeName() == "Sphere" + assert prim.GetAttribute("radius").Get() == cfg.radius + + +""" +Physics properties. +""" + + +def test_spawn_cone_with_rigid_props(sim): + """Test spawning of UsdGeom.Cone prim with rigid body API. + + Note: + Playing the simulation in this case will give a warning that no mass is specified! + Need to also setup mass and colliders. """ - Basic spawning. - """ - - def test_spawn_cone(self): - """Test spawning of UsdGeom.Cone prim.""" - # Spawn cone - cfg = sim_utils.ConeCfg(radius=1.0, height=2.0, axis="Y") - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Cone") - self.assertEqual(prim.GetAttribute("radius").Get(), cfg.radius) - self.assertEqual(prim.GetAttribute("height").Get(), cfg.height) - self.assertEqual(prim.GetAttribute("axis").Get(), cfg.axis) - - def test_spawn_capsule(self): - """Test spawning of UsdGeom.Capsule prim.""" - # Spawn capsule - cfg = sim_utils.CapsuleCfg(radius=1.0, height=2.0, axis="Y") - prim = cfg.func("/World/Capsule", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Capsule")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Capsule/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Capsule") - self.assertEqual(prim.GetAttribute("radius").Get(), cfg.radius) - self.assertEqual(prim.GetAttribute("height").Get(), cfg.height) - self.assertEqual(prim.GetAttribute("axis").Get(), cfg.axis) - - def test_spawn_cylinder(self): - """Test spawning of UsdGeom.Cylinder prim.""" - # Spawn cylinder - cfg = sim_utils.CylinderCfg(radius=1.0, height=2.0, axis="Y") - prim = cfg.func("/World/Cylinder", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cylinder")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cylinder/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Cylinder") - self.assertEqual(prim.GetAttribute("radius").Get(), cfg.radius) - self.assertEqual(prim.GetAttribute("height").Get(), cfg.height) - self.assertEqual(prim.GetAttribute("axis").Get(), cfg.axis) - - def test_spawn_cuboid(self): - """Test spawning of UsdGeom.Cube prim.""" - # Spawn cuboid - cfg = sim_utils.CuboidCfg(size=(1.0, 2.0, 3.0)) - prim = cfg.func("/World/Cube", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cube")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cube/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Cube") - self.assertEqual(prim.GetAttribute("size").Get(), min(cfg.size)) - - def test_spawn_sphere(self): - """Test spawning of UsdGeom.Sphere prim.""" - # Spawn sphere - cfg = sim_utils.SphereCfg(radius=1.0) - prim = cfg.func("/World/Sphere", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Sphere")) - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Xform") - # Check properties - prim = prim_utils.get_prim_at_path("/World/Sphere/geometry/mesh") - self.assertEqual(prim.GetPrimTypeInfo().GetTypeName(), "Sphere") - self.assertEqual(prim.GetAttribute("radius").Get(), cfg.radius) - + cfg = sim_utils.ConeCfg( + radius=1.0, + height=2.0, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 + ), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cone") + assert prim.GetAttribute("physics:rigidBodyEnabled").Get() == cfg.rigid_props.rigid_body_enabled + assert ( + prim.GetAttribute("physxRigidBody:solverPositionIterationCount").Get() + == cfg.rigid_props.solver_position_iteration_count + ) + assert prim.GetAttribute("physxRigidBody:sleepThreshold").Get() == pytest.approx(cfg.rigid_props.sleep_threshold) + + +def test_spawn_cone_with_rigid_and_mass_props(sim): + """Test spawning of UsdGeom.Cone prim with rigid body and mass API.""" + cfg = sim_utils.ConeCfg( + radius=1.0, + height=2.0, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 + ), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cone") + assert prim.GetAttribute("physics:mass").Get() == cfg.mass_props.mass + + # check sim playing + sim.play() + for _ in range(10): + sim.step() + + +def test_spawn_cone_with_rigid_and_density_props(sim): + """Test spawning of UsdGeom.Cone prim with rigid body and mass API. + + Note: + In this case, we specify the density instead of the mass. In that case, physics need to know + the collision shape to compute the mass. Thus, we have to set the collider properties. In + order to not have a collision shape, we disable the collision. """ - Physics properties. - """ - - def test_spawn_cone_with_rigid_props(self): - """Test spawning of UsdGeom.Cone prim with rigid body API. - - Note: - Playing the simulation in this case will give a warning that no mass is specified! - Need to also setup mass and colliders. - """ - # Spawn cone - cfg = sim_utils.ConeCfg( - radius=1.0, - height=2.0, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 - ), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cone") - self.assertEqual(prim.GetAttribute("physics:rigidBodyEnabled").Get(), cfg.rigid_props.rigid_body_enabled) - self.assertEqual( - prim.GetAttribute("physxRigidBody:solverPositionIterationCount").Get(), - cfg.rigid_props.solver_position_iteration_count, - ) - self.assertAlmostEqual( - prim.GetAttribute("physxRigidBody:sleepThreshold").Get(), cfg.rigid_props.sleep_threshold - ) - - def test_spawn_cone_with_rigid_and_mass_props(self): - """Test spawning of UsdGeom.Cone prim with rigid body and mass API.""" - # Spawn cone - cfg = sim_utils.ConeCfg( - radius=1.0, - height=2.0, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 - ), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cone") - self.assertEqual(prim.GetAttribute("physics:mass").Get(), cfg.mass_props.mass) - - # check sim playing - self.sim.play() - for _ in range(10): - self.sim.step() - - def test_spawn_cone_with_rigid_and_density_props(self): - """Test spawning of UsdGeom.Cone prim with rigid body and mass API. - - Note: - In this case, we specify the density instead of the mass. In that case, physics need to know - the collision shape to compute the mass. Thus, we have to set the collider properties. In - order to not have a collision shape, we disable the collision. - """ - # Spawn cone - cfg = sim_utils.ConeCfg( - radius=1.0, - height=2.0, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 - ), - mass_props=sim_utils.MassPropertiesCfg(density=10.0), - collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=False), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - # Check properties - prim = prim_utils.get_prim_at_path("/World/Cone") - self.assertEqual(prim.GetAttribute("physics:density").Get(), cfg.mass_props.density) - # check sim playing - self.sim.play() - for _ in range(10): - self.sim.step() - - def test_spawn_cone_with_all_props(self): - """Test spawning of UsdGeom.Cone prim with all properties.""" - # Spawn cone - cfg = sim_utils.ConeCfg( - radius=1.0, - height=2.0, - mass_props=sim_utils.MassPropertiesCfg(mass=5.0), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), - physics_material=sim_utils.materials.RigidBodyMaterialCfg(), - ) - prim = cfg.func("/World/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone")) - self.assertTrue(prim_utils.is_prim_path_valid("/World/Cone/geometry/material")) - # Check properties - # -- rigid body - prim = prim_utils.get_prim_at_path("/World/Cone") - self.assertEqual(prim.GetAttribute("physics:rigidBodyEnabled").Get(), True) - # -- collision shape - prim = prim_utils.get_prim_at_path("/World/Cone/geometry/mesh") - self.assertEqual(prim.GetAttribute("physics:collisionEnabled").Get(), True) - - # check sim playing - self.sim.play() - for _ in range(10): - self.sim.step() - - """ - Cloning. - """ - - def test_spawn_cone_clones_invalid_paths(self): - """Test spawning of cone clones on invalid cloning paths.""" - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - # Spawn cone - cfg = sim_utils.ConeCfg(radius=1.0, height=2.0, copy_from_source=True) - # Should raise error for invalid path - with self.assertRaises(RuntimeError): - cfg.func("/World/env/env_.*/Cone", cfg) - - def test_spawn_cone_clones(self): - """Test spawning of cone clones.""" - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - # Spawn cone - cfg = sim_utils.ConeCfg(radius=1.0, height=2.0, copy_from_source=True) - prim = cfg.func("/World/env_.*/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertEqual(prim_utils.get_prim_path(prim), "/World/env_0/Cone") - # Find matching prims - prims = prim_utils.find_matching_prim_paths("/World/env_*/Cone") - self.assertEqual(len(prims), num_clones) - - def test_spawn_cone_clone_with_all_props_global_material(self): - """Test spawning of cone clones with global material reference.""" - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - # Spawn cone - cfg = sim_utils.ConeCfg( - radius=1.0, - height=2.0, - mass_props=sim_utils.MassPropertiesCfg(mass=5.0), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), - physics_material=sim_utils.materials.RigidBodyMaterialCfg(), - visual_material_path="/Looks/visualMaterial", - physics_material_path="/Looks/physicsMaterial", - ) - prim = cfg.func("/World/env_.*/Cone", cfg) - # Check validity - self.assertTrue(prim.IsValid()) - self.assertEqual(prim_utils.get_prim_path(prim), "/World/env_0/Cone") - # Find matching prims - prims = prim_utils.find_matching_prim_paths("/World/env_*/Cone") - self.assertEqual(len(prims), num_clones) - # Find global materials - prims = prim_utils.find_matching_prim_paths("/Looks/visualMaterial.*") - self.assertEqual(len(prims), 1) - - -if __name__ == "__main__": - run_tests() + cfg = sim_utils.ConeCfg( + radius=1.0, + height=2.0, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, solver_position_iteration_count=8, sleep_threshold=0.1 + ), + mass_props=sim_utils.MassPropertiesCfg(density=10.0), + collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=False), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + # Check properties + prim = sim.stage.GetPrimAtPath("/World/Cone") + assert prim.GetAttribute("physics:density").Get() == cfg.mass_props.density + + # check sim playing + sim.play() + for _ in range(10): + sim.step() + + +def test_spawn_cone_with_all_props(sim): + """Test spawning of UsdGeom.Cone prim with all properties.""" + cfg = sim_utils.ConeCfg( + radius=1.0, + height=2.0, + mass_props=sim_utils.MassPropertiesCfg(mass=5.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), + physics_material=sim_utils.materials.RigidBodyMaterialCfg(), + ) + prim = cfg.func("/World/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone").IsValid() + assert sim.stage.GetPrimAtPath("/World/Cone/geometry/material").IsValid() + # Check properties + # -- rigid body properties + prim = sim.stage.GetPrimAtPath("/World/Cone") + assert prim.GetAttribute("physics:rigidBodyEnabled").Get() is True + # -- collision properties + prim = sim.stage.GetPrimAtPath("/World/Cone/geometry/mesh") + assert prim.GetAttribute("physics:collisionEnabled").Get() is True + + # check sim playing + sim.play() + for _ in range(10): + sim.step() + + +""" +Cloning. +""" + + +def test_spawn_cone_clones_invalid_paths(sim): + """Test spawning of cone clones on invalid cloning paths.""" + num_clones = 10 + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + # Spawn cone on invalid cloning path -- should raise an error + cfg = sim_utils.ConeCfg(radius=1.0, height=2.0, copy_from_source=True) + with pytest.raises(RuntimeError): + cfg.func("/World/env/env_.*/Cone", cfg) + + +def test_spawn_cone_clones(sim): + """Test spawning of cone clones.""" + num_clones = 10 + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + # Spawn cone on valid cloning path + cfg = sim_utils.ConeCfg(radius=1.0, height=2.0, copy_from_source=True) + prim = cfg.func("/World/env_.*/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert str(prim.GetPath()) == "/World/env_0/Cone" + # find matching prims + prims = sim_utils.find_matching_prim_paths("/World/env_.*/Cone") + assert len(prims) == num_clones + + +def test_spawn_cone_clone_with_all_props_global_material(sim): + """Test spawning of cone clones with global material reference.""" + num_clones = 10 + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + # Spawn cone on valid cloning path + cfg = sim_utils.ConeCfg( + radius=1.0, + height=2.0, + mass_props=sim_utils.MassPropertiesCfg(mass=5.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.materials.PreviewSurfaceCfg(diffuse_color=(0.0, 0.75, 0.5)), + physics_material=sim_utils.materials.RigidBodyMaterialCfg(), + visual_material_path="/Looks/visualMaterial", + physics_material_path="/Looks/physicsMaterial", + ) + prim = cfg.func("/World/env_.*/Cone", cfg) + + # Check validity + assert prim.IsValid() + assert str(prim.GetPath()) == "/World/env_0/Cone" + # find matching prims + prims = sim_utils.find_matching_prim_paths("/World/env_.*/Cone") + assert len(prims) == num_clones + # find matching material prims + prims = sim_utils.find_matching_prim_paths("/Looks/visualMaterial.*") + assert len(prims) == 1 diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index f224f4d6b7c7..1571bb62bdc4 100644 --- a/source/isaaclab/test/sim/test_spawn_wrappers.py +++ b/source/isaaclab/test/sim/test_spawn_wrappers.py @@ -1,191 +1,161 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import pytest import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR -class TestSpawningWrappers(unittest.TestCase): - """Test fixture for checking spawning of multiple assets wrappers.""" - - def setUp(self) -> None: - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # Simulation time-step - self.dt = 0.1 - # Load kit helper - self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") - # Wait for spawning - stage_utils.update_stage() - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - """ - Tests - Multiple assets. - """ - - def test_spawn_multiple_shapes_with_global_settings(self): - """Test spawning of shapes randomly with global rigid body settings.""" - # Define prim parents - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - - # Spawn shapes - cfg = sim_utils.MultiAssetSpawnerCfg( - assets_cfg=[ - sim_utils.ConeCfg( - radius=0.3, - height=0.6, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), - mass_props=sim_utils.MassPropertiesCfg(mass=100.0), # this one should get overridden - ), - sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), metallic=0.2), - ), - sim_utils.SphereCfg( - radius=0.3, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), - ), - ], - random_choice=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - solver_position_iteration_count=4, solver_velocity_iteration_count=0 +@pytest.fixture +def sim(): + """Create a simulation context.""" + sim_utils.create_new_stage() + dt = 0.1 + sim = SimulationContext(SimulationCfg(dt=dt)) + sim_utils.update_stage() + yield sim + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +def test_spawn_multiple_shapes_with_global_settings(sim): + """Test spawning of shapes randomly with global rigid body settings.""" + num_clones = 10 + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + + cfg = sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.ConeCfg( + radius=0.3, + height=0.6, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), + mass_props=sim_utils.MassPropertiesCfg(mass=100.0), # this one should get overridden ), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), - ) - prim = cfg.func("/World/env_.*/Cone", cfg) - - # Check validity - self.assertTrue(prim.IsValid()) - self.assertEqual(prim_utils.get_prim_path(prim), "/World/env_0/Cone") - # Find matching prims - prim_paths = prim_utils.find_matching_prim_paths("/World/env_*/Cone") - self.assertEqual(len(prim_paths), num_clones) - - # Check all prims have correct settings - for prim_path in prim_paths: - prim = prim_utils.get_prim_at_path(prim_path) - self.assertEqual(prim.GetAttribute("physics:mass").Get(), cfg.mass_props.mass) - - def test_spawn_multiple_shapes_with_individual_settings(self): - """Test spawning of shapes randomly with individual rigid object settings""" - # Define prim parents - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - - # Make a list of masses - mass_variations = [2.0, 3.0, 4.0] - # Spawn shapes - cfg = sim_utils.MultiAssetSpawnerCfg( - assets_cfg=[ - sim_utils.ConeCfg( - radius=0.3, - height=0.6, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=mass_variations[0]), - collision_props=sim_utils.CollisionPropertiesCfg(), - ), - sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), metallic=0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=mass_variations[1]), - collision_props=sim_utils.CollisionPropertiesCfg(), - ), - sim_utils.SphereCfg( - radius=0.3, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=mass_variations[2]), - collision_props=sim_utils.CollisionPropertiesCfg(), - ), - ], - random_choice=True, - ) - prim = cfg.func("/World/env_.*/Cone", cfg) - - # Check validity - self.assertTrue(prim.IsValid()) - self.assertEqual(prim_utils.get_prim_path(prim), "/World/env_0/Cone") - # Find matching prims - prim_paths = prim_utils.find_matching_prim_paths("/World/env_*/Cone") - self.assertEqual(len(prim_paths), num_clones) - - # Check all prims have correct settings - for prim_path in prim_paths: - prim = prim_utils.get_prim_at_path(prim_path) - self.assertTrue(prim.GetAttribute("physics:mass").Get() in mass_variations) - - """ - Tests - Multiple USDs. - """ - - def test_spawn_multiple_files_with_global_settings(self): - """Test spawning of files randomly with global articulation settings.""" - # Define prim parents - num_clones = 10 - for i in range(num_clones): - prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) - - # Spawn shapes - cfg = sim_utils.MultiUsdFileCfg( - usd_path=[ - f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd", - f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd", - ], - random_choice=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, + sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), metallic=0.2), ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + sim_utils.SphereCfg( + radius=0.3, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), ), - activate_contact_sensors=True, - ) - prim = cfg.func("/World/env_.*/Robot", cfg) - - # Check validity - self.assertTrue(prim.IsValid()) - self.assertEqual(prim_utils.get_prim_path(prim), "/World/env_0/Robot") - # Find matching prims - prim_paths = prim_utils.find_matching_prim_paths("/World/env_*/Robot") - self.assertEqual(len(prim_paths), num_clones) - - -if __name__ == "__main__": - run_tests() + ], + random_choice=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ) + prim = cfg.func("/World/env_.*/Cone", cfg) + + assert prim.IsValid() + assert str(prim.GetPath()) == "/World/env_0/Cone" + prim_paths = sim_utils.find_matching_prim_paths("/World/env_.*/Cone") + assert len(prim_paths) == num_clones + + for prim_path in prim_paths: + prim = sim.stage.GetPrimAtPath(prim_path) + assert prim.GetAttribute("physics:mass").Get() == cfg.mass_props.mass + + +def test_spawn_multiple_shapes_with_individual_settings(sim): + """Test spawning of shapes randomly with individual rigid object settings.""" + num_clones = 10 + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + + mass_variations = [2.0, 3.0, 4.0] + cfg = sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.ConeCfg( + radius=0.3, + height=0.6, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=mass_variations[0]), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), metallic=0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=mass_variations[1]), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + sim_utils.SphereCfg( + radius=0.3, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=mass_variations[2]), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + ], + random_choice=True, + ) + prim = cfg.func("/World/env_.*/Cone", cfg) + + assert prim.IsValid() + assert str(prim.GetPath()) == "/World/env_0/Cone" + prim_paths = sim_utils.find_matching_prim_paths("/World/env_.*/Cone") + assert len(prim_paths) == num_clones + + for prim_path in prim_paths: + prim = sim.stage.GetPrimAtPath(prim_path) + assert prim.GetAttribute("physics:mass").Get() in mass_variations + + +""" +Tests - Multiple USDs. +""" + + +def test_spawn_multiple_files_with_global_settings(sim): + """Test spawning of files randomly with global articulation settings.""" + num_clones = 10 + for i in range(num_clones): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) + + cfg = sim_utils.MultiUsdFileCfg( + usd_path=[ + f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd", + f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd", + ], + random_choice=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 + ), + activate_contact_sensors=True, + ) + prim = cfg.func("/World/env_.*/Robot", cfg) + + assert prim.IsValid() + assert str(prim.GetPath()) == "/World/env_0/Robot" + prim_paths = sim_utils.find_matching_prim_paths("/World/env_.*/Robot") + assert len(prim_paths) == num_clones diff --git a/source/isaaclab/test/sim/test_urdf_converter.py b/source/isaaclab/test/sim/test_urdf_converter.py index 4cb2af01e02d..f91c58f015f9 100644 --- a/source/isaaclab/test/sim/test_urdf_converter.py +++ b/source/isaaclab/test/sim/test_urdf_converter.py @@ -1,144 +1,152 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -config = {"headless": True} -simulation_app = AppLauncher(config).app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import numpy as np import os -import unittest -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.api.simulation_context import SimulationContext +import numpy as np +import pytest +from packaging.version import Version + +import omni.kit.app from isaacsim.core.prims import Articulation -from isaacsim.core.utils.extensions import enable_extension, get_extension_path_from_name +import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg - - -class TestUrdfConverter(unittest.TestCase): - """Test fixture for the UrdfConverter class.""" - - def setUp(self): - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - # retrieve path to urdf importer extension - enable_extension("isaacsim.asset.importer.urdf") - extension_path = get_extension_path_from_name("isaacsim.asset.importer.urdf") - # default configuration - self.config = UrdfConverterCfg( - asset_path=f"{extension_path}/data/urdf/robots/franka_description/robots/panda_arm_hand.urdf", - fix_base=True, - joint_drive=UrdfConverterCfg.JointDriveCfg( - gains=UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=400.0, damping=40.0) - ), - ) - # Simulation time-step - self.dt = 0.01 - # Load kit helper - self.sim = SimulationContext( - physics_dt=self.dt, rendering_dt=self.dt, stage_units_in_meters=1.0, backend="numpy" - ) - - def tearDown(self) -> None: - """Stops simulator after each test.""" - # stop simulation - self.sim.stop() - # cleanup stage and context - self.sim.clear() - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - def test_no_change(self): - """Call conversion twice. This should not generate a new USD file.""" - - urdf_converter = UrdfConverter(self.config) - time_usd_file_created = os.stat(urdf_converter.usd_path).st_mtime_ns - - # no change to config only define the usd directory - new_config = self.config - new_config.usd_dir = urdf_converter.usd_dir - # convert to usd but this time in the same directory as previous step - new_urdf_converter = UrdfConverter(new_config) - new_time_usd_file_created = os.stat(new_urdf_converter.usd_path).st_mtime_ns - - self.assertEqual(time_usd_file_created, new_time_usd_file_created) - - def test_config_change(self): - """Call conversion twice but change the config in the second call. This should generate a new USD file.""" - - urdf_converter = UrdfConverter(self.config) - time_usd_file_created = os.stat(urdf_converter.usd_path).st_mtime_ns - - # change the config - new_config = self.config - new_config.fix_base = not self.config.fix_base - # define the usd directory - new_config.usd_dir = urdf_converter.usd_dir - # convert to usd but this time in the same directory as previous step - new_urdf_converter = UrdfConverter(new_config) - new_time_usd_file_created = os.stat(new_urdf_converter.usd_path).st_mtime_ns - - self.assertNotEqual(time_usd_file_created, new_time_usd_file_created) - - def test_create_prim_from_usd(self): - """Call conversion and create a prim from it.""" - - urdf_converter = UrdfConverter(self.config) - - prim_path = "/World/Robot" - prim_utils.create_prim(prim_path, usd_path=urdf_converter.usd_path) - - self.assertTrue(prim_utils.is_prim_path_valid(prim_path)) - - def test_config_drive_type(self): - """Change the drive mechanism of the robot to be position.""" - - # Create directory to dump results - test_dir = os.path.dirname(os.path.abspath(__file__)) - output_dir = os.path.join(test_dir, "output", "urdf_converter") - if not os.path.exists(output_dir): - os.makedirs(output_dir, exist_ok=True) - - # change the config - self.config.force_usd_conversion = True - self.config.joint_drive.target_type = "position" - self.config.joint_drive.gains.stiffness = 42.0 - self.config.joint_drive.gains.damping = 4.2 - self.config.usd_dir = output_dir - urdf_converter = UrdfConverter(self.config) - # check the drive type of the robot - prim_path = "/World/Robot" - prim_utils.create_prim(prim_path, usd_path=urdf_converter.usd_path) - - # access the robot - robot = Articulation(prim_path, reset_xform_properties=False) - # play the simulator and initialize the robot - self.sim.reset() - robot.initialize() - - # check drive values for the robot (read from physx) - drive_stiffness, drive_damping = robot.get_gains() - np.testing.assert_array_equal(drive_stiffness, self.config.joint_drive.gains.stiffness) - np.testing.assert_array_equal(drive_damping, self.config.joint_drive.gains.damping) - - # check drive values for the robot (read from usd) - self.sim.stop() - drive_stiffness, drive_damping = robot.get_gains() - np.testing.assert_array_equal(drive_stiffness, self.config.joint_drive.gains.stiffness) - np.testing.assert_array_equal(drive_damping, self.config.joint_drive.gains.damping) - - -if __name__ == "__main__": - run_tests() +from isaaclab.utils.version import get_isaac_sim_version + + +# Create a fixture for setup and teardown +@pytest.fixture +def sim_config(): + # Create a new stage + sim_utils.create_new_stage() + # pin the urdf importer extension to the older version + manager = omni.kit.app.get_app().get_extension_manager() + if get_isaac_sim_version() >= Version("5.1"): + pinned_urdf_extension_name = "isaacsim.asset.importer.urdf-2.4.31" + manager.set_extension_enabled_immediate(pinned_urdf_extension_name, True) + else: + pinned_urdf_extension_name = "isaacsim.asset.importer.urdf" + # obtain the extension path + extension_id = manager.get_enabled_extension_id(pinned_urdf_extension_name) + extension_path = manager.get_extension_path(extension_id) + # default configuration + config = UrdfConverterCfg( + asset_path=f"{extension_path}/data/urdf/robots/franka_description/robots/panda_arm_hand.urdf", + fix_base=True, + joint_drive=UrdfConverterCfg.JointDriveCfg( + gains=UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=400.0, damping=40.0) + ), + ) + # Simulation time-step + dt = 0.01 + # Load kit helper + sim = SimulationContext(SimulationCfg(dt=dt)) + yield sim, config + # Teardown + sim._disable_app_control_on_stop_handle = True # prevent timeout + sim.stop() + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_no_change(sim_config): + """Call conversion twice. This should not generate a new USD file.""" + sim, config = sim_config + urdf_converter = UrdfConverter(config) + time_usd_file_created = os.stat(urdf_converter.usd_path).st_mtime_ns + + # no change to config only define the usd directory + new_config = config + new_config.usd_dir = urdf_converter.usd_dir + # convert to usd but this time in the same directory as previous step + new_urdf_converter = UrdfConverter(new_config) + new_time_usd_file_created = os.stat(new_urdf_converter.usd_path).st_mtime_ns + + assert time_usd_file_created == new_time_usd_file_created + + +@pytest.mark.isaacsim_ci +def test_config_change(sim_config): + """Call conversion twice but change the config in the second call. This should generate a new USD file.""" + sim, config = sim_config + urdf_converter = UrdfConverter(config) + time_usd_file_created = os.stat(urdf_converter.usd_path).st_mtime_ns + + # change the config + new_config = config + new_config.fix_base = not config.fix_base + # define the usd directory + new_config.usd_dir = urdf_converter.usd_dir + # convert to usd but this time in the same directory as previous step + new_urdf_converter = UrdfConverter(new_config) + new_time_usd_file_created = os.stat(new_urdf_converter.usd_path).st_mtime_ns + + assert time_usd_file_created != new_time_usd_file_created + + +@pytest.mark.isaacsim_ci +def test_create_prim_from_usd(sim_config): + """Call conversion and create a prim from it.""" + sim, config = sim_config + urdf_converter = UrdfConverter(config) + + prim_path = "/World/Robot" + sim_utils.create_prim(prim_path, usd_path=urdf_converter.usd_path) + + assert sim.stage.GetPrimAtPath(prim_path).IsValid() + + +@pytest.mark.isaacsim_ci +def test_config_drive_type(sim_config): + """Change the drive mechanism of the robot to be position.""" + sim, config = sim_config + # Create directory to dump results + test_dir = os.path.dirname(os.path.abspath(__file__)) + output_dir = os.path.join(test_dir, "output", "urdf_converter") + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + # change the config + config.force_usd_conversion = True + config.joint_drive.target_type = "position" + config.joint_drive.gains.stiffness = 42.0 + config.joint_drive.gains.damping = 4.2 + config.usd_dir = output_dir + urdf_converter = UrdfConverter(config) + # check the drive type of the robot + prim_path = "/World/Robot" + sim_utils.create_prim(prim_path, usd_path=urdf_converter.usd_path) + + # access the robot + robot = Articulation(prim_path, reset_xform_properties=False) + # play the simulator and initialize the robot + sim.reset() + robot.initialize() + + # check drive values for the robot (read from physx) + drive_stiffness, drive_damping = robot.get_gains() + np.testing.assert_array_equal(drive_stiffness.cpu().numpy(), config.joint_drive.gains.stiffness) + np.testing.assert_array_equal(drive_damping.cpu().numpy(), config.joint_drive.gains.damping) + + # check drive values for the robot (read from usd) + # Note: Disable the app control callback to prevent hanging during sim.stop() + sim._disable_app_control_on_stop_handle = True + sim.stop() + drive_stiffness, drive_damping = robot.get_gains() + np.testing.assert_array_equal(drive_stiffness.cpu().numpy(), config.joint_drive.gains.stiffness) + np.testing.assert_array_equal(drive_damping.cpu().numpy(), config.joint_drive.gains.damping) diff --git a/source/isaaclab/test/sim/test_utils.py b/source/isaaclab/test/sim/test_utils.py deleted file mode 100644 index cf42e4ba999c..000000000000 --- a/source/isaaclab/test/sim/test_utils.py +++ /dev/null @@ -1,130 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Launch Isaac Sim Simulator first.""" - -from isaaclab.app import AppLauncher, run_tests - -# launch omniverse app -config = {"headless": True} -simulation_app = AppLauncher(config).app - -"""Rest everything follows.""" - -import numpy as np -import unittest - -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from pxr import Sdf, Usd, UsdGeom - -import isaaclab.sim as sim_utils -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR - - -class TestUtilities(unittest.TestCase): - """Test fixture for the sim utility functions.""" - - def setUp(self): - """Create a blank new stage for each test.""" - # Create a new stage - stage_utils.create_new_stage() - stage_utils.update_stage() - - def tearDown(self) -> None: - """Clear stage after each test.""" - stage_utils.clear_stage() - - def test_get_all_matching_child_prims(self): - """Test get_all_matching_child_prims() function.""" - # create scene - prim_utils.create_prim("/World/Floor") - prim_utils.create_prim( - "/World/Floor/thefloor", "Cube", position=np.array([75, 75, -150.1]), attributes={"size": 300} - ) - prim_utils.create_prim("/World/Room", "Sphere", attributes={"radius": 1e3}) - - # test - isaac_sim_result = prim_utils.get_all_matching_child_prims("/World") - isaaclab_result = sim_utils.get_all_matching_child_prims("/World") - self.assertListEqual(isaac_sim_result, isaaclab_result) - - # test valid path - with self.assertRaises(ValueError): - sim_utils.get_all_matching_child_prims("World/Room") - - def test_find_matching_prim_paths(self): - """Test find_matching_prim_paths() function.""" - # create scene - for index in range(2048): - random_pos = np.random.uniform(-100, 100, size=3) - prim_utils.create_prim(f"/World/Floor_{index}", "Cube", position=random_pos, attributes={"size": 2.0}) - prim_utils.create_prim(f"/World/Floor_{index}/Sphere", "Sphere", attributes={"radius": 10}) - prim_utils.create_prim(f"/World/Floor_{index}/Sphere/childSphere", "Sphere", attributes={"radius": 1}) - prim_utils.create_prim(f"/World/Floor_{index}/Sphere/childSphere2", "Sphere", attributes={"radius": 1}) - - # test leaf paths - isaac_sim_result = prim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere") - isaaclab_result = sim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere") - self.assertListEqual(isaac_sim_result, isaaclab_result) - - # test non-leaf paths - isaac_sim_result = prim_utils.find_matching_prim_paths("/World/Floor_.*") - isaaclab_result = sim_utils.find_matching_prim_paths("/World/Floor_.*") - self.assertListEqual(isaac_sim_result, isaaclab_result) - - # test child-leaf paths - isaac_sim_result = prim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere/childSphere.*") - isaaclab_result = sim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere/childSphere.*") - self.assertListEqual(isaac_sim_result, isaaclab_result) - - # test valid path - with self.assertRaises(ValueError): - sim_utils.get_all_matching_child_prims("World/Floor_.*") - - def test_find_global_fixed_joint_prim(self): - """Test find_global_fixed_joint_prim() function.""" - # create scene - prim_utils.create_prim("/World") - prim_utils.create_prim( - "/World/ANYmal", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd" - ) - prim_utils.create_prim( - "/World/Franka", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" - ) - prim_utils.create_prim("/World/Franka_Isaac", usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka.usd") - - # test - self.assertIsNone(sim_utils.find_global_fixed_joint_prim("/World/ANYmal")) - self.assertIsNotNone(sim_utils.find_global_fixed_joint_prim("/World/Franka")) - self.assertIsNotNone(sim_utils.find_global_fixed_joint_prim("/World/Franka_Isaac")) - - # make fixed joint disabled manually - joint_prim = sim_utils.find_global_fixed_joint_prim("/World/Franka") - joint_prim.GetJointEnabledAttr().Set(False) - self.assertIsNotNone(sim_utils.find_global_fixed_joint_prim("/World/Franka")) - self.assertIsNone(sim_utils.find_global_fixed_joint_prim("/World/Franka", check_enabled_only=True)) - - def test_select_usd_variants(self): - """Test select_usd_variants() function.""" - stage = stage_utils.get_current_stage() - prim: Usd.Prim = UsdGeom.Xform.Define(stage, Sdf.Path("/World")).GetPrim() - stage.SetDefaultPrim(prim) - - # Create the variant set and add your variants to it. - variants = ["red", "blue", "green"] - variant_set = prim.GetVariantSets().AddVariantSet("colors") - for variant in variants: - variant_set.AddVariant(variant) - - # Set the variant selection - sim_utils.utils.select_usd_variants("/World", {"colors": "red"}, stage) - - # Check if the variant selection is correct - self.assertEqual(variant_set.GetVariantSelection(), "red") - - -if __name__ == "__main__": - run_tests() diff --git a/source/isaaclab/test/sim/test_utils_prims.py b/source/isaaclab/test/sim/test_utils_prims.py new file mode 100644 index 000000000000..16584d113ed7 --- /dev/null +++ b/source/isaaclab/test/sim/test_utils_prims.py @@ -0,0 +1,716 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +# note: need to enable cameras to be able to make replicator core available +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +"""Rest everything follows.""" + +import math + +import numpy as np +import pytest +import torch + +from pxr import Gf, Sdf, Usd, UsdGeom + +import isaaclab.sim as sim_utils +from isaaclab.sim.utils.prims import _to_tuple # type: ignore[reportPrivateUsage] +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + sim_utils.update_stage() + + # Yield for the test + yield + + # Teardown: Clear stage after each test + sim_utils.clear_stage() + + +def assert_quat_close(q1: Gf.Quatf | Gf.Quatd, q2: Gf.Quatf | Gf.Quatd, eps: float = 1e-6): + """Assert two quaternions are close.""" + assert math.isclose(q1.GetReal(), q2.GetReal(), abs_tol=eps) + for i in range(3): + assert math.isclose(q1.GetImaginary()[i], q2.GetImaginary()[i], abs_tol=eps) + + +""" +General Utils +""" + + +def test_create_prim(): + """Test create_prim() function.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create scene + prim = sim_utils.create_prim(prim_path="/World/Test", prim_type="Xform", stage=stage) + # check prim created + assert prim.IsValid() + assert prim.GetPrimPath() == "/World/Test" + assert prim.GetTypeName() == "Xform" + + # check recreation of prim + with pytest.raises(ValueError, match="already exists"): + sim_utils.create_prim(prim_path="/World/Test", prim_type="Xform", stage=stage) + + # check attribute setting + prim = sim_utils.create_prim(prim_path="/World/Test/Cube", prim_type="Cube", stage=stage, attributes={"size": 100}) + # check attribute set + assert prim.IsValid() + assert prim.GetPrimPath() == "/World/Test/Cube" + assert prim.GetTypeName() == "Cube" + assert prim.GetAttribute("size").Get() == 100 + + # check adding USD reference + franka_usd = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + prim = sim_utils.create_prim("/World/Test/USDReference", usd_path=franka_usd, stage=stage) + # check USD reference set + assert prim.IsValid() + assert prim.GetPrimPath() == "/World/Test/USDReference" + assert prim.GetTypeName() == "Xform" + # get the reference of the prim + references = [] + for prim_spec in prim.GetPrimStack(): + references.extend(prim_spec.referenceList.prependedItems) + assert len(references) == 1 + assert str(references[0].assetPath) == franka_usd + + # check adding semantic label + prim = sim_utils.create_prim( + "/World/Test/Sphere", "Sphere", stage=stage, semantic_label="sphere", attributes={"radius": 10.0} + ) + # check semantic label set + assert prim.IsValid() + assert prim.GetPrimPath() == "/World/Test/Sphere" + assert prim.GetTypeName() == "Sphere" + assert prim.GetAttribute("radius").Get() == 10.0 + assert sim_utils.get_labels(prim)["class"] == ["sphere"] + + # check setting transform + pos = (1.0, 2.0, 3.0) + quat = (0.0, 0.0, 0.0, 1.0) + scale = (1.0, 0.5, 0.5) + prim = sim_utils.create_prim( + "/World/Test/Xform", "Xform", stage=stage, translation=pos, orientation=quat, scale=scale + ) + # check transform set + assert prim.IsValid() + assert prim.GetPrimPath() == "/World/Test/Xform" + assert prim.GetTypeName() == "Xform" + assert prim.GetAttribute("xformOp:translate").Get() == Gf.Vec3d(pos) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), Gf.Quatd(*quat)) + assert prim.GetAttribute("xformOp:scale").Get() == Gf.Vec3d(scale) + # check xform operation order + op_names = [op.GetOpName() for op in UsdGeom.Xformable(prim).GetOrderedXformOps()] + assert op_names == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +@pytest.mark.parametrize( + "input_type", + ["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], + ids=["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], +) +def test_create_prim_with_different_input_types(input_type: str): + """Test create_prim() with different input types (list, tuple, numpy array, torch tensor).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Define test values + translation_vals = [1.0, 2.0, 3.0] + orientation_vals = [1.0, 0.0, 0.0, 0.0] # w, x, y, z + scale_vals = [2.0, 3.0, 4.0] + + # Convert to the specified input type + if input_type == "list": + translation = translation_vals + orientation = orientation_vals + scale = scale_vals + elif input_type == "tuple": + translation = tuple(translation_vals) + orientation = tuple(orientation_vals) + scale = tuple(scale_vals) + elif input_type == "numpy": + translation = np.array(translation_vals) + orientation = np.array(orientation_vals) + scale = np.array(scale_vals) + elif input_type == "torch_cpu": + translation = torch.tensor(translation_vals) + orientation = torch.tensor(orientation_vals) + scale = torch.tensor(scale_vals) + elif input_type == "torch_cuda": + if not torch.cuda.is_available(): + pytest.skip("CUDA not available") + translation = torch.tensor(translation_vals, device="cuda") + orientation = torch.tensor(orientation_vals, device="cuda") + scale = torch.tensor(scale_vals, device="cuda") + + # Create prim with translation (local space) + prim = sim_utils.create_prim( + f"/World/Test/Xform_{input_type}", + "Xform", + stage=stage, + translation=translation, + orientation=orientation, + scale=scale, + ) + + # Verify prim was created correctly + assert prim.IsValid() + assert prim.GetPrimPath() == f"/World/Test/Xform_{input_type}" + + # Verify transform values + assert prim.GetAttribute("xformOp:translate").Get() == Gf.Vec3d(*translation_vals) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), Gf.Quatd(*orientation_vals)) + assert prim.GetAttribute("xformOp:scale").Get() == Gf.Vec3d(*scale_vals) + + # Verify xform operation order + op_names = [op.GetOpName() for op in UsdGeom.Xformable(prim).GetOrderedXformOps()] + assert op_names == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +@pytest.mark.parametrize( + "input_type", + ["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], + ids=["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], +) +def test_create_prim_with_world_position_different_types(input_type: str): + """Test create_prim() with world position using different input types.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a parent prim + _ = sim_utils.create_prim( + "/World/Parent", + "Xform", + stage=stage, + translation=(5.0, 10.0, 15.0), + orientation=(1.0, 0.0, 0.0, 0.0), + ) + + # Define world position and orientation values + world_pos_vals = [10.0, 20.0, 30.0] + world_orient_vals = [0.7071068, 0.0, 0.7071068, 0.0] # 90 deg around Y + + # Convert to the specified input type + if input_type == "list": + world_pos = world_pos_vals + world_orient = world_orient_vals + elif input_type == "tuple": + world_pos = tuple(world_pos_vals) + world_orient = tuple(world_orient_vals) + elif input_type == "numpy": + world_pos = np.array(world_pos_vals) + world_orient = np.array(world_orient_vals) + elif input_type == "torch_cpu": + world_pos = torch.tensor(world_pos_vals) + world_orient = torch.tensor(world_orient_vals) + elif input_type == "torch_cuda": + if not torch.cuda.is_available(): + pytest.skip("CUDA not available") + world_pos = torch.tensor(world_pos_vals, device="cuda") + world_orient = torch.tensor(world_orient_vals, device="cuda") + + # Create child prim with world position + child = sim_utils.create_prim( + f"/World/Parent/Child_{input_type}", + "Xform", + stage=stage, + position=world_pos, # Using position (world space) + orientation=world_orient, + ) + + # Verify prim was created + assert child.IsValid() + + # Verify world pose matches what we specified + world_pose = sim_utils.resolve_prim_pose(child) + pos_result, quat_result = world_pose + + # Check position (should be close to world_pos_vals) + for i in range(3): + assert math.isclose(pos_result[i], world_pos_vals[i], abs_tol=1e-4) + + # Check orientation (quaternions may have sign flipped) + quat_match = all(math.isclose(quat_result[i], world_orient_vals[i], abs_tol=1e-4) for i in range(4)) + quat_match_neg = all(math.isclose(quat_result[i], -world_orient_vals[i], abs_tol=1e-4) for i in range(4)) + assert quat_match or quat_match_neg + + +def test_create_prim_non_xformable(): + """Test create_prim() with non-Xformable prim types (Material, Shader, Scope). + + This test verifies that prims which are not Xformable (like Material, Shader, Scope) + are created successfully but transform operations are not applied to them. + This is expected behavior as documented in the create_prim function. + """ + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Test with Material prim (not Xformable) + material_prim = sim_utils.create_prim( + "/World/TestMaterial", + "Material", + stage=stage, + translation=(1.0, 2.0, 3.0), # These should be ignored + orientation=(1.0, 0.0, 0.0, 0.0), # These should be ignored + scale=(2.0, 2.0, 2.0), # These should be ignored + ) + + # Verify prim was created + assert material_prim.IsValid() + assert material_prim.GetPrimPath() == "/World/TestMaterial" + assert material_prim.GetTypeName() == "Material" + + # Verify that it's not Xformable + assert not material_prim.IsA(UsdGeom.Xformable) + + # Verify that no xform operations were applied (Material prims don't support these) + assert not material_prim.HasAttribute("xformOp:translate") + assert not material_prim.HasAttribute("xformOp:orient") + assert not material_prim.HasAttribute("xformOp:scale") + + # Test with Scope prim (not Xformable) + scope_prim = sim_utils.create_prim( + "/World/TestScope", + "Scope", + stage=stage, + translation=(5.0, 6.0, 7.0), # These should be ignored + ) + + # Verify prim was created + assert scope_prim.IsValid() + assert scope_prim.GetPrimPath() == "/World/TestScope" + assert scope_prim.GetTypeName() == "Scope" + + # Verify that it's not Xformable + assert not scope_prim.IsA(UsdGeom.Xformable) + + # Verify that no xform operations were applied (Scope prims don't support these) + assert not scope_prim.HasAttribute("xformOp:translate") + assert not scope_prim.HasAttribute("xformOp:orient") + assert not scope_prim.HasAttribute("xformOp:scale") + + +def test_delete_prim(): + """Test delete_prim() function.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create scene + prim = sim_utils.create_prim("/World/Test/Xform", "Xform", stage=stage) + # delete prim + sim_utils.delete_prim("/World/Test/Xform") + # check prim deleted + assert not prim.IsValid() + + # check for usd reference + prim = sim_utils.create_prim( + "/World/Test/USDReference", + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd", + stage=stage, + ) + # delete prim + sim_utils.delete_prim("/World/Test/USDReference", stage=stage) + # check prim deleted + assert not prim.IsValid() + + # check deleting multiple prims + prim1 = sim_utils.create_prim("/World/Test/Xform1", "Xform", stage=stage) + prim2 = sim_utils.create_prim("/World/Test/Xform2", "Xform", stage=stage) + sim_utils.delete_prim(("/World/Test/Xform1", "/World/Test/Xform2"), stage=stage) + # check prims deleted + assert not prim1.IsValid() + assert not prim2.IsValid() + + +def test_move_prim(): + """Test move_prim() function.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create scene + sim_utils.create_prim("/World/Test", "Xform", stage=stage) + prim = sim_utils.create_prim( + "/World/Test/Xform", + "Xform", + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd", + translation=(1.0, 2.0, 3.0), + orientation=(0.0, 0.0, 0.0, 1.0), + stage=stage, + ) + + # move prim + sim_utils.create_prim("/World/TestMove", "Xform", stage=stage, translation=(1.0, 1.0, 1.0)) + sim_utils.move_prim("/World/Test/Xform", "/World/TestMove/Xform", stage=stage) + # check prim moved + prim = stage.GetPrimAtPath("/World/TestMove/Xform") + assert prim.IsValid() + assert prim.GetPrimPath() == "/World/TestMove/Xform" + assert prim.GetAttribute("xformOp:translate").Get() == Gf.Vec3d((0.0, 1.0, 2.0)) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), Gf.Quatd(0.0, 0.0, 0.0, 1.0)) + + # check moving prim with keep_world_transform=False + # it should preserve the local transform from last move + sim_utils.create_prim( + "/World/TestMove2", "Xform", stage=stage, translation=(2.0, 2.0, 2.0), orientation=(0.0, 0.7071, 0.0, 0.7071) + ) + sim_utils.move_prim("/World/TestMove/Xform", "/World/TestMove2/Xform", keep_world_transform=False, stage=stage) + # check prim moved + prim = stage.GetPrimAtPath("/World/TestMove2/Xform") + assert prim.IsValid() + assert prim.GetPrimPath() == "/World/TestMove2/Xform" + assert prim.GetAttribute("xformOp:translate").Get() == Gf.Vec3d((0.0, 1.0, 2.0)) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), Gf.Quatd(0.0, 0.0, 0.0, 1.0)) + + +""" +USD references and variants. +""" + + +def test_get_usd_references(): + """Test get_usd_references() function.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim without USD reference + sim_utils.create_prim("/World/NoReference", "Xform", stage=stage) + # Check that it has no references + refs = sim_utils.get_usd_references("/World/NoReference", stage=stage) + assert len(refs) == 0 + + # Create a prim with a USD reference + franka_usd = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + sim_utils.create_prim("/World/WithReference", usd_path=franka_usd, stage=stage) + # Check that it has the expected reference + refs = sim_utils.get_usd_references("/World/WithReference", stage=stage) + assert len(refs) == 1 + assert refs == [franka_usd] + + # Test with invalid prim path + with pytest.raises(ValueError, match="not valid"): + sim_utils.get_usd_references("/World/NonExistent", stage=stage) + + +def test_select_usd_variants(): + """Test select_usd_variants() function.""" + stage = sim_utils.get_current_stage() + + # Create a dummy prim + prim: Usd.Prim = UsdGeom.Xform.Define(stage, Sdf.Path("/World")).GetPrim() + stage.SetDefaultPrim(prim) + + # Create the variant set and add your variants to it. + variants = ["red", "blue", "green"] + variant_set = prim.GetVariantSets().AddVariantSet("colors") + for variant in variants: + variant_set.AddVariant(variant) + + # Set the variant selection + sim_utils.utils.select_usd_variants("/World", {"colors": "red"}, stage) + + # Check if the variant selection is correct + assert variant_set.GetVariantSelection() == "red" + + +def test_select_usd_variants_in_usd_file(): + """Test select_usd_variants() function in USD file.""" + stage = sim_utils.get_current_stage() + + prim = sim_utils.create_prim( + "/World/Test", "Xform", usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/UniversalRobots/ur10e/ur10e.usd", stage=stage + ) + + variant_sets = prim.GetVariantSets() + + # show all variants + for name in variant_sets.GetNames(): + vs = variant_sets.GetVariantSet(name) + options = vs.GetVariantNames() + selected = vs.GetVariantSelection() + + print(f"{name}: {selected} / {options}") + + print("Setting variant 'Gripper' to 'Robotiq_2f_140'.") + # The following performs the operations done internally + # in Isaac Lab. This should be removed in favor of 'select_usd_variants'. + target_vs = variant_sets.GetVariantSet("Gripper") + target_vs.SetVariantSelection("Robotiq_2f_140") + + # show again all variants + variant_sets = prim.GetVariantSets() + + for name in variant_sets.GetNames(): + vs = variant_sets.GetVariantSet(name) + options = vs.GetVariantNames() + selected = vs.GetVariantSelection() + + print(f"{name}: {selected} / {options}") + + # Uncomment the following once resolved + + # Set the variant selection + # sim_utils.select_usd_variants(prim.GetPath(), {"Gripper": "Robotiq_2f_140"}, stage) + + # Obtain variant set + # variant_set = prim.GetVariantSet("Gripper") + # # Check if the variant selection is correct + # assert variant_set.GetVariantSelection() == "Robotiq_2f_140" + + +""" +Property Management. +""" + + +def test_change_prim_property_basic(): + """Test change_prim_property() with existing property.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create a cube prim + prim = sim_utils.create_prim("/World/Cube", "Cube", stage=stage, attributes={"size": 1.0}) + + # check initial value + assert prim.GetAttribute("size").Get() == 1.0 + + # change the property + result = sim_utils.change_prim_property( + prop_path="/World/Cube.size", + value=2.0, + stage=stage, + ) + + # check that the change was successful + assert result is True + assert prim.GetAttribute("size").Get() == 2.0 + + +def test_change_prim_property_create_new(): + """Test change_prim_property() creates new property when it doesn't exist.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create a prim + prim = sim_utils.create_prim("/World/Test", "Xform", stage=stage) + + # check that the property doesn't exist + assert prim.GetAttribute("customValue").Get() is None + + # create a new property + result = sim_utils.change_prim_property( + prop_path="/World/Test.customValue", + value=42, + stage=stage, + type_to_create_if_not_exist=Sdf.ValueTypeNames.Int, + is_custom=True, + ) + + # check that the property was created successfully + assert result is True + assert prim.GetAttribute("customValue").Get() == 42 + + +def test_change_prim_property_clear_value(): + """Test change_prim_property() clears property value when value is None.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create a cube with an attribute + prim = sim_utils.create_prim("/World/Cube", "Cube", stage=stage, attributes={"size": 1.0}) + + # check initial value + assert prim.GetAttribute("size").Get() == 1.0 + + # clear the property value + result = sim_utils.change_prim_property( + prop_path="/World/Cube.size", + value=None, + stage=stage, + ) + + # check that the value was cleared + assert result is True + # Note: After clearing, the attribute should go its default value + assert prim.GetAttribute("size").Get() == 2.0 + + +@pytest.mark.parametrize( + "attr_name,value,value_type,expected", + [ + ("floatValue", 3.14, Sdf.ValueTypeNames.Float, 3.14), + ("boolValue", True, Sdf.ValueTypeNames.Bool, True), + ("intValue", 42, Sdf.ValueTypeNames.Int, 42), + ("stringValue", "test", Sdf.ValueTypeNames.String, "test"), + ("vec3Value", Gf.Vec3f(1.0, 2.0, 3.0), Sdf.ValueTypeNames.Float3, Gf.Vec3f(1.0, 2.0, 3.0)), + ("colorValue", Gf.Vec3f(1.0, 0.0, 0.5), Sdf.ValueTypeNames.Color3f, Gf.Vec3f(1.0, 0.0, 0.5)), + ], + ids=["float", "bool", "int", "string", "vec3", "color"], +) +def test_change_prim_property_different_types(attr_name: str, value, value_type, expected): + """Test change_prim_property() with different value types.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create a prim + prim = sim_utils.create_prim("/World/Test", "Xform", stage=stage) + + # change the property + result = sim_utils.change_prim_property( + prop_path=f"/World/Test.{attr_name}", + value=value, + stage=stage, + type_to_create_if_not_exist=value_type, + is_custom=True, + ) + + # check that the change was successful + assert result is True + actual_value = prim.GetAttribute(attr_name).Get() + + # handle float comparison separately for precision + if isinstance(expected, float): + assert math.isclose(actual_value, expected, abs_tol=1e-6) + else: + assert actual_value == expected + + +@pytest.mark.parametrize( + "prop_path_input", + ["/World/Cube.size", Sdf.Path("/World/Cube.size")], + ids=["str_path", "sdf_path"], +) +def test_change_prim_property_path_types(prop_path_input): + """Test change_prim_property() with different path input types.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create a cube prim + prim = sim_utils.create_prim("/World/Cube", "Cube", stage=stage, attributes={"size": 1.0}) + + # change property using different path types + result = sim_utils.change_prim_property( + prop_path=prop_path_input, + value=3.0, + stage=stage, + ) + + # check that the change was successful + assert result is True + assert prim.GetAttribute("size").Get() == 3.0 + + +def test_change_prim_property_error_invalid_prim(): + """Test change_prim_property() raises error for invalid prim path.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # try to change property on non-existent prim + with pytest.raises(ValueError, match="Prim does not exist"): + sim_utils.change_prim_property( + prop_path="/World/NonExistent.property", + value=1.0, + stage=stage, + ) + + +def test_change_prim_property_error_missing_type(): + """Test change_prim_property() returns False when property doesn't exist and type not provided.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + # create a prim + prim = sim_utils.create_prim("/World/Test", "Xform", stage=stage) + + # try to create property without providing type + result = sim_utils.change_prim_property( + prop_path="/World/Test.nonExistentProperty", + value=42, + stage=stage, + ) + + # should return False since type was not provided + assert result is False + # property should not have been created + assert prim.GetAttribute("nonExistentProperty").Get() is None + + +""" +Internal Helpers. +""" + + +def test_to_tuple_basic(): + """Test _to_tuple() with basic input types.""" + # Test with list + result = _to_tuple([1.0, 2.0, 3.0]) + assert result == (1.0, 2.0, 3.0) + assert isinstance(result, tuple) + + # Test with tuple + result = _to_tuple((1.0, 2.0, 3.0)) + assert result == (1.0, 2.0, 3.0) + + # Test with numpy array + result = _to_tuple(np.array([1.0, 2.0, 3.0])) + assert result == (1.0, 2.0, 3.0) + + # Test with torch tensor (CPU) + result = _to_tuple(torch.tensor([1.0, 2.0, 3.0])) + assert result == (1.0, 2.0, 3.0) + + # Test squeezing first dimension (batch size 1) + result = _to_tuple(torch.tensor([[1.0, 2.0]])) + assert result == (1.0, 2.0) + + result = _to_tuple(np.array([[1.0, 2.0, 3.0]])) + assert result == (1.0, 2.0, 3.0) + + +def test_to_tuple_raises_error(): + """Test _to_tuple() raises an error for N-dimensional arrays.""" + + with pytest.raises(ValueError, match="not one dimensional"): + _to_tuple(np.array([[1.0, 2.0], [3.0, 4.0]])) + + with pytest.raises(ValueError, match="not one dimensional"): + _to_tuple(torch.tensor([[[1.0, 2.0]], [[3.0, 4.0]]])) + + with pytest.raises(ValueError, match="only one element tensors can be converted"): + _to_tuple((torch.tensor([1.0, 2.0]), 3.0)) + + +def test_to_tuple_mixed_sequences(): + """Test _to_tuple() with mixed type sequences.""" + + # Mixed list with numpy and floats + result = _to_tuple([np.float32(1.0), 2.0, 3.0]) + assert len(result) == 3 + assert all(isinstance(x, float) for x in result) + + # Mixed tuple with torch tensor items and floats + result = _to_tuple([torch.tensor(1.0), 2.0, 3.0]) + assert result == (1.0, 2.0, 3.0) + + # Mixed tuple with numpy array items and torch tensor + result = _to_tuple((np.float32(1.0), 2.0, torch.tensor(3.0))) + assert result == (1.0, 2.0, 3.0) + + +def test_to_tuple_precision(): + """Test _to_tuple() maintains numerical precision.""" + from isaaclab.sim.utils.prims import _to_tuple + + # Test with high precision values + high_precision = [1.123456789, 2.987654321, 3.141592653] + result = _to_tuple(torch.tensor(high_precision, dtype=torch.float64)) + + # Check that precision is maintained reasonably well + for i, val in enumerate(high_precision): + assert math.isclose(result[i], val, abs_tol=1e-6) diff --git a/source/isaaclab/test/sim/test_utils_queries.py b/source/isaaclab/test/sim/test_utils_queries.py new file mode 100644 index 000000000000..4f5a0758342c --- /dev/null +++ b/source/isaaclab/test/sim/test_utils_queries.py @@ -0,0 +1,171 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +# note: need to enable cameras to be able to make replicator core available +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +"""Rest everything follows.""" + +import pytest + +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + sim_utils.update_stage() + + # Yield for the test + yield + + # Teardown: Clear stage after each test + sim_utils.clear_stage() + + +""" +USD Stage Querying. +""" + + +def test_get_next_free_prim_path(): + """Test get_next_free_prim_path() function.""" + # create scene + sim_utils.create_prim("/World/Floor") + sim_utils.create_prim("/World/Floor/Box", "Cube", position=[75, 75, -150.1], attributes={"size": 300}) + sim_utils.create_prim("/World/Wall", "Sphere", attributes={"radius": 1e3}) + + # test + isaaclab_result = sim_utils.get_next_free_prim_path("/World/Floor") + assert isaaclab_result == "/World/Floor_01" + + # create another prim + sim_utils.create_prim("/World/Floor/Box_01", "Cube", position=[75, 75, -150.1], attributes={"size": 300}) + + # test again + isaaclab_result = sim_utils.get_next_free_prim_path("/World/Floor/Box") + assert isaaclab_result == "/World/Floor/Box_02" + + +def test_get_first_matching_ancestor_prim(): + """Test get_first_matching_ancestor_prim() function.""" + # create scene + sim_utils.create_prim("/World/Floor") + sim_utils.create_prim("/World/Floor/Box", "Cube", position=[75, 75, -150.1], attributes={"size": 300}) + sim_utils.create_prim("/World/Floor/Box/Sphere", "Sphere", attributes={"radius": 1e3}) + + # test with input prim not having the predicate + isaaclab_result = sim_utils.get_first_matching_ancestor_prim( + "/World/Floor/Box/Sphere", predicate=lambda x: x.GetTypeName() == "Cube" + ) + assert isaaclab_result is not None + assert isaaclab_result.GetPrimPath() == "/World/Floor/Box" + + # test with input prim having the predicate + isaaclab_result = sim_utils.get_first_matching_ancestor_prim( + "/World/Floor/Box", predicate=lambda x: x.GetTypeName() == "Cube" + ) + assert isaaclab_result is not None + assert isaaclab_result.GetPrimPath() == "/World/Floor/Box" + + # test with no predicate match + isaaclab_result = sim_utils.get_first_matching_ancestor_prim( + "/World/Floor/Box/Sphere", predicate=lambda x: x.GetTypeName() == "Cone" + ) + assert isaaclab_result is None + + +def test_get_all_matching_child_prims(): + """Test get_all_matching_child_prims() function.""" + # create scene + sim_utils.create_prim("/World/Floor") + sim_utils.create_prim("/World/Floor/Box", "Cube", position=[75, 75, -150.1], attributes={"size": 300}) + sim_utils.create_prim("/World/Wall", "Sphere", attributes={"radius": 1e3}) + + # add articulation root prim -- this asset has instanced prims + # note: isaac sim function does not support instanced prims so we add it here + # after the above test for the above test to still pass. + sim_utils.create_prim( + "/World/Franka", "Xform", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + ) + + # test with predicate + isaaclab_result = sim_utils.get_all_matching_child_prims("/World", predicate=lambda x: x.GetTypeName() == "Cube") + assert len(isaaclab_result) == 1 + assert isaaclab_result[0].GetPrimPath() == "/World/Floor/Box" + + # test with predicate and instanced prims + isaaclab_result = sim_utils.get_all_matching_child_prims( + "/World/Franka/panda_hand/visuals", predicate=lambda x: x.GetTypeName() == "Mesh" + ) + assert len(isaaclab_result) == 1 + assert isaaclab_result[0].GetPrimPath() == "/World/Franka/panda_hand/visuals/panda_hand" + + # test valid path + with pytest.raises(ValueError): + sim_utils.get_all_matching_child_prims("World/Room") + + +def test_get_first_matching_child_prim(): + """Test get_first_matching_child_prim() function.""" + # create scene + sim_utils.create_prim("/World/Floor") + sim_utils.create_prim( + "/World/env_1/Franka", "Xform", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + ) + sim_utils.create_prim( + "/World/env_2/Franka", "Xform", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + ) + sim_utils.create_prim( + "/World/env_0/Franka", "Xform", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + ) + + # test + isaaclab_result = sim_utils.get_first_matching_child_prim( + "/World", predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI) + ) + assert isaaclab_result is not None + assert isaaclab_result.GetPrimPath() == "/World/env_1/Franka" + + # test with instanced prims + isaaclab_result = sim_utils.get_first_matching_child_prim( + "/World/env_1/Franka", predicate=lambda prim: prim.GetTypeName() == "Mesh" + ) + assert isaaclab_result is not None + assert isaaclab_result.GetPrimPath() == "/World/env_1/Franka/panda_link0/visuals/panda_link0" + + +def test_find_global_fixed_joint_prim(): + """Test find_global_fixed_joint_prim() function.""" + # create scene + sim_utils.create_prim("/World") + sim_utils.create_prim("/World/ANYmal", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd") + sim_utils.create_prim("/World/Franka", usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd") + if "4.5" in ISAAC_NUCLEUS_DIR: + franka_usd = f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka.usd" + else: + franka_usd = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" + sim_utils.create_prim("/World/Franka_Isaac", usd_path=franka_usd) + + # test + assert sim_utils.find_global_fixed_joint_prim("/World/ANYmal") is None + assert sim_utils.find_global_fixed_joint_prim("/World/Franka") is not None + assert sim_utils.find_global_fixed_joint_prim("/World/Franka_Isaac") is not None + + # make fixed joint disabled manually + joint_prim = sim_utils.find_global_fixed_joint_prim("/World/Franka") + joint_prim.GetJointEnabledAttr().Set(False) + assert sim_utils.find_global_fixed_joint_prim("/World/Franka") is not None + assert sim_utils.find_global_fixed_joint_prim("/World/Franka", check_enabled_only=True) is None diff --git a/source/isaaclab/test/sim/test_utils_semantics.py b/source/isaaclab/test/sim/test_utils_semantics.py new file mode 100644 index 000000000000..fe8cbd37187a --- /dev/null +++ b/source/isaaclab/test/sim/test_utils_semantics.py @@ -0,0 +1,231 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +# note: need to enable cameras to be able to make replicator core available +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +"""Rest everything follows.""" + +import pytest + +import isaaclab.sim as sim_utils + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + sim_utils.update_stage() + + # Yield for the test + yield + + # Teardown: Clear stage after each test + sim_utils.clear_stage() + + +def create_test_environment_with_labels(): + """Creates a test environment with objects with labels.""" + # create 3 cubes with label "cube" + for i in range(3): + sim_utils.create_prim(f"/World/Test/Object{i}", "Cube", semantic_label="cube") + # create a sphere without any labels + sim_utils.create_prim("/World/Test/Object3", "Sphere") + # create a nested prim with label "nested" + nested_prim = sim_utils.create_prim("/World/Test/Object0/Nested", "Cube") + sim_utils.add_labels(nested_prim, ["nested"], instance_name="shape") + + return [f"/World/Test/Object{i}" for i in range(4)] + [str(nested_prim.GetPrimPath())] + + +""" +Tests. +""" + + +def test_add_and_get_labels(): + """Test add_labels() and get_labels() functions.""" + # get stage handle + stage = sim_utils.get_current_stage() + # create a test prim + prim = stage.DefinePrim("/test", "Xform") + nested_prim = stage.DefinePrim("/test/nested", "Xform") + + # Apply semantics + sim_utils.add_labels(prim, ["label_a", "label_b"], instance_name="class") + sim_utils.add_labels(prim, ["shape_a"], instance_name="shape") + sim_utils.add_labels(nested_prim, ["nested_label"], instance_name="class") + + # Get labels + labels_dict = sim_utils.get_labels(prim) + # Check labels are added correctly + assert "class" in labels_dict + assert sorted(labels_dict["class"]) == sorted(["label_a", "label_b"]) + assert "shape" in labels_dict + assert labels_dict["shape"] == ["shape_a"] + nested_labels_dict = sim_utils.get_labels(nested_prim) + assert "class" in nested_labels_dict + assert nested_labels_dict["class"] == ["nested_label"] + + +def test_add_labels_with_overwrite(): + """Test add_labels() function with overwriting existing labels.""" + # get stage handle + stage = sim_utils.get_current_stage() + # create a test prim + prim = stage.DefinePrim("/test", "Xform") + + # Add labels + sim_utils.add_labels(prim, ["label_a", "label_b"], instance_name="class") + sim_utils.add_labels(prim, ["shape_a"], instance_name="shape") + + # Overwrite existing labels for a specific instance + sim_utils.add_labels(prim, ["replaced_label"], instance_name="class", overwrite=True) + labels_dict = sim_utils.get_labels(prim) + assert labels_dict["class"] == ["replaced_label"] + assert "shape" in labels_dict + assert labels_dict["shape"] == ["shape_a"] + + +def test_add_labels_without_overwrite(): + """Test add_labels() function without overwriting existing labels.""" + # get stage handle + stage = sim_utils.get_current_stage() + # create a test prim + prim = stage.DefinePrim("/test", "Xform") + + # Add labels + sim_utils.add_labels(prim, ["label_a", "label_b"], instance_name="class") + sim_utils.add_labels(prim, ["shape_a"], instance_name="shape") + + # Re-add labels with overwrite=False (should append) + sim_utils.add_labels(prim, ["label_c"], instance_name="class", overwrite=False) + labels_dict = sim_utils.get_labels(prim) + assert sorted(labels_dict["class"]) == sorted(["label_a", "label_b", "label_c"]) + + +def test_remove_all_labels(): + """Test removing of all labels from a prim and its descendants.""" + # get stage handle + stage = sim_utils.get_current_stage() + # create a test prim + prim = stage.DefinePrim("/test", "Xform") + nested_prim = stage.DefinePrim("/test/nested", "Xform") + + # Add labels + sim_utils.add_labels(prim, ["label_a", "label_b"], instance_name="class") + sim_utils.add_labels(prim, ["shape_a"], instance_name="shape") + sim_utils.add_labels(nested_prim, ["nested_label"], instance_name="class") + + # Remove all labels + sim_utils.remove_labels(prim) + # Check labels are removed correctly + labels_dict = sim_utils.get_labels(prim) + assert len(labels_dict) == 0 + # Check nested prim labels are not removed + nested_labels_dict = sim_utils.get_labels(nested_prim) + assert "class" in nested_labels_dict + assert nested_labels_dict["class"] == ["nested_label"] + + # Re-add labels + sim_utils.add_labels(prim, ["label_a", "label_b"], instance_name="class") + sim_utils.add_labels(prim, ["shape_a"], instance_name="shape") + sim_utils.add_labels(nested_prim, ["nested_label"], instance_name="class") + # Remove all labels + sim_utils.remove_labels(prim, include_descendants=True) + # Check labels are removed correctly + labels_dict = sim_utils.get_labels(prim) + assert len(labels_dict) == 0 + # Check nested prim labels are removed + nested_labels_dict = sim_utils.get_labels(nested_prim) + assert len(nested_labels_dict) == 0 + + +def test_remove_specific_labels(): + """Test removing of specific labels from a prim and its descendants.""" + # get stage handle + stage = sim_utils.get_current_stage() + # create a test prim + prim = stage.DefinePrim("/test", "Xform") + nested_prim = stage.DefinePrim("/test/nested", "Xform") + + # Add labels + sim_utils.add_labels(prim, ["label_a", "label_b"], instance_name="class") + sim_utils.add_labels(prim, ["shape_a"], instance_name="shape") + sim_utils.add_labels(nested_prim, ["nested_label"], instance_name="class") + sim_utils.add_labels(nested_prim, ["nested_shape"], instance_name="shape") + + # Remove specific labels + sim_utils.remove_labels(prim, instance_name="shape") + # Check labels are removed correctly + labels_dict = sim_utils.get_labels(prim) + assert "shape" not in labels_dict + assert "class" in labels_dict + assert sorted(labels_dict["class"]) == sorted(["label_a", "label_b"]) + # Check nested prim labels are not removed + nested_labels_dict = sim_utils.get_labels(nested_prim) + assert "class" in nested_labels_dict + assert nested_labels_dict["class"] == ["nested_label"] + + # Remove specific labels + sim_utils.remove_labels(prim, instance_name="class", include_descendants=True) + # Check labels are removed correctly + labels_dict = sim_utils.get_labels(prim) + assert len(labels_dict) == 0 + # Check nested prim labels are removed + nested_labels_dict = sim_utils.get_labels(nested_prim) + assert "shape" in nested_labels_dict + assert nested_labels_dict["shape"] == ["nested_shape"] + + +def test_check_missing_labels(): + """Test the check_missing_labels() function.""" + # create a test environment with labels + object_paths = create_test_environment_with_labels() + + # Check from root + missing_paths = sim_utils.check_missing_labels() + + # Only the sphere should be missing + assert len(missing_paths) == 1 + assert object_paths[3] in missing_paths # Object3 should be missing + + # Check from specific subtree + missing_paths_subtree = sim_utils.check_missing_labels(prim_path="/World/Test/Object0") + # Object0 and Nested both have labels + assert len(missing_paths_subtree) == 0 + + # Check from invalid path + missing_paths_invalid = sim_utils.check_missing_labels(prim_path="/World/Test/Invalid") + assert len(missing_paths_invalid) == 0 + + +def test_count_labels_in_scene(): + """Test the count_labels_in_scene() function.""" + # create a test environment with labels + create_test_environment_with_labels() + + # Count from root + labels_dict = sim_utils.count_total_labels() + # Object0 and Nested both have labels + assert labels_dict.get("cube", 0) == 3 + assert labels_dict.get("nested", 0) == 1 + assert labels_dict.get("missing_labels", 0) == 1 + + # Count from specific subtree + labels_dict_subtree = sim_utils.count_total_labels(prim_path="/World/Test/Object0") + assert labels_dict_subtree.get("cube", 0) == 1 + assert labels_dict_subtree.get("nested", 0) == 1 + assert labels_dict_subtree.get("missing_labels", 0) == 0 + + # Count from invalid path + labels_dict_invalid = sim_utils.count_total_labels(prim_path="/World/Test/Invalid") + assert labels_dict_invalid.get("missing_labels", 0) == 0 diff --git a/source/isaaclab/test/sim/test_utils_stage.py b/source/isaaclab/test/sim/test_utils_stage.py new file mode 100644 index 000000000000..033a461e1a1f --- /dev/null +++ b/source/isaaclab/test/sim/test_utils_stage.py @@ -0,0 +1,289 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for stage utilities.""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import tempfile +from pathlib import Path + +import pytest + +from pxr import Usd + +import isaaclab.sim as sim_utils + + +def test_create_new_stage(): + """Test creating a new stage attached to USD context.""" + stage = sim_utils.create_new_stage() + + # Should return a valid stage + assert stage is not None + assert isinstance(stage, Usd.Stage) + + # Stage should be the current stage + current_stage = sim_utils.get_current_stage() + assert stage == current_stage + + # Stage should have a root prim + root_prim = stage.GetPseudoRoot() + assert root_prim.IsValid() + + +def test_create_multiple_stages(): + """Test creating multiple stages.""" + stage1 = sim_utils.create_new_stage() + stage2 = sim_utils.create_new_stage() + stage3 = sim_utils.create_new_stage() + + assert stage1 is not None + assert stage2 is not None + assert stage3 is not None + assert stage1 != stage2 + assert stage1 != stage3 + assert stage2 != stage3 + + +def test_create_new_stage_in_memory(): + """Test creating a new stage in memory (Isaac Sim 5.0+).""" + stage = sim_utils.create_new_stage_in_memory() + + # Should return a valid stage + assert stage is not None + assert isinstance(stage, Usd.Stage) + + # Stage should have a root prim + root_prim = stage.GetPseudoRoot() + assert root_prim.IsValid() + + +def test_is_current_stage_in_memory(): + """Test checking if current stage is in memory.""" + # Create a regular stage (attached to context) + sim_utils.create_new_stage() + is_in_memory = sim_utils.is_current_stage_in_memory() + + # Should return a boolean + assert isinstance(is_in_memory, bool) + assert is_in_memory is False + + # Create a stage in memory + stage = sim_utils.create_new_stage_in_memory() + with sim_utils.use_stage(stage): + is_in_memory = sim_utils.is_current_stage_in_memory() + assert isinstance(is_in_memory, bool) + assert is_in_memory is True + + +def test_save_and_open_stage(): + """Test saving and opening a stage.""" + with tempfile.TemporaryDirectory() as temp_dir: + # Create a stage with some content + stage = sim_utils.create_new_stage() + stage.DefinePrim("/World", "Xform") + stage.DefinePrim("/World/TestCube", "Cube") + + # Save the stage + save_path = Path(temp_dir) / "test_stage.usd" + result = sim_utils.save_stage(str(save_path), save_and_reload_in_place=False) + + # Save should succeed + assert result is True + assert save_path.exists() + + # Open the saved stage + open_result = sim_utils.open_stage(str(save_path)) + assert open_result is True + + # Verify content was preserved + opened_stage = sim_utils.get_current_stage() + test_cube = opened_stage.GetPrimAtPath("/World/TestCube") + assert test_cube.IsValid() + assert test_cube.GetTypeName() == "Cube" + + +def test_open_stage_invalid_path(): + """Test opening a stage with invalid path.""" + with pytest.raises(ValueError, match="not supported"): + sim_utils.open_stage("/invalid/path/to/stage.invalid") + + +def test_use_stage_context_manager(): + """Test use_stage context manager.""" + # Create two stages + stage1 = sim_utils.create_new_stage() + stage1.DefinePrim("/World", "Xform") + stage1.DefinePrim("/World/Stage1Marker", "Xform") + + stage2 = Usd.Stage.CreateInMemory() + stage2.DefinePrim("/World", "Xform") + stage2.DefinePrim("/World/Stage2Marker", "Xform") + + # Initially on stage1 + current = sim_utils.get_current_stage() + marker1 = current.GetPrimAtPath("/World/Stage1Marker") + assert marker1.IsValid() + + # Switch to stage2 temporarily + with sim_utils.use_stage(stage2): + temp_current = sim_utils.get_current_stage() + # Should be on stage2 now + marker2 = temp_current.GetPrimAtPath("/World/Stage2Marker") + assert marker2.IsValid() + + # Should be back on stage1 + final_current = sim_utils.get_current_stage() + marker1_again = final_current.GetPrimAtPath("/World/Stage1Marker") + assert marker1_again.IsValid() + + +def test_use_stage_with_invalid_input(): + """Test use_stage with invalid input.""" + with pytest.raises((TypeError, AssertionError)): + with sim_utils.use_stage("not a stage"): # type: ignore + pass + + +def test_update_stage(): + """Test updating the stage.""" + # Create a new stage + stage = sim_utils.create_new_stage() + + # Add a prim + prim_path = "/World/Test" + stage.DefinePrim(prim_path, "Xform") + + # Update stage should not raise errors + sim_utils.update_stage() + + # Prim should still exist + prim = stage.GetPrimAtPath(prim_path) + assert prim.IsValid() + + +def test_save_stage_with_reload(): + """Test saving stage with reload in place.""" + with tempfile.TemporaryDirectory() as temp_dir: + # Create a stage with content + stage = sim_utils.create_new_stage() + stage.DefinePrim("/World", "Xform") + stage.DefinePrim("/World/TestSphere", "Sphere") + + # Save with reload + save_path = Path(temp_dir) / "test_reload.usd" + result = sim_utils.save_stage(str(save_path), save_and_reload_in_place=True) + + assert result is True + assert save_path.exists() + + # Stage should be reloaded, content should be preserved + current_stage = sim_utils.get_current_stage() + test_sphere = current_stage.GetPrimAtPath("/World/TestSphere") + assert test_sphere.IsValid() + + +def test_save_stage_invalid_path(): + """Test saving stage with invalid path.""" + _ = sim_utils.create_new_stage() + + with pytest.raises(ValueError, match="not supported"): + sim_utils.save_stage("/tmp/test.invalid") + + +def test_close_stage(): + """Test closing the current stage.""" + # Create a stage + stage = sim_utils.create_new_stage() + assert stage is not None + + # Close it + result = sim_utils.close_stage() + + # Should succeed (or return bool) + assert isinstance(result, bool) + + +def test_close_stage_with_callback(): + """Test closing stage with a callback function.""" + # Create a stage + sim_utils.create_new_stage() + + # Track callback invocations + callback_called = [] + + def callback(success: bool, error_msg: str): + callback_called.append((success, error_msg)) + + # Close with callback + result = sim_utils.close_stage(callback_fn=callback) + + # Callback might be called or not depending on implementation + # Just verify no exceptions were raised + assert isinstance(result, bool) + + +def test_clear_stage(): + """Test clearing the stage.""" + # Create a new stage + stage = sim_utils.create_new_stage() + + # Add some prims + stage.DefinePrim("/World", "Xform") + stage.DefinePrim("/World/Cube", "Cube") + stage.DefinePrim("/World/Sphere", "Sphere") + + # Clear the stage + sim_utils.clear_stage() + + # Stage should still exist but prims should be removed + assert stage is not None + + +def test_is_stage_loading(): + """Test checking if stage is loading.""" + # Create a new stage + sim_utils.create_new_stage() + + # Check loading status + is_loading = sim_utils.is_stage_loading() + + # Should return a boolean + assert isinstance(is_loading, bool) + + # After creation, should not be loading + assert is_loading is False + + +def test_get_current_stage(): + """Test getting the current stage.""" + # Create a new stage + created_stage = sim_utils.create_new_stage() + + # Get current stage should return the same stage + current_stage = sim_utils.get_current_stage() + assert current_stage == created_stage + assert isinstance(current_stage, Usd.Stage) + + +def test_get_current_stage_id(): + """Test getting the current stage ID.""" + # Create a new stage + sim_utils.create_new_stage() + + # Get stage ID + stage_id = sim_utils.get_current_stage_id() + + # Should be a valid integer ID + assert isinstance(stage_id, int) + assert stage_id >= 0 diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py new file mode 100644 index 000000000000..040cfe333aa7 --- /dev/null +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -0,0 +1,1423 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import math + +import numpy as np +import pytest +import torch + +from pxr import Gf, Sdf, Usd, UsdGeom + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + sim_utils.update_stage() + + # Yield for the test + yield + + # Teardown: Clear stage after each test + sim_utils.clear_stage() + + +def assert_vec3_close(v1: Gf.Vec3d | Gf.Vec3f, v2: tuple | Gf.Vec3d | Gf.Vec3f, eps: float = 1e-6): + """Assert two 3D vectors are close.""" + if isinstance(v2, tuple): + v2 = Gf.Vec3d(*v2) + for i in range(3): + assert math.isclose(v1[i], v2[i], abs_tol=eps), f"Vector mismatch at index {i}: {v1[i]} != {v2[i]}" + + +def assert_quat_close(q1: Gf.Quatf | Gf.Quatd, q2: Gf.Quatf | Gf.Quatd | tuple, eps: float = 1e-6): + """Assert two quaternions are close, accounting for double-cover (q and -q represent same rotation).""" + if isinstance(q2, tuple): + q2 = Gf.Quatd(*q2) + # Check if quaternions are close (either q1 โ‰ˆ q2 or q1 โ‰ˆ -q2) + real_match = math.isclose(q1.GetReal(), q2.GetReal(), abs_tol=eps) + imag_match = all(math.isclose(q1.GetImaginary()[i], q2.GetImaginary()[i], abs_tol=eps) for i in range(3)) + + real_match_neg = math.isclose(q1.GetReal(), -q2.GetReal(), abs_tol=eps) + imag_match_neg = all(math.isclose(q1.GetImaginary()[i], -q2.GetImaginary()[i], abs_tol=eps) for i in range(3)) + + assert (real_match and imag_match) or (real_match_neg and imag_match_neg), ( + f"Quaternion mismatch: {q1} != {q2} (and not equal to negative either)" + ) + + +def get_xform_ops(prim: Usd.Prim) -> list[str]: + """Get the ordered list of xform operation names for a prim.""" + xformable = UsdGeom.Xformable(prim) + return [op.GetOpName() for op in xformable.GetOrderedXformOps()] + + +""" +Test standardize_xform_ops() function. +""" + + +def test_standardize_xform_ops_basic(): + """Test basic functionality of standardize_xform_ops on a simple prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a simple xform prim with standard operations + prim = sim_utils.create_prim( + "/World/TestXform", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), # w, x, y, z + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + + # Verify the operation succeeded + assert result is True + assert prim.IsValid() + + # Check that the xform operations are in the correct order + xform_ops = get_xform_ops(prim) + assert xform_ops == [ + "xformOp:translate", + "xformOp:orient", + "xformOp:scale", + ], f"Expected standard xform order, got {xform_ops}" + + # Verify the transform values are preserved (approximately) + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), (1.0, 2.0, 3.0)) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), (1.0, 0.0, 0.0, 0.0)) + assert_vec3_close(prim.GetAttribute("xformOp:scale").Get(), (1.0, 1.0, 1.0)) + + +def test_standardize_xform_ops_with_rotation_xyz(): + """Test standardize_xform_ops removes deprecated rotateXYZ operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim and manually add deprecated rotation operations + prim_path = "/World/TestRotateXYZ" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + # Add deprecated rotateXYZ operation + rotate_xyz_op = xformable.AddRotateXYZOp(UsdGeom.XformOp.PrecisionDouble) + rotate_xyz_op.Set(Gf.Vec3d(45.0, 30.0, 60.0)) + # Add translate operation + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Verify the deprecated operation exists + assert "xformOp:rotateXYZ" in prim.GetPropertyNames() + + # Get pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved (may have small numeric differences due to rotation conversion) + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-4) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-4) + + # Verify the deprecated operation is removed + assert "xformOp:rotateXYZ" not in prim.GetPropertyNames() + # Verify standard operations exist + assert "xformOp:translate" in prim.GetPropertyNames() + assert "xformOp:orient" in prim.GetPropertyNames() + assert "xformOp:scale" in prim.GetPropertyNames() + # Check the xform operation order + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_transform_matrix(): + """Test standardize_xform_ops removes transform matrix operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with a transform matrix + prim_path = "/World/TestTransformMatrix" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add transform matrix operation + transform_op = xformable.AddTransformOp(UsdGeom.XformOp.PrecisionDouble) + # Create a simple translation matrix + matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(5.0, 10.0, 15.0)) + transform_op.Set(matrix) + + # Verify the transform operation exists + assert "xformOp:transform" in prim.GetPropertyNames() + + # Get pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify the transform operation is removed + assert "xformOp:transform" not in prim.GetPropertyNames() + # Verify standard operations exist + assert "xformOp:translate" in prim.GetPropertyNames() + assert "xformOp:orient" in prim.GetPropertyNames() + assert "xformOp:scale" in prim.GetPropertyNames() + + +def test_standardize_xform_ops_preserves_world_pose(): + """Test that standardize_xform_ops preserves the world-space pose of the prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with specific world pose + translation = (10.0, 20.0, 30.0) + # Rotation of 90 degrees around Z axis + orientation = (0.7071068, 0.0, 0.0, 0.7071068) # w, x, y, z + scale = (2.0, 3.0, 4.0) + + prim = sim_utils.create_prim( + "/World/TestPreservePose", + "Xform", + translation=translation, + orientation=orientation, + scale=scale, + stage=stage, + ) + + # Get the world pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get the world pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify the world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + +def test_standardize_xform_ops_with_units_resolve(): + """Test standardize_xform_ops handles scale:unitsResolve attribute.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim + prim_path = "/World/TestUnitsResolve" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add scale operation + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble) + scale_op.Set(Gf.Vec3d(1.0, 1.0, 1.0)) + + # Manually add a unitsResolve scale attribute (simulating imported asset with different units) + units_resolve_attr = prim.CreateAttribute("xformOp:scale:unitsResolve", Sdf.ValueTypeNames.Double3) + units_resolve_attr.Set(Gf.Vec3d(100.0, 100.0, 100.0)) # e.g., cm to m conversion + + # Verify both attributes exist + assert "xformOp:scale" in prim.GetPropertyNames() + assert "xformOp:scale:unitsResolve" in prim.GetPropertyNames() + + # Get pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify unitsResolve is removed + assert "xformOp:scale:unitsResolve" not in prim.GetPropertyNames() + + # Verify scale is updated (1.0 * 100.0 = 100.0) + scale = prim.GetAttribute("xformOp:scale").Get() + assert_vec3_close(scale, (100.0, 100.0, 100.0)) + + +def test_standardize_xform_ops_with_hierarchy(): + """Test standardize_xform_ops works correctly with prim hierarchies.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent prim + parent_prim = sim_utils.create_prim( + "/World/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Create child prim + child_prim = sim_utils.create_prim( + "/World/Parent/Child", + "Xform", + translation=(0.0, 3.0, 0.0), + orientation=(0.7071068, 0.0, 0.7071068, 0.0), # 90 deg around Y + scale=(0.5, 0.5, 0.5), + stage=stage, + ) + + # Get world poses before standardization + parent_pos_before, parent_quat_before = sim_utils.resolve_prim_pose(parent_prim) + child_pos_before, child_quat_before = sim_utils.resolve_prim_pose(child_prim) + + # Apply standardize_xform_ops to both + sim_utils.standardize_xform_ops(parent_prim) + sim_utils.standardize_xform_ops(child_prim) + + # Get world poses after standardization + parent_pos_after, parent_quat_after = sim_utils.resolve_prim_pose(parent_prim) + child_pos_after, child_quat_after = sim_utils.resolve_prim_pose(child_prim) + + # Verify world poses are preserved + assert_vec3_close(Gf.Vec3d(*parent_pos_before), parent_pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*parent_quat_before), parent_quat_after, eps=1e-5) + assert_vec3_close(Gf.Vec3d(*child_pos_before), child_pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*child_quat_before), child_quat_after, eps=1e-5) + + +def test_standardize_xform_ops_multiple_deprecated_ops(): + """Test standardize_xform_ops removes multiple deprecated operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with multiple deprecated operations + prim_path = "/World/TestMultipleDeprecated" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add various deprecated rotation operations + rotate_x_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + rotate_x_op.Set(45.0) + rotate_y_op = xformable.AddRotateYOp(UsdGeom.XformOp.PrecisionDouble) + rotate_y_op.Set(30.0) + rotate_z_op = xformable.AddRotateZOp(UsdGeom.XformOp.PrecisionDouble) + rotate_z_op.Set(60.0) + + # Verify deprecated operations exist + assert "xformOp:rotateX" in prim.GetPropertyNames() + assert "xformOp:rotateY" in prim.GetPropertyNames() + assert "xformOp:rotateZ" in prim.GetPropertyNames() + + # Obtain current local transformations + pos, quat = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + sim_utils.standardize_xform_ops(prim) + + # Obtain current local transformations + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos), Gf.Vec3d(*pos_after), eps=1e-5) + assert_quat_close(Gf.Quatd(*quat), Gf.Quatd(*quat_after), eps=1e-5) + + # Verify all deprecated operations are removed + assert "xformOp:rotateX" not in prim.GetPropertyNames() + assert "xformOp:rotateY" not in prim.GetPropertyNames() + assert "xformOp:rotateZ" not in prim.GetPropertyNames() + # Verify standard operations exist + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_existing_standard_ops(): + """Test standardize_xform_ops when prim already has standard operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations already in place + prim = sim_utils.create_prim( + "/World/TestExistingStandard", + "Xform", + translation=(7.0, 8.0, 9.0), + orientation=(0.9238795, 0.3826834, 0.0, 0.0), # rotation around X + scale=(1.5, 2.5, 3.5), + stage=stage, + ) + + # Get initial values + initial_translate = prim.GetAttribute("xformOp:translate").Get() + initial_orient = prim.GetAttribute("xformOp:orient").Get() + initial_scale = prim.GetAttribute("xformOp:scale").Get() + + # Get world pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get world pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify operations still exist and are in correct order + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + # Verify values are approximately preserved + final_translate = prim.GetAttribute("xformOp:translate").Get() + final_orient = prim.GetAttribute("xformOp:orient").Get() + final_scale = prim.GetAttribute("xformOp:scale").Get() + + assert_vec3_close(initial_translate, final_translate, eps=1e-5) + assert_quat_close(initial_orient, final_orient, eps=1e-5) + assert_vec3_close(initial_scale, final_scale, eps=1e-5) + + +def test_standardize_xform_ops_invalid_prim(): + """Test standardize_xform_ops raises error for invalid prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim (non-existent path) + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + + # Verify the prim is invalid + assert not invalid_prim.IsValid() + + # Attempt to apply standardize_xform_ops and expect ValueError + with pytest.raises(ValueError, match="not valid"): + sim_utils.standardize_xform_ops(invalid_prim) + + +def test_standardize_xform_ops_on_geometry_prim(): + """Test standardize_xform_ops on a geometry prim (Cube, Sphere, etc.).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a cube with transform + cube_prim = sim_utils.create_prim( + "/World/TestCube", + "Cube", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + attributes={"size": 1.0}, + stage=stage, + ) + + # Get world pose before + pos_before, quat_before = sim_utils.resolve_prim_pose(cube_prim) + + # Apply standardize_xform_ops + sim_utils.standardize_xform_ops(cube_prim) + + # Get world pose after + pos_after, quat_after = sim_utils.resolve_prim_pose(cube_prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify standard operations exist + xform_ops = get_xform_ops(cube_prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_non_uniform_scale(): + """Test standardize_xform_ops with non-uniform scale.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with non-uniform scale + prim = sim_utils.create_prim( + "/World/TestNonUniformScale", + "Xform", + translation=(5.0, 10.0, 15.0), + orientation=(0.7071068, 0.0, 0.7071068, 0.0), # 90 deg around Y + scale=(1.0, 2.0, 3.0), # Non-uniform scale + stage=stage, + ) + + # Get initial scale + initial_scale = prim.GetAttribute("xformOp:scale").Get() + + # Get world pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get world pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + # Verify scale is preserved + final_scale = prim.GetAttribute("xformOp:scale").Get() + assert_vec3_close(initial_scale, final_scale, eps=1e-5) + + +def test_standardize_xform_ops_identity_transform(): + """Test standardize_xform_ops with identity transform (no translation, rotation, or scale).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with identity transform + prim = sim_utils.create_prim( + "/World/TestIdentity", + "Xform", + translation=(0.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), # Identity quaternion + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Apply standardize_xform_ops + sim_utils.standardize_xform_ops(prim) + + # Verify standard operations exist + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + # Verify identity values + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), (0.0, 0.0, 0.0)) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), (1.0, 0.0, 0.0, 0.0)) + assert_vec3_close(prim.GetAttribute("xformOp:scale").Get(), (1.0, 1.0, 1.0)) + + +def test_standardize_xform_ops_with_explicit_values(): + """Test standardize_xform_ops with explicit translation, orientation, and scale values.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with some initial transform + prim = sim_utils.create_prim( + "/World/TestExplicitValues", + "Xform", + translation=(10.0, 10.0, 10.0), + orientation=(0.7071068, 0.7071068, 0.0, 0.0), + scale=(5.0, 5.0, 5.0), + stage=stage, + ) + + # Apply standardize_xform_ops with new explicit values + new_translation = (1.0, 2.0, 3.0) + new_orientation = (1.0, 0.0, 0.0, 0.0) + new_scale = (2.0, 2.0, 2.0) + + result = sim_utils.standardize_xform_ops( + prim, translation=new_translation, orientation=new_orientation, scale=new_scale + ) + assert result is True + + # Verify the new values are set + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), new_translation) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), new_orientation) + assert_vec3_close(prim.GetAttribute("xformOp:scale").Get(), new_scale) + + # Verify the prim is at the expected world location + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + assert_vec3_close(Gf.Vec3d(*pos_after), new_translation, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_after), new_orientation, eps=1e-5) + + # Verify standard operation order + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_partial_values(): + """Test standardize_xform_ops with only some values specified.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim + prim = sim_utils.create_prim( + "/World/TestPartialValues", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(0.9238795, 0.3826834, 0.0, 0.0), # rotation around X + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Get initial local pose + pos_before, quat_before = sim_utils.resolve_prim_pose(prim, ref_prim=prim.GetParent()) + scale_before = prim.GetAttribute("xformOp:scale").Get() + + # Apply standardize_xform_ops with only translation specified + new_translation = (10.0, 20.0, 30.0) + result = sim_utils.standardize_xform_ops(prim, translation=new_translation) + assert result is True + + # Verify translation is updated + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), new_translation) + + # Verify orientation and scale are preserved + quat_after = prim.GetAttribute("xformOp:orient").Get() + scale_after = prim.GetAttribute("xformOp:scale").Get() + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + assert_vec3_close(scale_before, scale_after, eps=1e-5) + + # Verify the prim's world orientation hasn't changed (only translation changed) + _, quat_after_world = sim_utils.resolve_prim_pose(prim) + assert_quat_close(Gf.Quatd(*quat_before), quat_after_world, eps=1e-5) + + +def test_standardize_xform_ops_non_xformable_prim(caplog): + """Test standardize_xform_ops returns False for non-Xformable prims and logs error.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a Material prim (not Xformable) + from pxr import UsdShade + + material_prim = UsdShade.Material.Define(stage, "/World/TestMaterial").GetPrim() + + # Verify the prim is valid but not Xformable + assert material_prim.IsValid() + assert not material_prim.IsA(UsdGeom.Xformable) + + # Clear any previous logs + caplog.clear() + + # Attempt to apply standardize_xform_ops - should return False and log a error + with caplog.at_level("ERROR"): + result = sim_utils.standardize_xform_ops(material_prim) + + assert result is False + + # Verify that a error was logged + assert len(caplog.records) == 1 + assert caplog.records[0].levelname == "ERROR" + assert "not an Xformable" in caplog.records[0].message + assert "/World/TestMaterial" in caplog.records[0].message + + +def test_standardize_xform_ops_preserves_reset_xform_stack(): + """Test that standardize_xform_ops preserves the resetXformStack attribute.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim + prim = sim_utils.create_prim("/World/TestResetStack", "Xform", stage=stage) + xformable = UsdGeom.Xformable(prim) + + # Set resetXformStack to True + xformable.SetResetXformStack(True) + assert xformable.GetResetXformStack() is True + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Verify resetXformStack is preserved + assert xformable.GetResetXformStack() is True + + +def test_standardize_xform_ops_with_complex_hierarchy(): + """Test standardize_xform_ops on deeply nested hierarchy.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a complex hierarchy + root = sim_utils.create_prim("/World/Root", "Xform", translation=(1.0, 0.0, 0.0), stage=stage) + child1 = sim_utils.create_prim("/World/Root/Child1", "Xform", translation=(0.0, 1.0, 0.0), stage=stage) + child2 = sim_utils.create_prim("/World/Root/Child1/Child2", "Xform", translation=(0.0, 0.0, 1.0), stage=stage) + child3 = sim_utils.create_prim("/World/Root/Child1/Child2/Child3", "Cube", translation=(1.0, 1.0, 1.0), stage=stage) + + # Get world poses before + poses_before = {} + for name, prim in [("root", root), ("child1", child1), ("child2", child2), ("child3", child3)]: + poses_before[name] = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops to all prims + assert sim_utils.standardize_xform_ops(root) is True + assert sim_utils.standardize_xform_ops(child1) is True + assert sim_utils.standardize_xform_ops(child2) is True + assert sim_utils.standardize_xform_ops(child3) is True + + # Get world poses after + poses_after = {} + for name, prim in [("root", root), ("child1", child1), ("child2", child2), ("child3", child3)]: + poses_after[name] = sim_utils.resolve_prim_pose(prim) + + # Verify all world poses are preserved + for name in poses_before: + pos_before, quat_before = poses_before[name] + pos_after, quat_after = poses_after[name] + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + +def test_standardize_xform_ops_preserves_float_precision(): + """Test that standardize_xform_ops preserves float precision when it already exists.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim manually with FLOAT precision operations (not double) + prim_path = "/World/TestFloatPrecision" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add xform operations with FLOAT precision (not the default double) + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionFloat) + translate_op.Set(Gf.Vec3f(1.0, 2.0, 3.0)) + + orient_op = xformable.AddOrientOp(UsdGeom.XformOp.PrecisionFloat) + orient_op.Set(Gf.Quatf(1.0, 0.0, 0.0, 0.0)) + + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionFloat) + scale_op.Set(Gf.Vec3f(1.0, 1.0, 1.0)) + + # Verify operations exist with float precision + assert translate_op.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert orient_op.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert scale_op.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + + # Now apply standardize_xform_ops with new values (provided as double precision Python floats) + new_translation = (5.0, 10.0, 15.0) + new_orientation = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X + new_scale = (2.0, 3.0, 4.0) + + result = sim_utils.standardize_xform_ops( + prim, translation=new_translation, orientation=new_orientation, scale=new_scale + ) + assert result is True + + # Verify the precision is STILL float (not converted to double) + translate_op_after = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) + orient_op_after = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) + scale_op_after = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) + + assert translate_op_after.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert orient_op_after.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert scale_op_after.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + + # Verify the VALUES are set correctly (cast to float, so they're Gf.Vec3f and Gf.Quatf) + translate_value = prim.GetAttribute("xformOp:translate").Get() + assert isinstance(translate_value, Gf.Vec3f), f"Expected Gf.Vec3f, got {type(translate_value)}" + assert_vec3_close(translate_value, new_translation, eps=1e-5) + + orient_value = prim.GetAttribute("xformOp:orient").Get() + assert isinstance(orient_value, Gf.Quatf), f"Expected Gf.Quatf, got {type(orient_value)}" + assert_quat_close(orient_value, new_orientation, eps=1e-5) + + scale_value = prim.GetAttribute("xformOp:scale").Get() + assert isinstance(scale_value, Gf.Vec3f), f"Expected Gf.Vec3f, got {type(scale_value)}" + assert_vec3_close(scale_value, new_scale, eps=1e-5) + + # Verify the world pose matches what we set + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + assert_vec3_close(Gf.Vec3d(*pos_after), new_translation, eps=1e-4) + assert_quat_close(Gf.Quatd(*quat_after), new_orientation, eps=1e-4) + + +""" +Test validate_standard_xform_ops() function. +""" + + +def test_validate_standard_xform_ops_valid(): + """Test validate_standard_xform_ops returns True for standardized prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations + prim = sim_utils.create_prim( + "/World/TestValid", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Standardize the prim + sim_utils.standardize_xform_ops(prim) + + # Validate it + assert sim_utils.validate_standard_xform_ops(prim) is True + + +def test_validate_standard_xform_ops_invalid_order(): + """Test validate_standard_xform_ops returns False for non-standard operation order.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim and manually set up xform ops in wrong order + prim_path = "/World/TestInvalidOrder" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add operations in wrong order: scale, translate, orient (should be translate, orient, scale) + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble) + scale_op.Set(Gf.Vec3d(1.0, 1.0, 1.0)) + + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + orient_op = xformable.AddOrientOp(UsdGeom.XformOp.PrecisionDouble) + orient_op.Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_with_deprecated_ops(): + """Test validate_standard_xform_ops returns False when deprecated operations exist.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with deprecated rotateXYZ operation + prim_path = "/World/TestDeprecated" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add deprecated rotateXYZ operation + rotate_xyz_op = xformable.AddRotateXYZOp(UsdGeom.XformOp.PrecisionDouble) + rotate_xyz_op.Set(Gf.Vec3d(45.0, 30.0, 60.0)) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_missing_operations(): + """Test validate_standard_xform_ops returns False when standard operations are missing.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with only translate operation (missing orient and scale) + prim_path = "/World/TestMissing" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Validate it - should return False (missing orient and scale) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_invalid_prim(): + """Test validate_standard_xform_ops returns False for invalid prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(invalid_prim) is False + + +def test_validate_standard_xform_ops_non_xformable(): + """Test validate_standard_xform_ops returns False for non-Xformable prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a Material prim (not Xformable) + from pxr import UsdShade + + material_prim = UsdShade.Material.Define(stage, "/World/TestMaterial").GetPrim() + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(material_prim) is False + + +def test_validate_standard_xform_ops_with_transform_matrix(): + """Test validate_standard_xform_ops returns False when transform matrix operation exists.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with transform matrix + prim_path = "/World/TestTransformMatrix" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add transform matrix operation + transform_op = xformable.AddTransformOp(UsdGeom.XformOp.PrecisionDouble) + matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(5.0, 10.0, 15.0)) + transform_op.Set(matrix) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_extra_operations(): + """Test validate_standard_xform_ops returns False when extra operations exist.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations + prim = sim_utils.create_prim( + "/World/TestExtra", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Standardize it + sim_utils.standardize_xform_ops(prim) + + # Add an extra operation + xformable = UsdGeom.Xformable(prim) + extra_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + extra_op.Set(45.0) + + # Validate it - should return False (has extra operation) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_after_standardization(): + """Test validate_standard_xform_ops returns True after standardization of non-standard prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with non-standard operations + prim_path = "/World/TestBeforeAfter" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add deprecated operations + rotate_x_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + rotate_x_op.Set(45.0) + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Validate before standardization - should be False + assert sim_utils.validate_standard_xform_ops(prim) is False + + # Standardize the prim + sim_utils.standardize_xform_ops(prim) + + # Validate after standardization - should be True + assert sim_utils.validate_standard_xform_ops(prim) is True + + +def test_validate_standard_xform_ops_on_geometry(): + """Test validate_standard_xform_ops works correctly on geometry prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a cube with standard operations + cube_prim = sim_utils.create_prim( + "/World/TestCube", + "Cube", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Standardize it + sim_utils.standardize_xform_ops(cube_prim) + + # Validate it - should be True + assert sim_utils.validate_standard_xform_ops(cube_prim) is True + + +def test_validate_standard_xform_ops_empty_prim(): + """Test validate_standard_xform_ops on prim with no xform operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a bare prim with no xform operations + prim_path = "/World/TestEmpty" + prim = stage.DefinePrim(prim_path, "Xform") + + # Validate it - should return False (no operations at all) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +""" +Test resolve_prim_pose() function. +""" + + +def test_resolve_prim_pose(): + """Test resolve_prim_pose() function.""" + # number of objects + num_objects = 20 + # sample random scales for x, y, z + rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) + rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) + # sample random positions + rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + # sample random rotations + rand_quats = np.random.randn(num_objects, 3, 4) + rand_quats /= np.linalg.norm(rand_quats, axis=2, keepdims=True) + + # create objects + for i in range(num_objects): + # simple cubes + cube_prim = sim_utils.create_prim( + f"/World/Cubes/instance_{i:02d}", + "Cube", + translation=rand_positions[i, 0], + orientation=rand_quats[i, 0], + scale=rand_scales[i, 0], + attributes={"size": rand_widths[i]}, + ) + # xform hierarchy + xform_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}", + "Xform", + translation=rand_positions[i, 1], + orientation=rand_quats[i, 1], + scale=rand_scales[i, 1], + ) + geometry_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/geometry", + "Sphere", + translation=rand_positions[i, 2], + orientation=rand_quats[i, 2], + scale=rand_scales[i, 2], + attributes={"radius": rand_widths[i]}, + ) + dummy_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/dummy", + "Sphere", + ) + + # cube prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(cube_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 0, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 0], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 0], atol=1e-3) + # xform prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + # dummy prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(dummy_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + + # geometry prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(geometry_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 2, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 2] * rand_scales[i, 1], atol=1e-3) + # TODO: Enabling scale causes the test to fail because the current implementation of + # resolve_prim_pose does not correctly handle non-identity scales on Xform prims. This is a known + # limitation. Until this is fixed, the test is disabled here to ensure the test passes. + # np.testing.assert_allclose(quat, rand_quats[i, 2], atol=1e-3) + + # dummy prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(dummy_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + np.testing.assert_allclose(pos, np.zeros(3), atol=1e-3) + np.testing.assert_allclose(quat, np.array([1, 0, 0, 0]), atol=1e-3) + # xform prim w.r.t. cube prim + pos, quat = sim_utils.resolve_prim_pose(xform_prim, ref_prim=cube_prim) + pos, quat = np.array(pos), np.array(quat) + # -- compute ground truth values + gt_pos, gt_quat = math_utils.subtract_frame_transforms( + torch.from_numpy(rand_positions[i, 0]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 0]).unsqueeze(0), + torch.from_numpy(rand_positions[i, 1]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 1]).unsqueeze(0), + ) + gt_pos, gt_quat = gt_pos.squeeze(0).numpy(), gt_quat.squeeze(0).numpy() + quat = quat if np.sign(gt_quat[0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, gt_pos, atol=1e-3) + np.testing.assert_allclose(quat, gt_quat, atol=1e-3) + + +""" +Test resolve_prim_scale() function. +""" + + +def test_resolve_prim_scale(): + """Test resolve_prim_scale() function. + + To simplify the test, we assume that the effective scale at a prim + is the product of the scales of the prims in the hierarchy: + + scale = scale_of_xform * scale_of_geometry_prim + + This is only true when rotations are identity or the transforms are + orthogonal and uniformly scaled. Otherwise, scale is not composable + like that in local component-wise fashion. + """ + # number of objects + num_objects = 20 + # sample random scales for x, y, z + rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) + rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) + # sample random positions + rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + + # create objects + for i in range(num_objects): + # simple cubes + cube_prim = sim_utils.create_prim( + f"/World/Cubes/instance_{i:02d}", + "Cube", + translation=rand_positions[i, 0], + scale=rand_scales[i, 0], + attributes={"size": rand_widths[i]}, + ) + # xform hierarchy + xform_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}", + "Xform", + translation=rand_positions[i, 1], + scale=rand_scales[i, 1], + ) + geometry_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/geometry", + "Sphere", + translation=rand_positions[i, 2], + scale=rand_scales[i, 2], + attributes={"radius": rand_widths[i]}, + ) + dummy_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/dummy", + "Sphere", + ) + + # cube prim + scale = sim_utils.resolve_prim_scale(cube_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 0], atol=1e-5) + # xform prim + scale = sim_utils.resolve_prim_scale(xform_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) + # geometry prim + scale = sim_utils.resolve_prim_scale(geometry_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1] * rand_scales[i, 2], atol=1e-5) + # dummy prim + scale = sim_utils.resolve_prim_scale(dummy_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) + + +""" +Test convert_world_pose_to_local() function. +""" + + +def test_convert_world_pose_to_local_basic(): + """Test basic world-to-local pose conversion.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent and child prims + parent_prim = sim_utils.create_prim( + "/World/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), # identity rotation + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # World pose we want to achieve for a child + world_position = (10.0, 3.0, 0.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) # identity rotation + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + # Assert orientation is not None + assert local_orientation is not None + + # The expected local translation is world_position - parent_position = (10-5, 3-0, 0-0) = (5, 3, 0) + assert_vec3_close(Gf.Vec3d(*local_translation), (5.0, 3.0, 0.0), eps=1e-5) + assert_quat_close(Gf.Quatd(*local_orientation), (1.0, 0.0, 0.0, 0.0), eps=1e-5) + + +def test_convert_world_pose_to_local_with_rotation(): + """Test world-to-local conversion with parent rotation.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent with 90-degree rotation around Z axis + parent_prim = sim_utils.create_prim( + "/World/RotatedParent", + "Xform", + translation=(0.0, 0.0, 0.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # World pose: position at (1, 0, 0) with identity rotation + world_position = (1.0, 0.0, 0.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + + # Create a child with the local transform and verify world pose + child_prim = sim_utils.create_prim( + "/World/RotatedParent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Get world pose of child + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) + + # Verify it matches the desired world pose + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-5) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-5) + + +def test_convert_world_pose_to_local_with_scale(): + """Test world-to-local conversion with parent scale.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent with non-uniform scale + parent_prim = sim_utils.create_prim( + "/World/ScaledParent", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # World pose we want + world_position = (5.0, 6.0, 7.0) + world_orientation = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + + # Create child and verify + child_prim = sim_utils.create_prim( + "/World/ScaledParent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Get world pose + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) + + # Verify (may have some tolerance due to scale effects on rotation) + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) + + +def test_convert_world_pose_to_local_invalid_parent(): + """Test world-to-local conversion with invalid parent returns world pose unchanged.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + assert not invalid_prim.IsValid() + + world_position = (10.0, 20.0, 30.0) + world_orientation = (0.7071068, 0.0, 0.7071068, 0.0) + + # Convert with invalid reference prim + with pytest.raises(ValueError): + sim_utils.convert_world_pose_to_local(world_position, world_orientation, invalid_prim) + + +def test_convert_world_pose_to_local_root_parent(): + """Test world-to-local conversion with root as parent returns world pose unchanged.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get the pseudo-root prim + root_prim = stage.GetPrimAtPath("/") + + world_position = (15.0, 25.0, 35.0) + world_orientation = (0.9238795, 0.3826834, 0.0, 0.0) + + # Convert with root parent + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, root_prim + ) + # Assert orientation is not None + assert local_orientation is not None + + # Should return unchanged + assert_vec3_close(Gf.Vec3d(*local_translation), world_position, eps=1e-10) + assert_quat_close(Gf.Quatd(*local_orientation), world_orientation, eps=1e-10) + + +def test_convert_world_pose_to_local_none_orientation(): + """Test world-to-local conversion with None orientation.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent + parent_prim = sim_utils.create_prim( + "/World/ParentNoOrient", + "Xform", + translation=(3.0, 4.0, 5.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + stage=stage, + ) + + world_position = (10.0, 10.0, 10.0) + + # Convert with None orientation + local_translation, local_orientation = sim_utils.convert_world_pose_to_local(world_position, None, parent_prim) + + # Orientation should be None + assert local_orientation is None + # Translation should still be converted + assert local_translation is not None + + +def test_convert_world_pose_to_local_complex_hierarchy(): + """Test world-to-local conversion in a complex hierarchy.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a complex hierarchy + _ = sim_utils.create_prim( + "/World/Grandparent", + "Xform", + translation=(10.0, 0.0, 0.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + parent = sim_utils.create_prim( + "/World/Grandparent/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), # local to grandparent + orientation=(0.7071068, 0.7071068, 0.0, 0.0), # 90 deg around X + scale=(0.5, 0.5, 0.5), + stage=stage, + ) + + # World pose we want to achieve + world_position = (20.0, 15.0, 10.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) + + # Convert to local space relative to parent + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent + ) + + # Create child with the computed local transform + child = sim_utils.create_prim( + "/World/Grandparent/Parent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Verify world pose + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child) + + # Should match the desired world pose (with some tolerance for complex transforms) + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) + + +def test_convert_world_pose_to_local_with_mixed_prim_types(): + """Test world-to-local conversion with mixed prim types (Xform, Scope, Mesh).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a hierarchy with different prim types + # Grandparent: Xform with transform + sim_utils.create_prim( + "/World/Grandparent", + "Xform", + translation=(5.0, 3.0, 2.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Parent: Scope prim (organizational, typically has no transform) + parent = stage.DefinePrim("/World/Grandparent/Parent", "Scope") + + # Obtain parent prim pose (should be grandparent's transform) + parent_pos, parent_quat = sim_utils.resolve_prim_pose(parent) + assert_vec3_close(Gf.Vec3d(*parent_pos), (5.0, 3.0, 2.0), eps=1e-5) + assert_quat_close(Gf.Quatd(*parent_quat), (0.7071068, 0.0, 0.0, 0.7071068), eps=1e-5) + + # Child: Mesh prim (geometry) + child = sim_utils.create_prim("/World/Grandparent/Parent/Child", "Mesh", stage=stage) + + # World pose we want to achieve for the child + world_position = (10.0, 5.0, 3.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) # identity rotation + + # Convert to local space relative to parent (Scope) + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, child + ) + + # Verify orientation is not None + assert local_orientation is not None, "Expected orientation to be computed" + + # Set the local transform on the child (Mesh) + xformable = UsdGeom.Xformable(child) + translate_op = xformable.GetTranslateOp() + translate_op.Set(Gf.Vec3d(*local_translation)) + orient_op = xformable.GetOrientOp() + orient_op.Set(Gf.Quatd(*local_orientation)) + + # Verify world pose of child + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child) + + # Should match the desired world pose + # Note: Scope prims typically have no transform, so the child's world pose should account + # for the grandparent's transform + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-10) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-10) diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py new file mode 100644 index 000000000000..94b49a560bc3 --- /dev/null +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -0,0 +1,1500 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest # noqa: E402 +import torch # noqa: E402 + +try: + from isaacsim.core.prims import XFormPrim as _IsaacSimXformPrimView +except (ModuleNotFoundError, ImportError): + _IsaacSimXformPrimView = None + +import isaaclab.sim as sim_utils # noqa: E402 +from isaaclab.sim.views import XformPrimView as XformPrimView # noqa: E402 +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # noqa: E402 + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + sim_utils.update_stage() + + # Yield for the test + yield + + # Teardown: Clear stage after each test + sim_utils.clear_stage() + sim_utils.SimulationContext.clear_instance() + + +""" +Helper functions. +""" + + +def _prepare_indices(index_type, target_indices, num_prims, device): + """Helper function to prepare indices based on type.""" + if index_type == "list": + return target_indices, target_indices + elif index_type == "torch_tensor": + return torch.tensor(target_indices, dtype=torch.int64, device=device), target_indices + elif index_type == "slice_none": + return slice(None), list(range(num_prims)) + else: + raise ValueError(f"Unknown index type: {index_type}") + + +def _skip_if_backend_unavailable(backend: str, device: str): + """Skip tests when the requested backend is unavailable.""" + if device.startswith("cuda") and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + if backend == "fabric" and device == "cpu": + pytest.skip("Warp fabricarray operations on CPU have known issues") + + +def _prim_type_for_backend(backend: str) -> str: + """Return a prim type that is compatible with the backend.""" + return "Camera" if backend == "fabric" else "Xform" + + +def _create_view(pattern: str, device: str, backend: str) -> XformPrimView: + """Create an XformPrimView for the requested backend.""" + if backend == "fabric": + sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) + return XformPrimView(pattern, device=device) + + +""" +Tests - Initialization. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_single_prim(device): + """Test XformPrimView initialization with a single prim.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + # Create a single xform prim + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/Object", "Xform", translation=(1.0, 2.0, 3.0), stage=stage) + + # Create view + view = XformPrimView("/World/Object", device=device) + + # Verify properties + assert view.count == 1 + assert view.prim_paths == ["/World/Object"] + assert view.device == device + assert len(view.prims) == 1 + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_multiple_prims(device): + """Test XformPrimView initialization with multiple prims using pattern matching.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + # Create multiple prims + num_prims = 10 + stage = sim_utils.get_current_stage() + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(i * 2.0, 0.0, 1.0), stage=stage) + + # Create view with pattern + view = XformPrimView("/World/Env_.*/Object", device=device) + + # Verify properties + assert view.count == num_prims + assert view.device == device + assert len(view.prims) == num_prims + assert view.prim_paths == [f"/World/Env_{i}/Object" for i in range(num_prims)] + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_multiple_prims_order(device): + """Test XformPrimView initialization with multiple prims using pattern matching with multiple objects per prim. + + This test validates that XformPrimView respects USD stage traversal order, which is based on + creation order (depth-first search), NOT alphabetical/lexical sorting. This is an important + edge case that ensures deterministic prim ordering that matches USD's internal representation. + + The test creates prims in a deliberately non-alphabetical order (1, 0, A, a, 2) and verifies + that they are retrieved in creation order, not sorted order (0, 1, 2, A, a). + """ + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + # Create multiple prims + num_prims = 10 + stage = sim_utils.get_current_stage() + + # NOTE: Prims are created in a specific order to test that XformPrimView respects + # USD stage traversal order (DFS based on creation order), NOT alphabetical/lexical order. + # This is an important edge case: children under the same parent are returned in the + # order they were created, not sorted by name. + + # First batch: Create Object_1, Object_0, Object_A for each environment + # (intentionally non-alphabetical: 1, 0, A instead of 0, 1, A) + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}/Object_1", "Xform", translation=(i * 2.0, -2.0, 1.0), stage=stage) + sim_utils.create_prim(f"/World/Env_{i}/Object_0", "Xform", translation=(i * 2.0, 2.0, 1.0), stage=stage) + sim_utils.create_prim(f"/World/Env_{i}/Object_A", "Xform", translation=(i * 2.0, 0.0, -1.0), stage=stage) + + # Second batch: Create Object_a, Object_2 for each environment + # (created after the first batch to verify traversal is depth-first per environment) + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}/Object_a", "Xform", translation=(i * 2.0, 2.0, -1.0), stage=stage) + sim_utils.create_prim(f"/World/Env_{i}/Object_2", "Xform", translation=(i * 2.0, 2.0, 1.0), stage=stage) + + # Create view with pattern + view = XformPrimView("/World/Env_.*/Object_.*", device=device) + + # Expected ordering: DFS traversal by environment, with children in creation order + # For each Env_i, we expect: Object_1, Object_0, Object_A, Object_a, Object_2 + # (matches creation order, NOT alphabetical: would be 0, 1, 2, A, a if sorted) + expected_prim_paths_ordering = [] + for i in range(num_prims): + expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_1") + expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_0") + expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_A") + expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_a") + expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_2") + + # Verify properties + assert view.count == num_prims * 5 + assert view.device == device + assert len(view.prims) == num_prims * 5 + assert view.prim_paths == expected_prim_paths_ordering + + # Additional validation: Verify ordering is NOT alphabetical + # If it were alphabetical, Object_0 would come before Object_1 + alphabetical_order = [] + for i in range(num_prims): + alphabetical_order.append(f"/World/Env_{i}/Object_0") + alphabetical_order.append(f"/World/Env_{i}/Object_1") + alphabetical_order.append(f"/World/Env_{i}/Object_2") + alphabetical_order.append(f"/World/Env_{i}/Object_A") + alphabetical_order.append(f"/World/Env_{i}/Object_a") + + assert view.prim_paths != alphabetical_order, ( + "Prim paths should follow creation order, not alphabetical order. " + "This test validates that USD stage traversal respects creation order." + ) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_invalid_prim(device): + """Test XformPrimView initialization fails for non-xformable prims.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create a prim with non-standard xform operations + stage.DefinePrim("/World/InvalidPrim", "Xform") + + # XformPrimView should raise ValueError because prim doesn't have standard operations + with pytest.raises(ValueError, match="not a xformable prim"): + XformPrimView("/World/InvalidPrim", device=device) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_empty_pattern(device): + """Test XformPrimView initialization with pattern that matches no prims.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + sim_utils.create_new_stage() + + # Create view with pattern that matches nothing + view = XformPrimView("/World/NonExistent_.*", device=device) + + # Should have zero count + assert view.count == 0 + assert len(view.prims) == 0 + + +""" +Tests - Getters. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_get_world_poses(device, backend): + """Test getting world poses from XformPrimView.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims with known world poses + expected_positions = [(1.0, 2.0, 3.0), (4.0, 5.0, 6.0), (7.0, 8.0, 9.0)] + expected_orientations = [(1.0, 0.0, 0.0, 0.0), (0.7071068, 0.0, 0.0, 0.7071068), (0.7071068, 0.7071068, 0.0, 0.0)] + + for i, (pos, quat) in enumerate(zip(expected_positions, expected_orientations)): + sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=pos, orientation=quat, stage=stage) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Convert expected values to tensors + expected_positions_tensor = torch.tensor(expected_positions, dtype=torch.float32, device=device) + expected_orientations_tensor = torch.tensor(expected_orientations, dtype=torch.float32, device=device) + + # Get world poses + positions, orientations = view.get_world_poses() + + # Verify shapes + assert positions.shape == (3, 3) + assert orientations.shape == (3, 4) + + # Verify positions + torch.testing.assert_close(positions, expected_positions_tensor, atol=1e-5, rtol=0) + + # Verify orientations (allow for quaternion sign ambiguity) + try: + torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_get_local_poses(device, backend): + """Test getting local poses from XformPrimView.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create parent and child prims + sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) + + # Children with different local poses + expected_local_positions = [(1.0, 0.0, 0.0), (0.0, 2.0, 0.0), (0.0, 0.0, 3.0)] + expected_local_orientations = [ + (1.0, 0.0, 0.0, 0.0), + (0.7071068, 0.0, 0.0, 0.7071068), + (0.7071068, 0.7071068, 0.0, 0.0), + ] + + for i, (pos, quat) in enumerate(zip(expected_local_positions, expected_local_orientations)): + sim_utils.create_prim(f"/World/Parent/Child_{i}", prim_type, translation=pos, orientation=quat, stage=stage) + + # Create view + view = _create_view("/World/Parent/Child_.*", device=device, backend=backend) + + # Get local poses + translations, orientations = view.get_local_poses() + + # Verify shapes + assert translations.shape == (3, 3) + assert orientations.shape == (3, 4) + + # Convert expected values to tensors + expected_translations_tensor = torch.tensor(expected_local_positions, dtype=torch.float32, device=device) + expected_orientations_tensor = torch.tensor(expected_local_orientations, dtype=torch.float32, device=device) + + # Verify translations + torch.testing.assert_close(translations, expected_translations_tensor, atol=1e-5, rtol=0) + + # Verify orientations (allow for quaternion sign ambiguity) + try: + torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_get_scales(device, backend): + """Test getting scales from XformPrimView.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims with different scales + expected_scales = [(1.0, 1.0, 1.0), (2.0, 2.0, 2.0), (1.0, 2.0, 3.0)] + + for i, scale in enumerate(expected_scales): + sim_utils.create_prim(f"/World/Object_{i}", prim_type, scale=scale, stage=stage) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + expected_scales_tensor = torch.tensor(expected_scales, dtype=torch.float32, device=device) + + # Get scales + scales = view.get_scales() + + # Verify shape and values + assert scales.shape == (3, 3) + torch.testing.assert_close(scales, expected_scales_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_visibility(device): + """Test getting visibility when all prims are visible.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims (default is visible) + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + + # Create view + view = XformPrimView("/World/Object_.*", device=device) + + # Get visibility + visibility = view.get_visibility() + + # Verify shape and values + assert visibility.shape == (num_prims,) + assert visibility.dtype == torch.bool + assert torch.all(visibility), "All prims should be visible by default" + + +""" +Tests - Setters. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_set_world_poses(device, backend): + """Test setting world poses in XformPrimView.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(0.0, 0.0, 0.0), stage=stage) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Set new world poses + new_positions = torch.tensor( + [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0], [10.0, 11.0, 12.0], [13.0, 14.0, 15.0]], device=device + ) + new_orientations = torch.tensor( + [ + [1.0, 0.0, 0.0, 0.0], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.7071068, 0.7071068, 0.0, 0.0], + [0.9238795, 0.3826834, 0.0, 0.0], + [0.7071068, 0.0, 0.7071068, 0.0], + ], + device=device, + ) + + view.set_world_poses(new_positions, new_orientations) + + # Get the poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Verify they match + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + # Check quaternions (allow sign flip) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_set_world_poses_only_positions(device, backend): + """Test setting only positions, leaving orientations unchanged.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims with specific orientations + initial_quat = (0.7071068, 0.0, 0.0, 0.7071068) # 90 deg around Z + for i in range(3): + sim_utils.create_prim( + f"/World/Object_{i}", prim_type, translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage + ) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Get initial orientations + _, initial_orientations = view.get_world_poses() + + # Set only positions + new_positions = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) + view.set_world_poses(positions=new_positions, orientations=None) + + # Get poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Positions should be updated + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + + # Orientations should be unchanged + try: + torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_set_world_poses_only_orientations(device, backend): + """Test setting only orientations, leaving positions unchanged.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims with specific positions + for i in range(3): + sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(float(i), 0.0, 0.0), stage=stage) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Get initial positions + initial_positions, _ = view.get_world_poses() + + # Set only orientations + new_orientations = torch.tensor( + [[0.7071068, 0.0, 0.0, 0.7071068], [0.7071068, 0.7071068, 0.0, 0.0], [0.9238795, 0.3826834, 0.0, 0.0]], + device=device, + ) + view.set_world_poses(positions=None, orientations=new_orientations) + + # Get poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Positions should be unchanged + torch.testing.assert_close(retrieved_positions, initial_positions, atol=1e-5, rtol=0) + + # Orientations should be updated + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_set_world_poses_with_hierarchy(device, backend): + """Test setting world poses correctly handles parent transformations.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + child_prim_type = _prim_type_for_backend(backend) + + # Create parent prims + for i in range(3): + parent_pos = (i * 10.0, 0.0, 0.0) + parent_quat = (0.7071068, 0.0, 0.0, 0.7071068) # 90 deg around Z + sim_utils.create_prim( + f"/World/Parent_{i}", "Xform", translation=parent_pos, orientation=parent_quat, stage=stage + ) + # Create child prims + sim_utils.create_prim(f"/World/Parent_{i}/Child", child_prim_type, translation=(0.0, 0.0, 0.0), stage=stage) + + # Create view for children + view = _create_view("/World/Parent_.*/Child", device=device, backend=backend) + + # Set world poses for children + desired_world_positions = torch.tensor([[5.0, 5.0, 0.0], [15.0, 5.0, 0.0], [25.0, 5.0, 0.0]], device=device) + desired_world_orientations = torch.tensor( + [[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]], device=device + ) + + view.set_world_poses(desired_world_positions, desired_world_orientations) + + # Get world poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Should match desired world poses + torch.testing.assert_close(retrieved_positions, desired_world_positions, atol=1e-4, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, desired_world_orientations, atol=1e-4, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -desired_world_orientations, atol=1e-4, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_set_local_poses(device, backend): + """Test setting local poses in XformPrimView.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create parent + sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 5.0), stage=stage) + + # Create children + num_prims = 4 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Parent/Child_{i}", prim_type, translation=(0.0, 0.0, 0.0), stage=stage) + + # Create view + view = _create_view("/World/Parent/Child_.*", device=device, backend=backend) + + # Set new local poses + new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0], [4.0, 4.0, 4.0]], device=device) + new_orientations = torch.tensor( + [ + [1.0, 0.0, 0.0, 0.0], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.7071068, 0.7071068, 0.0, 0.0], + [0.9238795, 0.3826834, 0.0, 0.0], + ], + device=device, + ) + + view.set_local_poses(new_translations, new_orientations) + + # Get local poses back + retrieved_translations, retrieved_orientations = view.get_local_poses() + + # Verify they match + torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_set_local_poses_only_translations(device, backend): + """Test setting only local translations.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create parent and children with specific orientations + sim_utils.create_prim("/World/Parent", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + initial_quat = (0.7071068, 0.0, 0.0, 0.7071068) + + for i in range(3): + sim_utils.create_prim( + f"/World/Parent/Child_{i}", + prim_type, + translation=(0.0, 0.0, 0.0), + orientation=initial_quat, + stage=stage, + ) + + # Create view + view = _create_view("/World/Parent/Child_.*", device=device, backend=backend) + + # Get initial orientations + _, initial_orientations = view.get_local_poses() + + # Set only translations + new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) + view.set_local_poses(translations=new_translations, orientations=None) + + # Get poses back + retrieved_translations, retrieved_orientations = view.get_local_poses() + + # Translations should be updated + torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) + + # Orientations should be unchanged + try: + torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_set_scales(device, backend): + """Test setting scales in XformPrimView.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", prim_type, scale=(1.0, 1.0, 1.0), stage=stage) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Set new scales + new_scales = torch.tensor( + [[2.0, 2.0, 2.0], [1.0, 2.0, 3.0], [0.5, 0.5, 0.5], [3.0, 1.0, 2.0], [1.5, 1.5, 1.5]], device=device + ) + + view.set_scales(new_scales) + + # Get scales back + retrieved_scales = view.get_scales() + + # Verify they match + torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_visibility(device): + """Test toggling visibility multiple times.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 3 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", stage=stage) + + # Create view + view = XformPrimView("/World/Object_.*", device=device) + + # Initial state: all visible + visibility = view.get_visibility() + assert torch.all(visibility), "All should be visible initially" + + # Make all invisible + view.set_visibility(torch.zeros(num_prims, dtype=torch.bool, device=device)) + visibility = view.get_visibility() + assert not torch.any(visibility), "All should be invisible" + + # Make all visible again + view.set_visibility(torch.ones(num_prims, dtype=torch.bool, device=device)) + visibility = view.get_visibility() + assert torch.all(visibility), "All should be visible again" + + # Toggle individual prims + view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device), indices=[1]) + visibility = view.get_visibility() + assert visibility[0] and not visibility[1] and visibility[2], "Only middle prim should be invisible" + + +""" +Tests - Index Handling. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("index_type", ["list", "torch_tensor", "slice_none"]) +@pytest.mark.parametrize("method", ["world_poses", "local_poses", "scales", "visibility"]) +def test_index_types_get_methods(device, index_type, method): + """Test that getter methods work with different index types.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims based on method type + num_prims = 10 + if method == "local_poses": + # Create parent and children for local poses + sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) + for i in range(num_prims): + sim_utils.create_prim( + f"/World/Parent/Child_{i}", "Xform", translation=(float(i), float(i) * 0.5, 0.0), stage=stage + ) + view = XformPrimView("/World/Parent/Child_.*", device=device) + elif method == "scales": + # Create prims with different scales + for i in range(num_prims): + scale = (1.0 + i * 0.5, 1.0 + i * 0.3, 1.0 + i * 0.2) + sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=scale, stage=stage) + view = XformPrimView("/World/Object_.*", device=device) + else: # world_poses + # Create prims with different positions + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + view = XformPrimView("/World/Object_.*", device=device) + + # Get all data as reference + if method == "world_poses": + all_data1, all_data2 = view.get_world_poses() + elif method == "local_poses": + all_data1, all_data2 = view.get_local_poses() + elif method == "scales": + all_data1 = view.get_scales() + all_data2 = None + else: # visibility + all_data1 = view.get_visibility() + all_data2 = None + + # Prepare indices + target_indices_base = [2, 5, 7] + indices, target_indices = _prepare_indices(index_type, target_indices_base, num_prims, device) + + # Get subset + if method == "world_poses": + subset_data1, subset_data2 = view.get_world_poses(indices=indices) # type: ignore[arg-type] + elif method == "local_poses": + subset_data1, subset_data2 = view.get_local_poses(indices=indices) # type: ignore[arg-type] + elif method == "scales": + subset_data1 = view.get_scales(indices=indices) # type: ignore[arg-type] + subset_data2 = None + else: # visibility + subset_data1 = view.get_visibility(indices=indices) # type: ignore[arg-type] + subset_data2 = None + + # Verify shapes + expected_count = len(target_indices) + if method == "visibility": + assert subset_data1.shape == (expected_count,) + else: + assert subset_data1.shape == (expected_count, 3) + if subset_data2 is not None: + assert subset_data2.shape == (expected_count, 4) + + # Verify values + target_indices_tensor = torch.tensor(target_indices, dtype=torch.int64, device=device) + torch.testing.assert_close(subset_data1, all_data1[target_indices_tensor], atol=1e-5, rtol=0) + if subset_data2 is not None and all_data2 is not None: + torch.testing.assert_close(subset_data2, all_data2[target_indices_tensor], atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("index_type", ["list", "torch_tensor", "slice_none"]) +@pytest.mark.parametrize("method", ["world_poses", "local_poses", "scales", "visibility"]) +def test_index_types_set_methods(device, index_type, method): + """Test that setter methods work with different index types.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims based on method type + num_prims = 10 + if method == "local_poses": + # Create parent and children for local poses + sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 0.0), stage=stage) + for i in range(num_prims): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + view = XformPrimView("/World/Parent/Child_.*", device=device) + else: # world_poses or scales + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + view = XformPrimView("/World/Object_.*", device=device) + + # Get initial data + if method == "world_poses": + initial_data1, initial_data2 = view.get_world_poses() + elif method == "local_poses": + initial_data1, initial_data2 = view.get_local_poses() + elif method == "scales": + initial_data1 = view.get_scales() + initial_data2 = None + else: # visibility + initial_data1 = view.get_visibility() + initial_data2 = None + + # Prepare indices + target_indices_base = [2, 5, 7] + indices, target_indices = _prepare_indices(index_type, target_indices_base, num_prims, device) + + # Prepare new data + num_to_set = len(target_indices) + if method in ["world_poses", "local_poses"]: + new_data1 = torch.randn(num_to_set, 3, device=device) * 10.0 + new_data2 = torch.tensor([[1.0, 0.0, 0.0, 0.0]] * num_to_set, dtype=torch.float32, device=device) + elif method == "scales": + new_data1 = torch.rand(num_to_set, 3, device=device) * 2.0 + 0.5 + new_data2 = None + else: # visibility + # Set to False to test change (default is True) + new_data1 = torch.zeros(num_to_set, dtype=torch.bool, device=device) + new_data2 = None + + # Set data + if method == "world_poses": + view.set_world_poses(positions=new_data1, orientations=new_data2, indices=indices) # type: ignore[arg-type] + elif method == "local_poses": + view.set_local_poses(translations=new_data1, orientations=new_data2, indices=indices) # type: ignore[arg-type] + elif method == "scales": + view.set_scales(scales=new_data1, indices=indices) # type: ignore[arg-type] + else: # visibility + view.set_visibility(visibility=new_data1, indices=indices) # type: ignore[arg-type] + + # Get all data after update + if method == "world_poses": + updated_data1, updated_data2 = view.get_world_poses() + elif method == "local_poses": + updated_data1, updated_data2 = view.get_local_poses() + elif method == "scales": + updated_data1 = view.get_scales() + updated_data2 = None + else: # visibility + updated_data1 = view.get_visibility() + updated_data2 = None + + # Verify that specified indices were updated + for i, target_idx in enumerate(target_indices): + torch.testing.assert_close(updated_data1[target_idx], new_data1[i], atol=1e-5, rtol=0) + if new_data2 is not None and updated_data2 is not None: + try: + torch.testing.assert_close(updated_data2[target_idx], new_data2[i], atol=1e-5, rtol=0) + except AssertionError: + # Account for quaternion sign ambiguity + torch.testing.assert_close(updated_data2[target_idx], -new_data2[i], atol=1e-5, rtol=0) + + # Verify that other indices were NOT updated (only for non-slice(None) cases) + if index_type != "slice_none": + for i in range(num_prims): + if i not in target_indices: + torch.testing.assert_close(updated_data1[i], initial_data1[i], atol=1e-5, rtol=0) + if initial_data2 is not None and updated_data2 is not None: + try: + torch.testing.assert_close(updated_data2[i], initial_data2[i], atol=1e-5, rtol=0) + except AssertionError: + # Account for quaternion sign ambiguity + torch.testing.assert_close(updated_data2[i], -initial_data2[i], atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_indices_single_element(device, backend): + """Test with a single index.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(float(i), 0.0, 0.0), stage=stage) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Test with single index + indices = [3] + positions, orientations = view.get_world_poses(indices=indices) + + # Verify shapes + assert positions.shape == (1, 3) + assert orientations.shape == (1, 4) + + # Set pose for single index + new_position = torch.tensor([[100.0, 200.0, 300.0]], device=device) + view.set_world_poses(positions=new_position, indices=indices) + + # Verify it was set + retrieved_positions, _ = view.get_world_poses(indices=indices) + torch.testing.assert_close(retrieved_positions, new_position, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_indices_out_of_order(device, backend): + """Test with indices provided in non-sequential order.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims + num_prims = 10 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(0.0, 0.0, 0.0), stage=stage) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Use out-of-order indices + indices = [7, 2, 9, 0, 5] + new_positions = torch.tensor( + [[7.0, 0.0, 0.0], [2.0, 0.0, 0.0], [9.0, 0.0, 0.0], [0.0, 0.0, 0.0], [5.0, 0.0, 0.0]], device=device + ) + + # Set poses with out-of-order indices + view.set_world_poses(positions=new_positions, indices=indices) + + # Get all poses + all_positions, _ = view.get_world_poses() + + # Verify each index got the correct value + expected_x_values = [0.0, 0.0, 2.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 9.0] + for i in range(num_prims): + assert abs(all_positions[i, 0].item() - expected_x_values[i]) < 1e-5 + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("backend", ["usd", "fabric"]) +def test_indices_with_only_positions_or_orientations(device, backend): + """Test indices work correctly when setting only positions or only orientations.""" + _skip_if_backend_unavailable(backend, device) + + stage = sim_utils.get_current_stage() + prim_type = _prim_type_for_backend(backend) + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim( + f"/World/Object_{i}", + prim_type, + translation=(0.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), + stage=stage, + ) + + # Create view + view = _create_view("/World/Object_.*", device=device, backend=backend) + + # Get initial poses + initial_positions, initial_orientations = view.get_world_poses() + + # Set only positions for specific indices + indices = [1, 3] + new_positions = torch.tensor([[10.0, 0.0, 0.0], [30.0, 0.0, 0.0]], device=device) + view.set_world_poses(positions=new_positions, orientations=None, indices=indices) + + # Get updated poses + updated_positions, updated_orientations = view.get_world_poses() + + # Verify positions updated for indices 1 and 3, others unchanged + torch.testing.assert_close(updated_positions[1], new_positions[0], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_positions[3], new_positions[1], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_positions[0], initial_positions[0], atol=1e-5, rtol=0) + + # Verify all orientations unchanged + try: + torch.testing.assert_close(updated_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(updated_orientations, -initial_orientations, atol=1e-5, rtol=0) + + # Now set only orientations for different indices + indices2 = [0, 4] + new_orientations = torch.tensor([[0.7071068, 0.0, 0.0, 0.7071068], [0.7071068, 0.7071068, 0.0, 0.0]], device=device) + view.set_world_poses(positions=None, orientations=new_orientations, indices=indices2) + + # Get final poses + final_positions, final_orientations = view.get_world_poses() + + # Verify positions unchanged from previous step + torch.testing.assert_close(final_positions, updated_positions, atol=1e-5, rtol=0) + + # Verify orientations updated for indices 0 and 4 + try: + torch.testing.assert_close(final_orientations[0], new_orientations[0], atol=1e-5, rtol=0) + torch.testing.assert_close(final_orientations[4], new_orientations[1], atol=1e-5, rtol=0) + except AssertionError: + # Account for quaternion sign ambiguity + torch.testing.assert_close(final_orientations[0], -new_orientations[0], atol=1e-5, rtol=0) + torch.testing.assert_close(final_orientations[4], -new_orientations[1], atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_index_type_none_equivalent_to_all(device): + """Test that indices=None is equivalent to getting/setting all prims.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 6 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + + # Create view + view = XformPrimView("/World/Object_.*", device=device) + + # Get poses with indices=None + pos_none, quat_none = view.get_world_poses(indices=None) + + # Get poses with no argument (default) + pos_default, quat_default = view.get_world_poses() + + # Get poses with slice(None) + pos_slice, quat_slice = view.get_world_poses(indices=slice(None)) # type: ignore[arg-type] + + # All should be equivalent + torch.testing.assert_close(pos_none, pos_default, atol=1e-10, rtol=0) + torch.testing.assert_close(quat_none, quat_default, atol=1e-10, rtol=0) + torch.testing.assert_close(pos_none, pos_slice, atol=1e-10, rtol=0) + torch.testing.assert_close(quat_none, quat_slice, atol=1e-10, rtol=0) + + # Test the same for set operations + new_positions = torch.randn(num_prims, 3, device=device) * 10.0 + new_orientations = torch.tensor([[1.0, 0.0, 0.0, 0.0]] * num_prims, dtype=torch.float32, device=device) + + # Set with indices=None + view.set_world_poses(positions=new_positions, orientations=new_orientations, indices=None) + pos_after_none, quat_after_none = view.get_world_poses() + + # Reset + view.set_world_poses(positions=torch.zeros(num_prims, 3, device=device), indices=None) + + # Set with slice(None) + view.set_world_poses( + positions=new_positions, + orientations=new_orientations, + indices=slice(None), # type: ignore[arg-type] + ) + pos_after_slice, quat_after_slice = view.get_world_poses() + + # Should be equivalent + torch.testing.assert_close(pos_after_none, pos_after_slice, atol=1e-5, rtol=0) + torch.testing.assert_close(quat_after_none, quat_after_slice, atol=1e-5, rtol=0) + + +""" +Tests - Integration. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_with_franka_robots(device): + """Test XformPrimView with real Franka robot USD assets.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Load Franka robot assets + franka_usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" + + # Add two Franka robots to the stage + sim_utils.create_prim("/World/Franka_1", "Xform", usd_path=franka_usd_path, stage=stage) + sim_utils.create_prim("/World/Franka_2", "Xform", usd_path=franka_usd_path, stage=stage) + + # Create view for both Frankas + frankas_view = XformPrimView("/World/Franka_.*", device=device) + + # Verify count + assert frankas_view.count == 2 + + # Get initial world poses (should be at origin) + initial_positions, initial_orientations = frankas_view.get_world_poses() + + # Verify initial positions are at origin + expected_initial_positions = torch.zeros(2, 3, device=device) + torch.testing.assert_close(initial_positions, expected_initial_positions, atol=1e-5, rtol=0) + + # Verify initial orientations are identity + expected_initial_orientations = torch.tensor([[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]], device=device) + try: + torch.testing.assert_close(initial_orientations, expected_initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(initial_orientations, -expected_initial_orientations, atol=1e-5, rtol=0) + + # Set new world poses + new_positions = torch.tensor([[10.0, 10.0, 0.0], [-40.0, -40.0, 0.0]], device=device) + # 90ยฐ rotation around Z axis for first, -90ยฐ for second + new_orientations = torch.tensor( + [[0.7071068, 0.0, 0.0, 0.7071068], [0.7071068, 0.0, 0.0, -0.7071068]], device=device + ) + + frankas_view.set_world_poses(positions=new_positions, orientations=new_orientations) + + # Get poses back and verify + retrieved_positions, retrieved_orientations = frankas_view.get_world_poses() + + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_with_nested_targets(device): + """Test with nested frame/target structure similar to Isaac Sim tests.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create frames and targets + for i in range(1, 4): + sim_utils.create_prim(f"/World/Frame_{i}", "Xform", stage=stage) + sim_utils.create_prim(f"/World/Frame_{i}/Target", "Xform", stage=stage) + + # Create views + frames_view = XformPrimView("/World/Frame_.*", device=device) + targets_view = XformPrimView("/World/Frame_.*/Target", device=device) + + assert frames_view.count == 3 + assert targets_view.count == 3 + + # Set local poses for frames + frame_translations = torch.tensor([[0.0, 0.0, 0.0], [0.0, 10.0, 5.0], [0.0, 3.0, 5.0]], device=device) + frames_view.set_local_poses(translations=frame_translations) + + # Set local poses for targets + target_translations = torch.tensor([[0.0, 20.0, 10.0], [0.0, 30.0, 20.0], [0.0, 50.0, 10.0]], device=device) + targets_view.set_local_poses(translations=target_translations) + + # Get world poses of targets + world_positions, _ = targets_view.get_world_poses() + + # Expected world positions are frame_translation + target_translation + expected_positions = torch.tensor([[0.0, 20.0, 10.0], [0.0, 40.0, 25.0], [0.0, 53.0, 15.0]], device=device) + + torch.testing.assert_close(world_positions, expected_positions, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_visibility_with_hierarchy(device): + """Test visibility with parent-child hierarchy and inheritance.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create parent and children + sim_utils.create_prim("/World/Parent", "Xform", stage=stage) + + num_children = 4 + for i in range(num_children): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", stage=stage) + + # Create views for both parent and children + parent_view = XformPrimView("/World/Parent", device=device) + children_view = XformPrimView("/World/Parent/Child_.*", device=device) + + # Verify parent and all children are visible initially + parent_visibility = parent_view.get_visibility() + children_visibility = children_view.get_visibility() + assert parent_visibility[0], "Parent should be visible initially" + assert torch.all(children_visibility), "All children should be visible initially" + + # Make some children invisible directly + new_visibility = torch.tensor([True, False, True, False], dtype=torch.bool, device=device) + children_view.set_visibility(new_visibility) + + # Verify the visibility changes + retrieved_visibility = children_view.get_visibility() + torch.testing.assert_close(retrieved_visibility, new_visibility) + + # Make all children visible again + children_view.set_visibility(torch.ones(num_children, dtype=torch.bool, device=device)) + all_visible = children_view.get_visibility() + assert torch.all(all_visible), "All children should be visible again" + + # Now test parent visibility inheritance: + # Make parent invisible + parent_view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device)) + + # Verify parent is invisible + parent_visibility = parent_view.get_visibility() + assert not parent_visibility[0], "Parent should be invisible" + + # Verify children are also invisible (due to parent being invisible) + children_visibility = children_view.get_visibility() + assert not torch.any(children_visibility), "All children should be invisible when parent is invisible" + + # Make parent visible again + parent_view.set_visibility(torch.tensor([True], dtype=torch.bool, device=device)) + + # Verify parent is visible + parent_visibility = parent_view.get_visibility() + assert parent_visibility[0], "Parent should be visible again" + + # Verify children are also visible again + children_visibility = children_view.get_visibility() + assert torch.all(children_visibility), "All children should be visible again when parent is visible" + + +""" +Tests - Comparison with Isaac Sim Implementation. +""" + + +def test_compare_get_world_poses_with_isaacsim(): + """Compare get_world_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXformPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create prims with various poses + num_prims = 10 + for i in range(num_prims): + pos = (i * 2.0, i * 0.5, i * 1.5) + # Vary orientations + if i % 3 == 0: + quat = (1.0, 0.0, 0.0, 0.0) # Identity + elif i % 3 == 1: + quat = (0.7071068, 0.0, 0.0, 0.7071068) # 90 deg around Z + else: + quat = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=pos, orientation=quat, stage=stage) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) + + # Get world poses from both + isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() + isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_pos, torch.Tensor): + isaacsim_pos = torch.tensor(isaacsim_pos, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results + torch.testing.assert_close(isaaclab_pos, isaacsim_pos, atol=1e-5, rtol=0) + + # Compare quaternions (account for sign ambiguity) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-5, rtol=0) + + +def test_compare_set_world_poses_with_isaacsim(): + """Compare set_world_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXformPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create prims + num_prims = 8 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) + + # Generate new poses + new_positions = torch.randn(num_prims, 3) * 10.0 + new_orientations = torch.tensor([[1.0, 0.0, 0.0, 0.0]] * num_prims, dtype=torch.float32) + + # Set poses using both implementations + isaaclab_view.set_world_poses(new_positions.clone(), new_orientations.clone()) + isaacsim_view.set_world_poses(new_positions.clone(), new_orientations.clone()) + + # Get poses back from both + isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() + isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_pos, torch.Tensor): + isaacsim_pos = torch.tensor(isaacsim_pos, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results - both implementations should produce the same world poses + torch.testing.assert_close(isaaclab_pos, isaacsim_pos, atol=1e-4, rtol=0) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) + + +def test_compare_get_local_poses_with_isaacsim(): + """Compare get_local_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXformPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create hierarchical prims + num_prims = 5 + for i in range(num_prims): + # Create parent + sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 5.0, 0.0, 0.0), stage=stage) + # Create child with local pose + local_pos = (1.0, float(i), 0.0) + local_quat = (1.0, 0.0, 0.0, 0.0) if i % 2 == 0 else (0.7071068, 0.0, 0.0, 0.7071068) + sim_utils.create_prim( + f"/World/Env_{i}/Object", "Xform", translation=local_pos, orientation=local_quat, stage=stage + ) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) + + # Get local poses from both + isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() + isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_trans, torch.Tensor): + isaacsim_trans = torch.tensor(isaacsim_trans, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results + torch.testing.assert_close(isaaclab_trans, isaacsim_trans, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-5, rtol=0) + + +def test_compare_set_local_poses_with_isaacsim(): + """Compare set_local_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXformPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create hierarchical prims + num_prims = 6 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0.0, 0.0), stage=stage) + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) + + # Generate new local poses + new_translations = torch.randn(num_prims, 3) * 5.0 + new_orientations = torch.tensor( + [[1.0, 0.0, 0.0, 0.0], [0.7071068, 0.0, 0.0, 0.7071068]] * (num_prims // 2), dtype=torch.float32 + ) + + # Set local poses using both implementations + isaaclab_view.set_local_poses(new_translations.clone(), new_orientations.clone()) + isaacsim_view.set_local_poses(new_translations.clone(), new_orientations.clone()) + + # Get local poses back from both + isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() + isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_trans, torch.Tensor): + isaacsim_trans = torch.tensor(isaacsim_trans, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results + torch.testing.assert_close(isaaclab_trans, isaacsim_trans, atol=1e-4, rtol=0) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) + + +""" +Tests - Fabric Operations. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_fabric_initialization(device): + """Test XformPrimView initialization with Fabric enabled.""" + _skip_if_backend_unavailable("fabric", device) + + stage = sim_utils.get_current_stage() + + # Create camera prims (Boundable prims that support Fabric) + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Cam_{i}", "Camera", translation=(i * 1.0, 0.0, 1.0), stage=stage) + + # Create view with Fabric enabled + view = _create_view("/World/Cam_.*", device=device, backend="fabric") + + # Verify properties + assert view.count == num_prims + assert view.device == device + assert len(view.prims) == num_prims + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_fabric_usd_consistency(device): + """Test that Fabric round-trip (writeโ†’read) is consistent, matching Isaac Sim's design. + + Note: This does NOT test Fabric vs USD reads on initialization, as Fabric is designed + for write-first workflows. Instead, it tests that: + 1. Fabric writeโ†’read round-trip works correctly + 2. This matches Isaac Sim's Fabric behavior + """ + _skip_if_backend_unavailable("fabric", device) + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim( + f"/World/Cam_{i}", + "Camera", + translation=(i * 1.0, 2.0, 3.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), + stage=stage, + ) + + # Create Fabric view + view_fabric = _create_view("/World/Cam_.*", device=device, backend="fabric") + + # Test Fabric writeโ†’read round-trip (Isaac Sim's intended workflow) + # Initialize Fabric state by WRITING first + init_positions = torch.zeros((num_prims, 3), dtype=torch.float32, device=device) + init_positions[:, 0] = torch.arange(num_prims, dtype=torch.float32, device=device) + init_positions[:, 1] = 2.0 + init_positions[:, 2] = 3.0 + init_orientations = torch.tensor([[0.7071068, 0.0, 0.0, 0.7071068]] * num_prims, dtype=torch.float32, device=device) + + view_fabric.set_world_poses(init_positions, init_orientations) + + # Read back from Fabric (should match what we wrote) + pos_fabric, quat_fabric = view_fabric.get_world_poses() + torch.testing.assert_close(pos_fabric, init_positions, atol=1e-4, rtol=0) + torch.testing.assert_close(quat_fabric, init_orientations, atol=1e-4, rtol=0) + + # Test another round-trip with different values + new_positions = torch.rand((num_prims, 3), dtype=torch.float32, device=device) * 10.0 + new_orientations = torch.tensor([[1.0, 0.0, 0.0, 0.0]] * num_prims, dtype=torch.float32, device=device) + + view_fabric.set_world_poses(new_positions, new_orientations) + + # Read back from Fabric (should match) + pos_fabric_after, quat_fabric_after = view_fabric.get_world_poses() + torch.testing.assert_close(pos_fabric_after, new_positions, atol=1e-4, rtol=0) + torch.testing.assert_close(quat_fabric_after, new_orientations, atol=1e-4, rtol=0) diff --git a/source/isaaclab/test/terrains/check_height_field_subterrains.py b/source/isaaclab/test/terrains/check_height_field_subterrains.py index d6cb7a093cc2..972d4dc22884 100644 --- a/source/isaaclab/test/terrains/check_height_field_subterrains.py +++ b/source/isaaclab/test/terrains/check_height_field_subterrains.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -17,12 +17,12 @@ # launch omniverse app # note: we only need to do this because of `TerrainImporter` which uses Omniverse functions -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import os + import trimesh import isaaclab.terrains.height_field as hf_gen diff --git a/source/isaaclab/test/terrains/check_mesh_subterrains.py b/source/isaaclab/test/terrains/check_mesh_subterrains.py index 270f42184063..593b00e8fa23 100644 --- a/source/isaaclab/test/terrains/check_mesh_subterrains.py +++ b/source/isaaclab/test/terrains/check_mesh_subterrains.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -17,13 +17,13 @@ # launch omniverse app # note: we only need to do this because of `TerrainImporter` which uses Omniverse functions -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import argparse import os + import trimesh import isaaclab.terrains.trimesh as mesh_gen @@ -341,7 +341,7 @@ def test_repeated_objects_terrain( cfg = mesh_gen.MeshRepeatedPyramidsTerrainCfg( size=(8.0, 8.0), platform_width=1.5, - max_height_noise=0.5, + abs_height_noise=(-0.5, 0.5), object_params_start=mesh_gen.MeshRepeatedPyramidsTerrainCfg.ObjectCfg( num_objects=40, height=0.05, radius=0.6, max_yx_angle=0.0, degrees=True ), @@ -353,7 +353,7 @@ def test_repeated_objects_terrain( cfg = mesh_gen.MeshRepeatedBoxesTerrainCfg( size=(8.0, 8.0), platform_width=1.5, - max_height_noise=0.5, + abs_height_noise=(-0.5, 0.5), object_params_start=mesh_gen.MeshRepeatedBoxesTerrainCfg.ObjectCfg( num_objects=40, height=0.05, size=(0.6, 0.6), max_yx_angle=0.0, degrees=True ), @@ -365,7 +365,7 @@ def test_repeated_objects_terrain( cfg = mesh_gen.MeshRepeatedCylindersTerrainCfg( size=(8.0, 8.0), platform_width=1.5, - max_height_noise=0.5, + abs_height_noise=(-0.5, 0.5), object_params_start=mesh_gen.MeshRepeatedCylindersTerrainCfg.ObjectCfg( num_objects=40, height=0.05, radius=0.6, max_yx_angle=0.0, degrees=True ), diff --git a/source/isaaclab/test/terrains/check_terrain_importer.py b/source/isaaclab/test/terrains/check_terrain_importer.py index 2a8087ef87b4..d88ec65c86d0 100644 --- a/source/isaaclab/test/terrains/check_terrain_importer.py +++ b/source/isaaclab/test/terrains/check_terrain_importer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -64,19 +64,18 @@ import numpy as np -import isaacsim.core.utils.prims as prim_utils import omni.kit import omni.kit.commands -from isaacsim.core.api.simulation_context import SimulationContext +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.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 import isaaclab.sim as sim_utils import isaaclab.terrains as terrain_gen +from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG from isaaclab.terrains.terrain_importer import TerrainImporter from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -88,18 +87,9 @@ def main(): """Generates a terrain from isaaclab.""" # Load kit helper - sim_params = { - "use_gpu": True, - "use_gpu_pipeline": True, - "use_flatcache": True, - "use_fabric": True, - "enable_scene_query_support": True, - } - sim = SimulationContext( - physics_dt=1.0 / 60.0, rendering_dt=1.0 / 60.0, sim_params=sim_params, backend="torch", device="cuda:0" - ) + sim = SimulationContext(SimulationCfg()) # Set main camera - set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5]) + sim.set_camera_view(eye=(0.0, 30.0, 25.0), target=(0.0, 0.0, -2.5)) # Parameters num_balls = 2048 @@ -108,7 +98,7 @@ def main(): cloner = GridCloner(spacing=2.0) cloner.define_base_env("/World/envs") # Everything under the namespace "/World/envs/env_0" will be cloned - prim_utils.define_prim("/World/envs/env_0") + sim_utils.define_prim("/World/envs/env_0") # Handler for terrains importing terrain_importer_cfg = terrain_gen.TerrainImporterCfg( @@ -135,7 +125,7 @@ def main(): else: # -- Ball geometry cube_prim_path = omni.kit.commands.execute("CreateMeshPrimCommand", prim_type="Sphere")[1] - prim_utils.move_prim(cube_prim_path, "/World/envs/env_0/ball") + sim_utils.move_prim(cube_prim_path, "/World/envs/env_0/ball") # -- Ball physics SingleRigidPrim( prim_path="/World/envs/env_0/ball", mass=0.5, scale=(0.5, 0.5, 0.5), translation=(0.0, 0.0, 0.5) diff --git a/source/isaaclab/test/terrains/test_terrain_generator.py b/source/isaaclab/test/terrains/test_terrain_generator.py index f0f2f84dcb43..804176458cb5 100644 --- a/source/isaaclab/test/terrains/test_terrain_generator.py +++ b/source/isaaclab/test/terrains/test_terrain_generator.py @@ -1,170 +1,162 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import numpy as np import os import shutil -import torch -import unittest -import isaacsim.core.utils.torch as torch_utils +import numpy as np +import pytest +import torch from isaaclab.terrains import FlatPatchSamplingCfg, TerrainGenerator, TerrainGeneratorCfg from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG - - -class TestTerrainGenerator(unittest.TestCase): - """Test the procedural terrain generator.""" - - def setUp(self): - # Create directory to dump results - test_dir = os.path.dirname(os.path.abspath(__file__)) - self.output_dir = os.path.join(test_dir, "output", "generator") - - def test_generation(self): - """Generates assorted terrains and tests that the resulting mesh has the expected size.""" - # create terrain generator - cfg = ROUGH_TERRAINS_CFG.copy() - terrain_generator = TerrainGenerator(cfg=cfg) - - # print terrain generator info - print(terrain_generator) - - # get size from mesh bounds - bounds = terrain_generator.terrain_mesh.bounds - actualSize = abs(bounds[1] - bounds[0]) - # compute the expected size - expectedSizeX = cfg.size[0] * cfg.num_rows + 2 * cfg.border_width - expectedSizeY = cfg.size[1] * cfg.num_cols + 2 * cfg.border_width - - # check if the size is as expected - self.assertAlmostEqual(actualSize[0], expectedSizeX) - self.assertAlmostEqual(actualSize[1], expectedSizeY) - - def test_generation_reproducibility(self): - """Generates assorted terrains and tests that the resulting mesh is reproducible. - - We check both scenarios where the seed is set globally only and when it is set both globally and locally. - Setting only locally is not tested as it is not supported. - """ - for use_global_seed in [True, False]: - for seed in [20, 40, 80]: - with self.subTest(seed=seed): - # set initial seed - torch_utils.set_seed(seed) - - # create terrain generator - cfg = ROUGH_TERRAINS_CFG.copy() - cfg.use_cache = False - cfg.seed = seed if use_global_seed else None - terrain_generator = TerrainGenerator(cfg=cfg) - - # keep a copy of the generated terrain mesh - terrain_mesh_1 = terrain_generator.terrain_mesh.copy() - - # set seed again - torch_utils.set_seed(seed) - - # create terrain generator - terrain_generator = TerrainGenerator(cfg=cfg) - - # keep a copy of the generated terrain mesh - terrain_mesh_2 = terrain_generator.terrain_mesh.copy() - - # check if the meshes are equal - np.testing.assert_allclose( - terrain_mesh_1.vertices, terrain_mesh_2.vertices, atol=1e-5, err_msg="Vertices are not equal" - ) - np.testing.assert_allclose( - terrain_mesh_1.faces, terrain_mesh_2.faces, atol=1e-5, err_msg="Faces are not equal" - ) - - def test_generation_cache(self): - """Generate the terrain and check that caching works. - - When caching is enabled, the terrain should be generated only once and the same terrain should be returned - when the terrain generator is created again. - """ - # try out with and without curriculum - for curriculum in [True, False]: - with self.subTest(curriculum=curriculum): - # clear output directory - if os.path.exists(self.output_dir): - shutil.rmtree(self.output_dir) - # create terrain generator with cache enabled - cfg: TerrainGeneratorCfg = ROUGH_TERRAINS_CFG.copy() - cfg.use_cache = True - cfg.seed = 0 - cfg.cache_dir = self.output_dir - cfg.curriculum = curriculum - terrain_generator = TerrainGenerator(cfg=cfg) - # keep a copy of the generated terrain mesh - terrain_mesh_1 = terrain_generator.terrain_mesh.copy() - - # check cache exists and is equal to the number of terrains - # with curriculum, all sub-terrains are uniquely generated - hash_ids_1 = set(os.listdir(cfg.cache_dir)) - self.assertTrue(os.listdir(cfg.cache_dir)) - - # set a random seed to disturb the process - # this is to ensure that the seed inside the terrain generator makes deterministic results - torch_utils.set_seed(12456) - - # create terrain generator with cache enabled - terrain_generator = TerrainGenerator(cfg=cfg) - # keep a copy of the generated terrain mesh - terrain_mesh_2 = terrain_generator.terrain_mesh.copy() - - # check no new terrain is generated - hash_ids_2 = set(os.listdir(cfg.cache_dir)) - self.assertEqual(len(hash_ids_1), len(hash_ids_2)) - self.assertSetEqual(hash_ids_1, hash_ids_2) - - # check if the mesh is the same - # check they don't point to the same object - self.assertIsNot(terrain_mesh_1, terrain_mesh_2) - - # check if the meshes are equal - np.testing.assert_allclose( - terrain_mesh_1.vertices, terrain_mesh_2.vertices, atol=1e-5, err_msg="Vertices are not equal" - ) - np.testing.assert_allclose( - terrain_mesh_1.faces, terrain_mesh_2.faces, atol=1e-5, err_msg="Faces are not equal" - ) - - def test_terrain_flat_patches(self): - """Test the flat patches generation.""" - # create terrain generator - cfg = ROUGH_TERRAINS_CFG.copy() - # add flat patch configuration - for _, sub_terrain_cfg in cfg.sub_terrains.items(): - sub_terrain_cfg.flat_patch_sampling = { - "root_spawn": FlatPatchSamplingCfg(num_patches=8, patch_radius=0.5, max_height_diff=0.05), - "target_spawn": FlatPatchSamplingCfg(num_patches=5, patch_radius=0.35, max_height_diff=0.05), - } - # generate terrain - terrain_generator = TerrainGenerator(cfg=cfg) - - # check if flat patches are generated - self.assertTrue(terrain_generator.flat_patches) - # check the size of the flat patches - self.assertTupleEqual(terrain_generator.flat_patches["root_spawn"].shape, (cfg.num_rows, cfg.num_cols, 8, 3)) - self.assertTupleEqual(terrain_generator.flat_patches["target_spawn"].shape, (cfg.num_rows, cfg.num_cols, 5, 3)) - # check that no flat patches are zero - for _, flat_patches in terrain_generator.flat_patches.items(): - self.assertFalse(torch.allclose(flat_patches, torch.zeros_like(flat_patches))) - - -if __name__ == "__main__": - run_tests() +from isaaclab.utils.seed import configure_seed + + +@pytest.fixture +def output_dir(): + """Create directory to dump results.""" + test_dir = os.path.dirname(os.path.abspath(__file__)) + output_dir = os.path.join(test_dir, "output", "generator") + yield output_dir + # Cleanup + if os.path.exists(output_dir): + shutil.rmtree(output_dir) + + +def test_generation(output_dir): + """Generates assorted terrains and tests that the resulting mesh has the expected size.""" + # create terrain generator + cfg = ROUGH_TERRAINS_CFG + terrain_generator = TerrainGenerator(cfg=cfg) + + # print terrain generator info + print(terrain_generator) + + # get size from mesh bounds + bounds = terrain_generator.terrain_mesh.bounds + actualSize = abs(bounds[1] - bounds[0]) + # compute the expected size + expectedSizeX = cfg.size[0] * cfg.num_rows + 2 * cfg.border_width + expectedSizeY = cfg.size[1] * cfg.num_cols + 2 * cfg.border_width + + # check if the size is as expected + assert actualSize[0] == pytest.approx(expectedSizeX) + assert actualSize[1] == pytest.approx(expectedSizeY) + + +@pytest.mark.parametrize("use_global_seed", [True, False]) +@pytest.mark.parametrize("seed", [20, 40, 80]) +def test_generation_reproducibility(use_global_seed, seed): + """Generates assorted terrains and tests that the resulting mesh is reproducible. + + We check both scenarios where the seed is set globally only and when it is set both globally and locally. + Setting only locally is not tested as it is not supported. + """ + # set initial seed + configure_seed(seed) + + # create terrain generator + cfg = ROUGH_TERRAINS_CFG + cfg.use_cache = False + cfg.seed = seed if use_global_seed else None + terrain_generator = TerrainGenerator(cfg=cfg) + + # keep a copy of the generated terrain mesh + terrain_mesh_1 = terrain_generator.terrain_mesh.copy() + + # set seed again + configure_seed(seed) + + # create terrain generator + terrain_generator = TerrainGenerator(cfg=cfg) + + # keep a copy of the generated terrain mesh + terrain_mesh_2 = terrain_generator.terrain_mesh.copy() + + # check if the meshes are equal + np.testing.assert_allclose( + terrain_mesh_1.vertices, terrain_mesh_2.vertices, atol=1e-5, err_msg="Vertices are not equal" + ) + np.testing.assert_allclose(terrain_mesh_1.faces, terrain_mesh_2.faces, atol=1e-5, err_msg="Faces are not equal") + + +@pytest.mark.parametrize("curriculum", [True, False]) +def test_generation_cache(output_dir, curriculum): + """Generate the terrain and check that caching works. + + When caching is enabled, the terrain should be generated only once and the same terrain should be returned + when the terrain generator is created again. + """ + # create terrain generator with cache enabled + cfg: TerrainGeneratorCfg = ROUGH_TERRAINS_CFG + cfg.use_cache = True + cfg.seed = 0 + cfg.cache_dir = output_dir + cfg.curriculum = curriculum + terrain_generator = TerrainGenerator(cfg=cfg) + # keep a copy of the generated terrain mesh + terrain_mesh_1 = terrain_generator.terrain_mesh.copy() + + # check cache exists and is equal to the number of terrains + # with curriculum, all sub-terrains are uniquely generated + hash_ids_1 = set(os.listdir(cfg.cache_dir)) + assert os.listdir(cfg.cache_dir) + + # set a random seed to disturb the process + # this is to ensure that the seed inside the terrain generator makes deterministic results + configure_seed(12456) + + # create terrain generator with cache enabled + terrain_generator = TerrainGenerator(cfg=cfg) + # keep a copy of the generated terrain mesh + terrain_mesh_2 = terrain_generator.terrain_mesh.copy() + + # check no new terrain is generated + hash_ids_2 = set(os.listdir(cfg.cache_dir)) + assert len(hash_ids_1) == len(hash_ids_2) + assert hash_ids_1 == hash_ids_2 + + # check if the mesh is the same + # check they don't point to the same object + assert terrain_mesh_1 is not terrain_mesh_2 + + # check if the meshes are equal + np.testing.assert_allclose( + terrain_mesh_1.vertices, terrain_mesh_2.vertices, atol=1e-5, err_msg="Vertices are not equal" + ) + np.testing.assert_allclose(terrain_mesh_1.faces, terrain_mesh_2.faces, atol=1e-5, err_msg="Faces are not equal") + + +def test_terrain_flat_patches(): + """Test the flat patches generation.""" + # create terrain generator + cfg = ROUGH_TERRAINS_CFG + # add flat patch configuration + for _, sub_terrain_cfg in cfg.sub_terrains.items(): + sub_terrain_cfg.flat_patch_sampling = { + "root_spawn": FlatPatchSamplingCfg(num_patches=8, patch_radius=0.5, max_height_diff=0.05), + "target_spawn": FlatPatchSamplingCfg(num_patches=5, patch_radius=0.35, max_height_diff=0.05), + } + # generate terrain + terrain_generator = TerrainGenerator(cfg=cfg) + + # check if flat patches are generated + assert terrain_generator.flat_patches + # check the size of the flat patches + assert terrain_generator.flat_patches["root_spawn"].shape == (cfg.num_rows, cfg.num_cols, 8, 3) + assert terrain_generator.flat_patches["target_spawn"].shape == (cfg.num_rows, cfg.num_cols, 5, 3) + # check that no flat patches are zero + for _, flat_patches in terrain_generator.flat_patches.items(): + assert not torch.allclose(flat_patches, torch.zeros_like(flat_patches)) diff --git a/source/isaaclab/test/terrains/test_terrain_importer.py b/source/isaaclab/test/terrains/test_terrain_importer.py index c708ad9b0932..05ed76e0811e 100644 --- a/source/isaaclab/test/terrains/test_terrain_importer.py +++ b/source/isaaclab/test/terrains/test_terrain_importer.py @@ -1,24 +1,24 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" +from typing import Literal + import numpy as np +import pytest import torch import trimesh -import unittest -from typing import Literal -import isaacsim.core.utils.prims as prim_utils import omni.kit import omni.kit.commands from isaacsim.core.api.materials import PhysicsMaterial, PreviewSurface @@ -26,8 +26,9 @@ from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import RigidPrim, SingleGeometryPrim, SingleRigidPrim from isaacsim.core.utils.extensions import enable_extension -from pxr import UsdGeom +from pxr import Usd, UsdGeom +import isaaclab.sim as sim_utils import isaaclab.terrains as terrain_gen from isaaclab.sim import PreviewSurfaceCfg, SimulationContext, build_simulation_context, get_first_matching_child_prim from isaaclab.terrains import TerrainImporter, TerrainImporterCfg @@ -35,312 +36,299 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR -class TestTerrainImporter(unittest.TestCase): - """Test the terrain importer for different ground and procedural terrains.""" - - def test_grid_clone_env_origins(self): - """Tests that env origins are consistent when computed using the TerrainImporter and IsaacSim GridCloner.""" - # iterate over different number of environments and environment spacing - for device in ("cuda:0", "cpu"): - for env_spacing in [1.0, 4.325, 8.0]: - for num_envs in [1, 4, 125, 379, 1024]: - with self.subTest(num_envs=num_envs, env_spacing=env_spacing): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # create terrain importer - terrain_importer_cfg = TerrainImporterCfg( - num_envs=num_envs, - env_spacing=env_spacing, - prim_path="/World/ground", - terrain_type="plane", # for flat ground, origins are in grid - terrain_generator=None, - ) - terrain_importer = TerrainImporter(terrain_importer_cfg) - # obtain env origins using terrain importer - terrain_importer_origins = terrain_importer.env_origins - - # obtain env origins using grid cloner - grid_cloner_origins = self._obtain_grid_cloner_env_origins( - num_envs, env_spacing, device=sim.device - ) - - # check if the env origins are the same - torch.testing.assert_close( - terrain_importer_origins, grid_cloner_origins, rtol=1e-5, atol=1e-5 - ) - - def test_terrain_generation(self) -> None: - """Generates assorted terrains and tests that the resulting mesh has the correct size.""" - for device in ("cuda:0", "cpu"): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Handler for terrains importing - terrain_importer_cfg = terrain_gen.TerrainImporterCfg( - prim_path="/World/ground", - max_init_terrain_level=None, - terrain_type="generator", - terrain_generator=ROUGH_TERRAINS_CFG.replace(curriculum=True), - num_envs=1, - ) - terrain_importer = TerrainImporter(terrain_importer_cfg) - - # check if mesh prim path exists - mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" - self.assertIn(mesh_prim_path, terrain_importer.terrain_prim_paths) - - # obtain underling mesh - mesh = self._obtain_collision_mesh(mesh_prim_path, mesh_type="Mesh") - self.assertIsNotNone(mesh) - - # calculate expected size from config - cfg = terrain_importer.cfg.terrain_generator - self.assertIsNotNone(cfg) - expectedSizeX = cfg.size[0] * cfg.num_rows + 2 * cfg.border_width - expectedSizeY = cfg.size[1] * cfg.num_cols + 2 * cfg.border_width - - # get size from mesh bounds - bounds = mesh.bounds - actualSize = abs(bounds[1] - bounds[0]) - - self.assertAlmostEqual(actualSize[0], expectedSizeX) - self.assertAlmostEqual(actualSize[1], expectedSizeY) - - def test_plane(self) -> None: - """Generates a plane and tests that the resulting mesh has the correct size.""" - for device in ("cuda:0", "cpu"): - for use_custom_material in [True, False]: - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - - # create custom material - visual_material = PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)) if use_custom_material else None - # Handler for terrains importing - terrain_importer_cfg = terrain_gen.TerrainImporterCfg( - prim_path="/World/ground", - terrain_type="plane", - num_envs=1, - env_spacing=1.0, - visual_material=visual_material, - ) - terrain_importer = TerrainImporter(terrain_importer_cfg) - - # check if mesh prim path exists - mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" - self.assertIn(mesh_prim_path, terrain_importer.terrain_prim_paths) - - # obtain underling mesh - mesh = self._obtain_collision_mesh(mesh_prim_path, mesh_type="Plane") - self.assertIsNone(mesh) - - def test_usd(self) -> None: - """Imports terrain from a usd and tests that the resulting mesh has the correct size.""" - for device in ("cuda:0", "cpu"): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Handler for terrains importing - terrain_importer_cfg = terrain_gen.TerrainImporterCfg( - prim_path="/World/ground", - terrain_type="usd", - usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd", - num_envs=1, - env_spacing=1.0, - ) - terrain_importer = TerrainImporter(terrain_importer_cfg) - - # check if mesh prim path exists - mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" - self.assertIn(mesh_prim_path, terrain_importer.terrain_prim_paths) - - # obtain underling mesh - mesh = self._obtain_collision_mesh(mesh_prim_path, mesh_type="Mesh") - self.assertIsNotNone(mesh) - - # expect values from USD file - expectedSizeX = 96 - expectedSizeY = 96 - - # get size from mesh bounds - bounds = mesh.bounds - actualSize = abs(bounds[1] - bounds[0]) - - self.assertAlmostEqual(actualSize[0], expectedSizeX) - self.assertAlmostEqual(actualSize[1], expectedSizeY) - - def test_ball_drop(self) -> None: - """Generates assorted terrains and spheres created as meshes. - - Tests that spheres fall onto terrain and do not pass through it. This ensures that the triangle mesh - collision works as expected. - """ - for device in ("cuda:0", "cpu"): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with rough terrain and balls - self._populate_scene(geom_sphere=False, sim=sim) - - # Create a view over all the balls - ball_view = RigidPrim("/World/envs/env_.*/ball", reset_xform_properties=False) - - # Play simulator - sim.reset() - # Initialize the ball views for physics simulation - ball_view.initialize() - - # Run simulator - for _ in range(500): - sim.step(render=False) - - # Ball may have some small non-zero velocity if the roll on terrain <~.2 - # If balls fall through terrain velocity is much higher ~82.0 - max_velocity_z = torch.max(torch.abs(ball_view.get_linear_velocities()[:, 2])) - self.assertLessEqual(max_velocity_z.item(), 0.5) - - def test_ball_drop_geom_sphere(self) -> None: - """Generates assorted terrains and geom spheres. - - Tests that spheres fall onto terrain and do not pass through it. This ensures that the sphere collision - works as expected. - """ - for device in ("cuda:0", "cpu"): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with rough terrain and balls - # TODO: Currently the test fails with geom spheres, need to investigate with the PhysX team. - # Setting the geom_sphere as False to pass the test. This test should be enabled once - # the issue is fixed. - self._populate_scene(geom_sphere=False, sim=sim) - - # Create a view over all the balls - ball_view = RigidPrim("/World/envs/env_.*/ball", reset_xform_properties=False) - - # Play simulator - sim.reset() - # Initialize the ball views for physics simulation - ball_view.initialize() - - # Run simulator - for _ in range(500): - sim.step(render=False) - - # Ball may have some small non-zero velocity if the roll on terrain <~.2 - # If balls fall through terrain velocity is much higher ~82.0 - max_velocity_z = torch.max(torch.abs(ball_view.get_linear_velocities()[:, 2])) - self.assertLessEqual(max_velocity_z.item(), 0.5) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("env_spacing", [1.0, 4.325, 8.0]) +@pytest.mark.parametrize("num_envs", [1, 4, 125, 379, 1024]) +def test_grid_clone_env_origins(device, env_spacing, num_envs): + """Tests that env origins are consistent when computed using the TerrainImporter and IsaacSim GridCloner.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # create terrain importer + terrain_importer_cfg = TerrainImporterCfg( + num_envs=num_envs, + env_spacing=env_spacing, + prim_path="/World/ground", + terrain_type="plane", # for flat ground, origins are in grid + terrain_generator=None, + ) + terrain_importer = TerrainImporter(terrain_importer_cfg) + # obtain env origins using terrain importer + terrain_importer_origins = terrain_importer.env_origins - """ - Helper functions. - """ + # obtain env origins using grid cloner + grid_cloner_origins = _obtain_grid_cloner_env_origins(num_envs, env_spacing, stage=sim.stage, device=sim.device) - def _obtain_collision_mesh( - self, mesh_prim_path: str, mesh_type: Literal["Mesh", "Plane"] - ) -> trimesh.Trimesh | None: - """Get the collision mesh from the terrain.""" - # traverse the prim and get the collision mesh - mesh_prim = get_first_matching_child_prim(mesh_prim_path, lambda prim: prim.GetTypeName() == mesh_type) - # check it is valid - self.assertTrue(mesh_prim.IsValid()) - - if mesh_prim.GetTypeName() == "Mesh": - # cast into UsdGeomMesh - mesh_prim = UsdGeom.Mesh(mesh_prim) - # store the mesh - vertices = np.asarray(mesh_prim.GetPointsAttr().Get()) - faces = np.asarray(mesh_prim.GetFaceVertexIndicesAttr().Get()).reshape(-1, 3) - return trimesh.Trimesh(vertices=vertices, faces=faces) - else: - return None - - @staticmethod - def _obtain_grid_cloner_env_origins(num_envs: int, env_spacing: float, device: str) -> torch.Tensor: - """Obtain the env origins generated by IsaacSim GridCloner (grid_cloner.py).""" - # create grid cloner - cloner = GridCloner(spacing=env_spacing) - cloner.define_base_env("/World/envs") - envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) - prim_utils.define_prim("/World/envs/env_0") - # clone envs using grid cloner - env_origins = cloner.clone( - source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True - ) - # return as tensor - return torch.tensor(env_origins, dtype=torch.float32, device=device) + # check if the env origins are the same + torch.testing.assert_close(terrain_importer_origins, grid_cloner_origins, rtol=1e-5, atol=1e-5) - def _populate_scene(self, sim: SimulationContext, num_balls: int = 2048, geom_sphere: bool = False): - """Create a scene with terrain and randomly spawned balls. - The spawned balls are either USD Geom Spheres or are USD Meshes. We check against both these to make sure - both USD-shape and USD-mesh collisions work as expected. - """ +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_terrain_generation(device): + """Generates assorted terrains and tests that the resulting mesh has the correct size.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None # Handler for terrains importing terrain_importer_cfg = terrain_gen.TerrainImporterCfg( prim_path="/World/ground", max_init_terrain_level=None, terrain_type="generator", - terrain_generator=ROUGH_TERRAINS_CFG.replace(curriculum=True), - num_envs=num_balls, + terrain_generator=ROUGH_TERRAINS_CFG, + num_envs=1, ) terrain_importer = TerrainImporter(terrain_importer_cfg) - # Create interface to clone the scene - cloner = GridCloner(spacing=2.0) - cloner.define_base_env("/World/envs") - # Everything under the namespace "/World/envs/env_0" will be cloned - prim_utils.define_prim(prim_path="/World/envs/env_0", prim_type="Xform") - - # Define the scene - # -- Ball - if geom_sphere: - # -- Ball physics - _ = DynamicSphere( - prim_path="/World/envs/env_0/ball", translation=np.array([0.0, 0.0, 5.0]), mass=0.5, radius=0.25 - ) - else: - # -- Ball geometry - enable_extension("omni.kit.primitive.mesh") - cube_prim_path = omni.kit.commands.execute("CreateMeshPrimCommand", prim_type="Sphere")[1] - prim_utils.move_prim(cube_prim_path, "/World/envs/env_0/ball") - # -- Ball physics - SingleRigidPrim( - prim_path="/World/envs/env_0/ball", mass=0.5, scale=(0.5, 0.5, 0.5), translation=(0.0, 0.0, 0.5) - ) - SingleGeometryPrim(prim_path="/World/envs/env_0/ball", collision=True) - - # -- Ball material - sphere_geom = SingleGeometryPrim(prim_path="/World/envs/env_0/ball", collision=True) - visual_material = PreviewSurface(prim_path="/World/Looks/ballColorMaterial", color=np.asarray([0.0, 0.0, 1.0])) - physics_material = PhysicsMaterial( - prim_path="/World/Looks/ballPhysicsMaterial", - dynamic_friction=1.0, - static_friction=0.2, - restitution=0.0, - ) - sphere_geom.set_collision_approximation("convexHull") - sphere_geom.apply_visual_material(visual_material) - sphere_geom.apply_physics_material(physics_material) - - # Clone the scene - cloner.define_base_env("/World/envs") - envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_balls) - cloner.clone( - source_prim_path="/World/envs/env_0", - prim_paths=envs_prim_paths, - replicate_physics=True, + # check if mesh prim path exists + mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" + assert mesh_prim_path in terrain_importer.terrain_prim_paths + + # obtain underling mesh + mesh = _obtain_collision_mesh(mesh_prim_path, mesh_type="Mesh") + assert mesh is not None + + # calculate expected size from config + cfg = terrain_importer.cfg.terrain_generator + assert cfg is not None + expectedSizeX = cfg.size[0] * cfg.num_rows + 2 * cfg.border_width + expectedSizeY = cfg.size[1] * cfg.num_cols + 2 * cfg.border_width + + # get size from mesh bounds + bounds = mesh.bounds + actualSize = abs(bounds[1] - bounds[0]) + + assert actualSize[0] == pytest.approx(expectedSizeX) + assert actualSize[1] == pytest.approx(expectedSizeY) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("use_custom_material", [True, False]) +def test_plane(device, use_custom_material): + """Generates a plane and tests that the resulting mesh has the correct size.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + + # create custom material + visual_material = PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)) if use_custom_material else None + # Handler for terrains importing + terrain_importer_cfg = terrain_gen.TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + num_envs=1, + env_spacing=1.0, + visual_material=visual_material, ) - physics_scene_path = sim.get_physics_context().prim_path - cloner.filter_collisions( - physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] + terrain_importer = TerrainImporter(terrain_importer_cfg) + + # check if mesh prim path exists + mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" + assert mesh_prim_path in terrain_importer.terrain_prim_paths + + # obtain underling mesh + mesh = _obtain_collision_mesh(mesh_prim_path, mesh_type="Plane") + assert mesh is None + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_usd(device): + """Imports terrain from a usd and tests that the resulting mesh has the correct size.""" + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Handler for terrains importing + terrain_importer_cfg = terrain_gen.TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd", + num_envs=1, + env_spacing=1.0, ) + terrain_importer = TerrainImporter(terrain_importer_cfg) + + # check if mesh prim path exists + mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" + assert mesh_prim_path in terrain_importer.terrain_prim_paths + + # obtain underling mesh + mesh = _obtain_collision_mesh(mesh_prim_path, mesh_type="Mesh") + assert mesh is not None + + # expect values from USD file + expectedSizeX = 96 + expectedSizeY = 96 + + # get size from mesh bounds + bounds = mesh.bounds + actualSize = abs(bounds[1] - bounds[0]) + + assert actualSize[0] == pytest.approx(expectedSizeX) + assert actualSize[1] == pytest.approx(expectedSizeY) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_ball_drop(device): + """Generates assorted terrains and spheres created as meshes. + + Tests that spheres fall onto terrain and do not pass through it. This ensures that the triangle mesh + collision works as expected. + """ + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Create a scene with rough terrain and balls + _populate_scene(geom_sphere=False, sim=sim) - # Set ball positions over terrain origins # Create a view over all the balls ball_view = RigidPrim("/World/envs/env_.*/ball", reset_xform_properties=False) - # cache initial state of the balls - ball_initial_positions = terrain_importer.env_origins - ball_initial_positions[:, 2] += 5.0 - # set initial poses - # note: setting here writes to USD :) - ball_view.set_world_poses(positions=ball_initial_positions) + # Play simulator + sim.reset() + # Initialize the ball views for physics simulation + ball_view.initialize() -if __name__ == "__main__": - run_tests() + # Run simulator + for _ in range(500): + sim.step(render=False) + + # Ball may have some small non-zero velocity if the roll on terrain <~.2 + # If balls fall through terrain velocity is much higher ~82.0 + max_velocity_z = torch.max(torch.abs(ball_view.get_linear_velocities()[:, 2])) + assert max_velocity_z.item() <= 0.5 + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_ball_drop_geom_sphere(device): + """Generates assorted terrains and geom spheres. + + Tests that spheres fall onto terrain and do not pass through it. This ensures that the sphere collision + works as expected. + """ + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # Create a scene with rough terrain and balls + # TODO: Currently the test fails with geom spheres, need to investigate with the PhysX team. + # Setting the geom_sphere as False to pass the test. This test should be enabled once + # the issue is fixed. + _populate_scene(geom_sphere=False, sim=sim) + + # Create a view over all the balls + ball_view = RigidPrim("/World/envs/env_.*/ball", reset_xform_properties=False) + + # Play simulator + sim.reset() + # Initialize the ball views for physics simulation + ball_view.initialize() + + # Run simulator + for _ in range(500): + sim.step(render=False) + + # Ball may have some small non-zero velocity if the roll on terrain <~.2 + # If balls fall through terrain velocity is much higher ~82.0 + max_velocity_z = torch.max(torch.abs(ball_view.get_linear_velocities()[:, 2])) + assert max_velocity_z.item() <= 0.5 + + +def _obtain_collision_mesh(mesh_prim_path: str, mesh_type: Literal["Mesh", "Plane"]) -> trimesh.Trimesh | None: + """Get the collision mesh from the terrain.""" + # traverse the prim and get the collision mesh + mesh_prim = get_first_matching_child_prim(mesh_prim_path, lambda prim: prim.GetTypeName() == mesh_type) + # check it is valid + assert mesh_prim.IsValid() + + if mesh_prim.GetTypeName() == "Mesh": + # cast into UsdGeomMesh + mesh_prim = UsdGeom.Mesh(mesh_prim) + # store the mesh + vertices = np.asarray(mesh_prim.GetPointsAttr().Get()) + faces = np.asarray(mesh_prim.GetFaceVertexIndicesAttr().Get()).reshape(-1, 3) + return trimesh.Trimesh(vertices=vertices, faces=faces) + else: + return None + + +def _obtain_grid_cloner_env_origins(num_envs: int, env_spacing: float, stage: Usd.Stage, device: str) -> torch.Tensor: + """Obtain the env origins generated by IsaacSim GridCloner (grid_cloner.py).""" + # create grid cloner + cloner = GridCloner(spacing=env_spacing) + cloner.define_base_env("/World/envs") + envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) + # create source prim + stage.DefinePrim("/World/envs/env_0", "Xform") + # clone envs using grid cloner + env_origins = cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) + # return as tensor + return torch.tensor(env_origins, dtype=torch.float32, device=device) + + +def _populate_scene(sim: SimulationContext, num_balls: int = 2048, geom_sphere: bool = False): + """Create a scene with terrain and randomly spawned balls. + + The spawned balls are either USD Geom Spheres or are USD Meshes. We check against both these to make sure + both USD-shape and USD-mesh collisions work as expected. + """ + # Handler for terrains importing + terrain_importer_cfg = terrain_gen.TerrainImporterCfg( + prim_path="/World/ground", + max_init_terrain_level=None, + terrain_type="generator", + terrain_generator=ROUGH_TERRAINS_CFG, + num_envs=num_balls, + ) + terrain_importer = TerrainImporter(terrain_importer_cfg) + + # Create interface to clone the scene + cloner = GridCloner(spacing=2.0) + cloner.define_base_env("/World/envs") + # Everything under the namespace "/World/envs/env_0" will be cloned + sim.stage.DefinePrim("/World/envs/env_0", "Xform") + + # Define the scene + # -- Ball + if geom_sphere: + # -- Ball physics + _ = DynamicSphere( + prim_path="/World/envs/env_0/ball", translation=np.array([0.0, 0.0, 5.0]), mass=0.5, radius=0.25 + ) + else: + # -- Ball geometry + enable_extension("omni.kit.primitive.mesh") + cube_prim_path = omni.kit.commands.execute("CreateMeshPrimCommand", prim_type="Sphere")[1] + sim_utils.move_prim(cube_prim_path, "/World/envs/env_0/ball") + # -- Ball physics + SingleRigidPrim( + prim_path="/World/envs/env_0/ball", mass=0.5, scale=(0.5, 0.5, 0.5), translation=(0.0, 0.0, 0.5) + ) + SingleGeometryPrim(prim_path="/World/envs/env_0/ball", collision=True) + + # -- Ball material + sphere_geom = SingleGeometryPrim(prim_path="/World/envs/env_0/ball", collision=True) + visual_material = PreviewSurface(prim_path="/World/Looks/ballColorMaterial", color=np.asarray([0.0, 0.0, 1.0])) + physics_material = PhysicsMaterial( + prim_path="/World/Looks/ballPhysicsMaterial", + dynamic_friction=1.0, + static_friction=0.2, + restitution=0.0, + ) + sphere_geom.set_collision_approximation("convexHull") + sphere_geom.apply_visual_material(visual_material) + sphere_geom.apply_physics_material(physics_material) + + # Clone the scene + cloner.define_base_env("/World/envs") + envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_balls) + cloner.clone( + source_prim_path="/World/envs/env_0", + prim_paths=envs_prim_paths, + replicate_physics=True, + ) + physics_scene_path = sim.get_physics_context().prim_path + cloner.filter_collisions( + physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] + ) + + # Set ball positions over terrain origins + # Create a view over all the balls + ball_view = RigidPrim("/World/envs/env_.*/ball", reset_xform_properties=False) + # cache initial state of the balls + ball_initial_positions = terrain_importer.env_origins + ball_initial_positions[:, 2] += 5.0 + # set initial poses + # note: setting here writes to USD :) + ball_view.set_world_poses(positions=ball_initial_positions) diff --git a/source/isaaclab/test/utils/test_assets.py b/source/isaaclab/test/utils/test_assets.py index 17a81a8a1467..483c7d93d9fe 100644 --- a/source/isaaclab/test/utils/test_assets.py +++ b/source/isaaclab/test/utils/test_assets.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,41 +7,46 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import unittest - import isaaclab.utils.assets as assets_utils -class TestAssetsUtils(unittest.TestCase): - """Test cases for the assets utility functions.""" +def test_nucleus_connection(): + """Test checking the Nucleus connection.""" + # check nucleus connection + assert assets_utils.NUCLEUS_ASSET_ROOT_DIR is not None + + +def test_check_file_path_nucleus(): + """Test checking a file path on the Nucleus server.""" + # robot file path + usd_path = f"{assets_utils.ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + # check file path + assert assets_utils.check_file_path(usd_path) == 2 - def test_nucleus_connection(self): - """Test checking the Nucleus connection.""" - # check nucleus connection - self.assertIsNotNone(assets_utils.NUCLEUS_ASSET_ROOT_DIR) - def test_check_file_path_nucleus(self): - """Test checking a file path on the Nucleus server.""" - # robot file path - usd_path = f"{assets_utils.ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" - # check file path - self.assertEqual(assets_utils.check_file_path(usd_path), 2) +def test_check_file_path_invalid(): + """Test checking an invalid file path.""" + # robot file path + usd_path = f"{assets_utils.ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_xyz.usd" + # check file path + assert assets_utils.check_file_path(usd_path) == 0 - def test_check_file_path_invalid(self): - """Test checking an invalid file path.""" - # robot file path - usd_path = f"{assets_utils.ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_xyz.usd" - # check file path - self.assertEqual(assets_utils.check_file_path(usd_path), 0) +def test_check_usd_path_with_timeout(): + """Test checking a USD path with timeout.""" + # robot file path + usd_path = f"{assets_utils.ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + # check file path + assert assets_utils.check_usd_path_with_timeout(usd_path) is True -if __name__ == "__main__": - run_tests() + # invalid file path + usd_path = f"{assets_utils.ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_xyz.usd" + # check file path + assert assets_utils.check_usd_path_with_timeout(usd_path) is False diff --git a/source/isaaclab/test/utils/test_circular_buffer.py b/source/isaaclab/test/utils/test_circular_buffer.py index dbcccb056007..52a2c16829d8 100644 --- a/source/isaaclab/test/utils/test_circular_buffer.py +++ b/source/isaaclab/test/utils/test_circular_buffer.py @@ -1,14 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +import pytest import torch -import unittest """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app in headless mode simulation_app = AppLauncher(headless=True).app @@ -18,158 +18,165 @@ from isaaclab.utils import CircularBuffer -class TestCircularBuffer(unittest.TestCase): - """Test fixture for checking the circular buffer implementation.""" +@pytest.fixture +def circular_buffer(): + """Create a circular buffer for testing.""" + max_len = 5 + batch_size = 3 + device = "cpu" + return CircularBuffer(max_len, batch_size, device) + + +def test_initialization(circular_buffer): + """Test initialization of the circular buffer.""" + assert circular_buffer.max_length == 5 + assert circular_buffer.batch_size == 3 + assert circular_buffer.device == "cpu" + assert circular_buffer.current_length.tolist() == [0, 0, 0] + + +def test_reset(circular_buffer): + """Test resetting the circular buffer.""" + # append some data + data = torch.ones((circular_buffer.batch_size, 2), device=circular_buffer.device) + circular_buffer.append(data) + # reset the buffer + circular_buffer.reset() + + # check if the buffer has zeros entries + assert circular_buffer.current_length.tolist() == [0, 0, 0] + + +def test_reset_subset(circular_buffer): + """Test resetting a subset of batches in the circular buffer.""" + data1 = torch.ones((circular_buffer.batch_size, 2), device=circular_buffer.device) + data2 = 2.0 * data1.clone() + data3 = 3.0 * data1.clone() + circular_buffer.append(data1) + circular_buffer.append(data2) + # reset the buffer + reset_batch_id = 1 + circular_buffer.reset(batch_ids=[reset_batch_id]) + # check that correct batch is reset + assert circular_buffer.current_length.tolist()[reset_batch_id] == 0 + # Append new set of data + circular_buffer.append(data3) + # check if the correct number of entries are in each batch + expected_length = [3, 3, 3] + expected_length[reset_batch_id] = 1 + assert circular_buffer.current_length.tolist() == expected_length + # check that all entries of the recently reset and appended batch are equal + for i in range(circular_buffer.max_length): + torch.testing.assert_close(circular_buffer.buffer[reset_batch_id, 0], circular_buffer.buffer[reset_batch_id, i]) + + +def test_append_and_retrieve(circular_buffer): + """Test appending and retrieving data from the circular buffer.""" + # append some data + data1 = torch.tensor([[1, 1], [1, 1], [1, 1]], device=circular_buffer.device) + data2 = torch.tensor([[2, 2], [2, 2], [2, 2]], device=circular_buffer.device) + + circular_buffer.append(data1) + circular_buffer.append(data2) + + assert circular_buffer.current_length.tolist() == [2, 2, 2] + + retrieved_data = circular_buffer[torch.tensor([0, 0, 0], device=circular_buffer.device)] + assert torch.equal(retrieved_data, data2) + + retrieved_data = circular_buffer[torch.tensor([1, 1, 1], device=circular_buffer.device)] + assert torch.equal(retrieved_data, data1) + + +def test_buffer_overflow(circular_buffer): + """Test buffer overflow. + + If the buffer is full, the oldest data should be overwritten. + """ + # add data in ascending order + for count in range(circular_buffer.max_length + 2): + data = torch.full((circular_buffer.batch_size, 4), count, device=circular_buffer.device) + circular_buffer.append(data) - def setUp(self): - self.max_len = 5 - self.batch_size = 3 - self.device = "cpu" - self.buffer = CircularBuffer(self.max_len, self.batch_size, self.device) + # check buffer length is correct + assert circular_buffer.current_length.tolist() == [ + circular_buffer.max_length, + circular_buffer.max_length, + circular_buffer.max_length, + ] + # retrieve most recent data + key = torch.tensor([0, 0, 0], device=circular_buffer.device) + retrieved_data = circular_buffer[key] + expected_data = torch.full_like(data, circular_buffer.max_length + 1) + + assert torch.equal(retrieved_data, expected_data) + + # retrieve the oldest data + key = torch.tensor( + [circular_buffer.max_length - 1, circular_buffer.max_length - 1, circular_buffer.max_length - 1], + device=circular_buffer.device, + ) + retrieved_data = circular_buffer[key] + expected_data = torch.full_like(data, 2) + + assert torch.equal(retrieved_data, expected_data) + + +def test_empty_buffer_access(circular_buffer): + """Test accessing an empty buffer.""" + with pytest.raises(RuntimeError): + circular_buffer[torch.tensor([0, 0, 0], device=circular_buffer.device)] + + +def test_invalid_batch_size(circular_buffer): + """Test appending data with an invalid batch size.""" + data = torch.ones((circular_buffer.batch_size + 1, 2), device=circular_buffer.device) + with pytest.raises(ValueError): + circular_buffer.append(data) + + with pytest.raises(ValueError): + circular_buffer[torch.tensor([0, 0], device=circular_buffer.device)] + + +def test_key_greater_than_pushes(circular_buffer): + """Test retrieving data with a key greater than the number of pushes. + + In this case, the oldest data should be returned. """ - Test cases for CircularBuffer class. - """ + data1 = torch.tensor([[1, 1], [1, 1], [1, 1]], device=circular_buffer.device) + data2 = torch.tensor([[2, 2], [2, 2], [2, 2]], device=circular_buffer.device) - def test_initialization(self): - """Test initialization of the circular buffer.""" - self.assertEqual(self.buffer.max_length, self.max_len) - self.assertEqual(self.buffer.batch_size, self.batch_size) - self.assertEqual(self.buffer.device, self.device) - self.assertEqual(self.buffer.current_length.tolist(), [0, 0, 0]) - - def test_reset(self): - """Test resetting the circular buffer.""" - # append some data - data = torch.ones((self.batch_size, 2), device=self.device) - self.buffer.append(data) - # reset the buffer - self.buffer.reset() - - # check if the buffer has zeros entries - self.assertEqual(self.buffer.current_length.tolist(), [0, 0, 0]) - - def test_reset_subset(self): - """Test resetting a subset of batches in the circular buffer.""" - data1 = torch.ones((self.batch_size, 2), device=self.device) - data2 = 2.0 * data1.clone() - data3 = 3.0 * data1.clone() - self.buffer.append(data1) - self.buffer.append(data2) - # reset the buffer - reset_batch_id = 1 - self.buffer.reset(batch_ids=[reset_batch_id]) - # check that correct batch is reset - self.assertEqual(self.buffer.current_length.tolist()[reset_batch_id], 0) - # Append new set of data - self.buffer.append(data3) - # check if the correct number of entries are in each batch - expected_length = [3, 3, 3] - expected_length[reset_batch_id] = 1 - self.assertEqual(self.buffer.current_length.tolist(), expected_length) - # check that all entries of the recently reset and appended batch are equal - for i in range(self.max_len): - torch.testing.assert_close(self.buffer.buffer[reset_batch_id, 0], self.buffer.buffer[reset_batch_id, i]) - - def test_append_and_retrieve(self): - """Test appending and retrieving data from the circular buffer.""" - # append some data - data1 = torch.tensor([[1, 1], [1, 1], [1, 1]], device=self.device) - data2 = torch.tensor([[2, 2], [2, 2], [2, 2]], device=self.device) - - self.buffer.append(data1) - self.buffer.append(data2) - - self.assertEqual(self.buffer.current_length.tolist(), [2, 2, 2]) - - retrieved_data = self.buffer[torch.tensor([0, 0, 0], device=self.device)] - self.assertTrue(torch.equal(retrieved_data, data2)) - - retrieved_data = self.buffer[torch.tensor([1, 1, 1], device=self.device)] - self.assertTrue(torch.equal(retrieved_data, data1)) - - def test_buffer_overflow(self): - """Test buffer overflow. - - If the buffer is full, the oldest data should be overwritten. - """ - # add data in ascending order - for count in range(self.max_len + 2): - data = torch.full((self.batch_size, 4), count, device=self.device) - self.buffer.append(data) - - # check buffer length is correct - self.assertEqual(self.buffer.current_length.tolist(), [self.max_len, self.max_len, self.max_len]) - - # retrieve most recent data - key = torch.tensor([0, 0, 0], device=self.device) - retrieved_data = self.buffer[key] - expected_data = torch.full_like(data, self.max_len + 1) - - self.assertTrue(torch.equal(retrieved_data, expected_data)) - - # retrieve the oldest data - key = torch.tensor([self.max_len - 1, self.max_len - 1, self.max_len - 1], device=self.device) - retrieved_data = self.buffer[key] - expected_data = torch.full_like(data, 2) - - self.assertTrue(torch.equal(retrieved_data, expected_data)) - - def test_empty_buffer_access(self): - """Test accessing an empty buffer.""" - with self.assertRaises(RuntimeError): - self.buffer[torch.tensor([0, 0, 0], device=self.device)] - - def test_invalid_batch_size(self): - """Test appending data with an invalid batch size.""" - data = torch.ones((self.batch_size + 1, 2), device=self.device) - with self.assertRaises(ValueError): - self.buffer.append(data) - - with self.assertRaises(ValueError): - self.buffer[torch.tensor([0, 0], device=self.device)] - - def test_key_greater_than_pushes(self): - """Test retrieving data with a key greater than the number of pushes. - - In this case, the oldest data should be returned. - """ - data1 = torch.tensor([[1, 1], [1, 1], [1, 1]], device=self.device) - data2 = torch.tensor([[2, 2], [2, 2], [2, 2]], device=self.device) - - self.buffer.append(data1) - self.buffer.append(data2) - - retrieved_data = self.buffer[torch.tensor([5, 5, 5], device=self.device)] - self.assertTrue(torch.equal(retrieved_data, data1)) - - def test_return_buffer_prop(self): - """Test retrieving the whole buffer for correct size and contents. - Returning the whole buffer should have the shape [batch_size,max_len,data.shape[1:]] - """ - num_overflow = 2 - for i in range(self.buffer.max_length + num_overflow): - data = torch.tensor([[i]], device=self.device).repeat(3, 2) - self.buffer.append(data) - - retrieved_buffer = self.buffer.buffer - # check shape - self.assertTrue(retrieved_buffer.shape == torch.Size([self.buffer.batch_size, self.buffer.max_length, 2])) - # check that batch is first dimension - torch.testing.assert_close(retrieved_buffer[0], retrieved_buffer[1]) - # check oldest - torch.testing.assert_close( - retrieved_buffer[:, 0], torch.tensor([[num_overflow]], device=self.device).repeat(3, 2) - ) - # check most recent - torch.testing.assert_close( - retrieved_buffer[:, -1], - torch.tensor([[self.buffer.max_length + num_overflow - 1]], device=self.device).repeat(3, 2), - ) - # check that it is returned oldest first - for idx in range(self.buffer.max_length - 1): - self.assertTrue(torch.all(torch.le(retrieved_buffer[:, idx], retrieved_buffer[:, idx + 1]))) - - -if __name__ == "__main__": - run_tests() + circular_buffer.append(data1) + circular_buffer.append(data2) + + retrieved_data = circular_buffer[torch.tensor([5, 5, 5], device=circular_buffer.device)] + assert torch.equal(retrieved_data, data1) + + +def test_return_buffer_prop(circular_buffer): + """Test retrieving the whole buffer for correct size and contents. + Returning the whole buffer should have the shape [batch_size,max_len,data.shape[1:]] + """ + num_overflow = 2 + for i in range(circular_buffer.max_length + num_overflow): + data = torch.tensor([[i]], device=circular_buffer.device).repeat(3, 2) + circular_buffer.append(data) + + retrieved_buffer = circular_buffer.buffer + # check shape + assert retrieved_buffer.shape == torch.Size([circular_buffer.batch_size, circular_buffer.max_length, 2]) + # check that batch is first dimension + torch.testing.assert_close(retrieved_buffer[0], retrieved_buffer[1]) + # check oldest + torch.testing.assert_close( + retrieved_buffer[:, 0], torch.tensor([[num_overflow]], device=circular_buffer.device).repeat(3, 2) + ) + # check most recent + torch.testing.assert_close( + retrieved_buffer[:, -1], + torch.tensor([[circular_buffer.max_length + num_overflow - 1]], device=circular_buffer.device).repeat(3, 2), + ) + # check that it is returned oldest first + for idx in range(circular_buffer.max_length - 1): + assert torch.all(torch.le(retrieved_buffer[:, idx], retrieved_buffer[:, idx + 1])) diff --git a/source/isaaclab/test/utils/test_configclass.py b/source/isaaclab/test/utils/test_configclass.py index d41502a5d3d0..0c024be03f3b 100644 --- a/source/isaaclab/test/utils/test_configclass.py +++ b/source/isaaclab/test/utils/test_configclass.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,23 +9,23 @@ # because warp is only available in the context of a running simulation """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import copy import os -import torch -import unittest from collections.abc import Callable from dataclasses import MISSING, asdict, field from functools import wraps from typing import Any, ClassVar +import pytest +import torch + from isaaclab.utils.configclass import configclass from isaaclab.utils.dict import class_to_dict, dict_to_md5_hash, update_class_from_dict from isaaclab.utils.io import dump_yaml, load_yaml @@ -472,15 +472,15 @@ class MissingChildDemoCfg(MissingParentDemoCfg): """ functions_demo_cfg_correct = { - "func": "__main__:dummy_function1", - "wrapped_func": "__main__:wrapped_dummy_function3", - "func_in_dict": {"func": "__main__:dummy_function1"}, + "func": "test_configclass:dummy_function1", + "wrapped_func": "test_configclass:wrapped_dummy_function3", + "func_in_dict": {"func": "test_configclass:dummy_function1"}, } functions_demo_cfg_for_updating = { - "func": "__main__:dummy_function2", - "wrapped_func": "__main__:wrapped_dummy_function4", - "func_in_dict": {"func": "__main__:dummy_function2"}, + "func": "test_configclass:dummy_function2", + "wrapped_func": "test_configclass:wrapped_dummy_function4", + "func_in_dict": {"func": "test_configclass:dummy_function2"}, } """ @@ -504,526 +504,576 @@ class MissingChildDemoCfg(MissingParentDemoCfg): """ -class TestConfigClass(unittest.TestCase): - """Test cases for various situations with configclass decorator for configuration.""" - - def test_str(self): - """Test printing the configuration.""" - cfg = BasicDemoCfg() - print() - print(cfg) - - def test_str_dict(self): - """Test printing the configuration using dataclass utility.""" - cfg = BasicDemoCfg() - print() - print("Using dataclass function: ", asdict(cfg)) - print("Using internal function: ", cfg.to_dict()) - self.assertDictEqual(asdict(cfg), cfg.to_dict()) - - def test_dict_conversion(self): - """Test dictionary conversion of configclass instance.""" - cfg = BasicDemoCfg() - # dataclass function - self.assertDictEqual(asdict(cfg), basic_demo_cfg_correct) - self.assertDictEqual(asdict(cfg.env), basic_demo_cfg_correct["env"]) - # utility function - self.assertDictEqual(class_to_dict(cfg), basic_demo_cfg_correct) - self.assertDictEqual(class_to_dict(cfg.env), basic_demo_cfg_correct["env"]) - # internal function - self.assertDictEqual(cfg.to_dict(), basic_demo_cfg_correct) - self.assertDictEqual(cfg.env.to_dict(), basic_demo_cfg_correct["env"]) - - torch_cfg = BasicDemoTorchCfg() - torch_cfg_dict = torch_cfg.to_dict() - # We have to do a manual check because torch.Tensor does not work with assertDictEqual. - self.assertEqual(torch_cfg_dict["some_number"], 0) - self.assertTrue(torch.all(torch_cfg_dict["some_tensor"] == torch.tensor([1, 2, 3]))) - - def test_actuator_cfg_dict_conversion(self): - """Test dict conversion of ActuatorConfig.""" - # create a basic RemotizedPDActuator config - actuator_cfg = BasicActuatorCfg() - # return writable attributes of config object - actuator_cfg_dict_attr = actuator_cfg.__dict__ - # check if __dict__ attribute of config is not empty - self.assertTrue(len(actuator_cfg_dict_attr) > 0) - # class_to_dict utility function should return a primitive dictionary - actuator_cfg_dict = class_to_dict(actuator_cfg) - self.assertTrue(isinstance(actuator_cfg_dict, dict)) - - def test_dict_conversion_order(self): - """Tests that order is conserved when converting to dictionary.""" - true_outer_order = ["device_id", "env", "robot_default_state", "list_config"] - true_env_order = ["num_envs", "episode_length", "viewer"] - # create config - cfg = BasicDemoCfg() - # check ordering - for label, parsed_value in zip(true_outer_order, cfg.__dict__.keys()): - self.assertEqual(label, parsed_value) - for label, parsed_value in zip(true_env_order, cfg.env.__dict__.keys()): - self.assertEqual(label, parsed_value) - # convert config to dictionary - cfg_dict = class_to_dict(cfg) - # check ordering - for label, parsed_value in zip(true_outer_order, cfg_dict.keys()): - self.assertEqual(label, parsed_value) - for label, parsed_value in zip(true_env_order, cfg_dict["env"].keys()): - self.assertEqual(label, parsed_value) - # check ordering when copied - cfg_dict_copied = copy.deepcopy(cfg_dict) - cfg_dict_copied.pop("list_config") - # check ordering - for label, parsed_value in zip(true_outer_order, cfg_dict_copied.keys()): - self.assertEqual(label, parsed_value) - for label, parsed_value in zip(true_env_order, cfg_dict_copied["env"].keys()): - self.assertEqual(label, parsed_value) - - def test_config_update_via_constructor(self): - """Test updating configclass through initialization.""" - cfg = BasicDemoCfg(env=EnvCfg(num_envs=22, viewer=ViewerCfg(eye=(2.0, 2.0, 2.0)))) - self.assertDictEqual(asdict(cfg), basic_demo_cfg_change_correct) - - def test_config_update_after_init(self): - """Test updating configclass using instance members.""" - cfg = BasicDemoCfg() - cfg.env.num_envs = 22 - cfg.env.viewer.eye = (2.0, 2.0, 2.0) # note: changes from list to tuple - self.assertDictEqual(asdict(cfg), basic_demo_cfg_change_correct) - - def test_config_update_dict(self): - """Test updating configclass using dictionary.""" - cfg = BasicDemoCfg() - cfg_dict = {"env": {"num_envs": 22, "viewer": {"eye": (2.0, 2.0, 2.0)}}} - update_class_from_dict(cfg, cfg_dict) - self.assertDictEqual(asdict(cfg), basic_demo_cfg_change_correct) +def test_str(): + """Test printing the configuration.""" + cfg = BasicDemoCfg() + print() + print(cfg) + + +def test_str_dict(): + """Test printing the configuration using dataclass utility.""" + cfg = BasicDemoCfg() + print() + print("Using dataclass function: ", asdict(cfg)) + print("Using internal function: ", cfg.to_dict()) + assert asdict(cfg) == cfg.to_dict() + + +def test_dict_conversion(): + """Test dictionary conversion of configclass instance.""" + cfg = BasicDemoCfg() + # dataclass function + assert asdict(cfg) == basic_demo_cfg_correct + assert asdict(cfg.env) == basic_demo_cfg_correct["env"] + # utility function + assert class_to_dict(cfg) == basic_demo_cfg_correct + assert class_to_dict(cfg.env) == basic_demo_cfg_correct["env"] + # internal function + assert cfg.to_dict() == basic_demo_cfg_correct + assert cfg.env.to_dict() == basic_demo_cfg_correct["env"] + + torch_cfg = BasicDemoTorchCfg() + torch_cfg_dict = torch_cfg.to_dict() + # We have to do a manual check because torch.Tensor does not work with assertDictEqual. + assert torch_cfg_dict["some_number"] == 0 + assert torch.all(torch_cfg_dict["some_tensor"] == torch.tensor([1, 2, 3])) + + +def test_actuator_cfg_dict_conversion(): + """Test dict conversion of ActuatorConfig.""" + # create a basic RemotizedPDActuator config + actuator_cfg = BasicActuatorCfg() + # return writable attributes of config object + actuator_cfg_dict_attr = actuator_cfg.__dict__ + # check if __dict__ attribute of config is not empty + assert len(actuator_cfg_dict_attr) > 0 + # class_to_dict utility function should return a primitive dictionary + actuator_cfg_dict = class_to_dict(actuator_cfg) + assert isinstance(actuator_cfg_dict, dict) + + +def test_dict_conversion_order(): + """Tests that order is conserved when converting to dictionary.""" + true_outer_order = ["device_id", "env", "robot_default_state", "list_config"] + true_env_order = ["num_envs", "episode_length", "viewer"] + # create config + cfg = BasicDemoCfg() + # check ordering + for label, parsed_value in zip(true_outer_order, cfg.__dict__.keys()): + assert label == parsed_value + for label, parsed_value in zip(true_env_order, cfg.env.__dict__.keys()): + assert label == parsed_value + # convert config to dictionary + cfg_dict = class_to_dict(cfg) + # check ordering + for label, parsed_value in zip(true_outer_order, cfg_dict.keys()): + assert label == parsed_value + for label, parsed_value in zip(true_env_order, cfg_dict["env"].keys()): + assert label == parsed_value + # check ordering when copied + cfg_dict_copied = copy.deepcopy(cfg_dict) + cfg_dict_copied.pop("list_config") + # check ordering + for label, parsed_value in zip(true_outer_order, cfg_dict_copied.keys()): + assert label == parsed_value + for label, parsed_value in zip(true_env_order, cfg_dict_copied["env"].keys()): + assert label == parsed_value + + +def test_config_update_via_constructor(): + """Test updating configclass through initialization.""" + cfg = BasicDemoCfg(env=EnvCfg(num_envs=22, viewer=ViewerCfg(eye=(2.0, 2.0, 2.0)))) + assert asdict(cfg) == basic_demo_cfg_change_correct + + +def test_config_update_after_init(): + """Test updating configclass using instance members.""" + cfg = BasicDemoCfg() + cfg.env.num_envs = 22 + cfg.env.viewer.eye = (2.0, 2.0, 2.0) # note: changes from list to tuple + assert asdict(cfg) == basic_demo_cfg_change_correct + + +def test_config_update_dict(): + """Test updating configclass using dictionary.""" + cfg = BasicDemoCfg() + cfg_dict = {"env": {"num_envs": 22, "viewer": {"eye": (2.0, 2.0, 2.0)}}} + update_class_from_dict(cfg, cfg_dict) + assert asdict(cfg) == basic_demo_cfg_change_correct + + # check types are also correct + assert isinstance(cfg.env.viewer, ViewerCfg) + assert isinstance(cfg.env.viewer.eye, tuple) + + +def test_config_update_dict_with_none(): + """Test updating configclass using a dictionary that contains None.""" + cfg = BasicDemoCfg() + cfg_dict = {"env": {"num_envs": 22, "viewer": None}} + update_class_from_dict(cfg, cfg_dict) + assert asdict(cfg) == basic_demo_cfg_change_with_none_correct + + +def test_config_update_dict_tuple(): + """Test updating configclass using a dictionary that modifies a tuple.""" + cfg = BasicDemoCfg() + cfg_dict = {"list_config": [{"params": {"A": -1, "B": -2}}, {"params": {"A": -3, "B": -4}}]} + update_class_from_dict(cfg, cfg_dict) + assert asdict(cfg) == basic_demo_cfg_change_with_tuple_correct + + +def test_config_update_nested_dict(): + """Test updating configclass with sub-dictionaries.""" + cfg = NestedDictAndListCfg() + cfg_dict = { + "dict_1": {"dict_2": {"func": "test_configclass:dummy_function2"}}, + "list_1": [ + {"num_envs": 23, "episode_length": 3000, "viewer": {"eye": [5.0, 5.0, 5.0]}}, + {"num_envs": 24, "viewer": {"eye": [6.0, 6.0, 6.0]}}, + ], + } + update_class_from_dict(cfg, cfg_dict) + assert asdict(cfg) == basic_demo_cfg_nested_dict_and_list + + # check types are also correct + assert isinstance(cfg.list_1[0], EnvCfg) + assert isinstance(cfg.list_1[1], EnvCfg) + assert isinstance(cfg.list_1[0].viewer, ViewerCfg) + assert isinstance(cfg.list_1[1].viewer, ViewerCfg) + + +def test_config_update_different_iterable_lengths(): + """Iterables are whole replaced, even if their lengths are different.""" + + # original cfg has length-6 tuple and list + cfg = RobotDefaultStateCfg() + assert cfg.dof_pos == (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + assert cfg.dof_vel == [0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + + # patch uses different lengths + patch = { + "dof_pos": (1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), # longer tuple + "dof_vel": [9.0, 8.0, 7.0], # shorter list + } + + # should not raise + update_class_from_dict(cfg, patch) + + # whole sequences are replaced + assert cfg.dof_pos == (1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0) + assert cfg.dof_vel == [9.0, 8.0, 7.0] + + +def test_config_update_dict_using_internal(): + """Test updating configclass from a dictionary using configclass method.""" + cfg = BasicDemoCfg() + cfg_dict = {"env": {"num_envs": 22, "viewer": {"eye": (2.0, 2.0, 2.0)}}} + cfg.from_dict(cfg_dict) + assert cfg.to_dict() == basic_demo_cfg_change_correct + + +def test_config_update_dict_using_post_init(): + cfg = BasicDemoPostInitCfg() + assert cfg.to_dict() == basic_demo_post_init_cfg_correct - # check types are also correct - self.assertIsInstance(cfg.env.viewer, ViewerCfg) - self.assertIsInstance(cfg.env.viewer.eye, tuple) - def test_config_update_dict_with_none(self): - """Test updating configclass using a dictionary that contains None.""" - cfg = BasicDemoCfg() - cfg_dict = {"env": {"num_envs": 22, "viewer": None}} +def test_invalid_update_key(): + """Test invalid key update.""" + cfg = BasicDemoCfg() + cfg_dict = {"env": {"num_envs": 22, "viewer": {"pos": (2.0, 2.0, 2.0)}}} + with pytest.raises(KeyError): update_class_from_dict(cfg, cfg_dict) - self.assertDictEqual(asdict(cfg), basic_demo_cfg_change_with_none_correct) - def test_config_update_dict_tuple(self): - """Test updating configclass using a dictionary that modifies a tuple.""" - cfg = BasicDemoCfg() - cfg_dict = {"list_config": [{"params": {"A": -1, "B": -2}}, {"params": {"A": -3, "B": -4}}]} - update_class_from_dict(cfg, cfg_dict) - self.assertDictEqual(asdict(cfg), basic_demo_cfg_change_with_tuple_correct) - - def test_config_update_nested_dict(self): - """Test updating configclass with sub-dictionaries.""" - cfg = NestedDictAndListCfg() - cfg_dict = { - "dict_1": {"dict_2": {"func": "__main__:dummy_function2"}}, - "list_1": [ - {"num_envs": 23, "episode_length": 3000, "viewer": {"eye": [5.0, 5.0, 5.0]}}, - {"num_envs": 24, "viewer": {"eye": [6.0, 6.0, 6.0]}}, - ], - } - update_class_from_dict(cfg, cfg_dict) - self.assertDictEqual(asdict(cfg), basic_demo_cfg_nested_dict_and_list) - - # check types are also correct - self.assertIsInstance(cfg.list_1[0], EnvCfg) - self.assertIsInstance(cfg.list_1[1], EnvCfg) - self.assertIsInstance(cfg.list_1[0].viewer, ViewerCfg) - self.assertIsInstance(cfg.list_1[1].viewer, ViewerCfg) - - def test_config_update_dict_using_internal(self): - """Test updating configclass from a dictionary using configclass method.""" - cfg = BasicDemoCfg() - cfg_dict = {"env": {"num_envs": 22, "viewer": {"eye": (2.0, 2.0, 2.0)}}} - cfg.from_dict(cfg_dict) - self.assertDictEqual(cfg.to_dict(), basic_demo_cfg_change_correct) - - def test_config_update_dict_using_post_init(self): - cfg = BasicDemoPostInitCfg() - self.assertDictEqual(cfg.to_dict(), basic_demo_post_init_cfg_correct) - - def test_invalid_update_key(self): - """Test invalid key update.""" - cfg = BasicDemoCfg() - cfg_dict = {"env": {"num_envs": 22, "viewer": {"pos": (2.0, 2.0, 2.0)}}} - with self.assertRaises(KeyError): - update_class_from_dict(cfg, cfg_dict) - - def test_multiple_instances(self): - """Test multiple instances with twice instantiation.""" - # create two config instances - cfg1 = BasicDemoCfg() - cfg2 = BasicDemoCfg() - - # check variables - # mutable -- variables should be different - self.assertNotEqual(id(cfg1.env.viewer.eye), id(cfg2.env.viewer.eye)) - self.assertNotEqual(id(cfg1.env.viewer.lookat), id(cfg2.env.viewer.lookat)) - self.assertNotEqual(id(cfg1.robot_default_state), id(cfg2.robot_default_state)) - # immutable -- variables are the same - self.assertEqual(id(cfg1.robot_default_state.dof_pos), id(cfg2.robot_default_state.dof_pos)) - self.assertEqual(id(cfg1.env.num_envs), id(cfg2.env.num_envs)) - self.assertEqual(id(cfg1.device_id), id(cfg2.device_id)) - - # check values - self.assertDictEqual(cfg1.env.to_dict(), cfg2.env.to_dict()) - self.assertDictEqual(cfg1.robot_default_state.to_dict(), cfg2.robot_default_state.to_dict()) - - def test_alter_values_multiple_instances(self): - """Test alterations in multiple instances of the same configclass.""" - # create two config instances - cfg1 = BasicDemoCfg() - cfg2 = BasicDemoCfg() - - # alter configurations - cfg1.env.num_envs = 22 # immutable data: int - cfg1.env.viewer.eye[0] = 1.0 # mutable data: list - cfg1.env.viewer.lookat[2] = 12.0 # mutable data: list - - # check variables - # values should be different - self.assertNotEqual(cfg1.env.num_envs, cfg2.env.num_envs) - self.assertNotEqual(cfg1.env.viewer.eye, cfg2.env.viewer.eye) - self.assertNotEqual(cfg1.env.viewer.lookat, cfg2.env.viewer.lookat) - # mutable -- variables are different ids - self.assertNotEqual(id(cfg1.env.viewer.eye), id(cfg2.env.viewer.eye)) - self.assertNotEqual(id(cfg1.env.viewer.lookat), id(cfg2.env.viewer.lookat)) - # immutable -- altered variables are different ids - self.assertNotEqual(id(cfg1.env.num_envs), id(cfg2.env.num_envs)) - - def test_multiple_instances_with_replace(self): - """Test multiple instances with creation through replace function.""" - # create two config instances - cfg1 = BasicDemoCfg() - cfg2 = cfg1.replace() - - # check variable IDs - # mutable -- variables should be different - self.assertNotEqual(id(cfg1.env.viewer.eye), id(cfg2.env.viewer.eye)) - self.assertNotEqual(id(cfg1.env.viewer.lookat), id(cfg2.env.viewer.lookat)) - self.assertNotEqual(id(cfg1.robot_default_state), id(cfg2.robot_default_state)) - # immutable -- variables are the same - self.assertEqual(id(cfg1.robot_default_state.dof_pos), id(cfg2.robot_default_state.dof_pos)) - self.assertEqual(id(cfg1.env.num_envs), id(cfg2.env.num_envs)) - self.assertEqual(id(cfg1.device_id), id(cfg2.device_id)) - - # check values - self.assertDictEqual(cfg1.to_dict(), cfg2.to_dict()) - - def test_alter_values_multiple_instances_wth_replace(self): - """Test alterations in multiple instances through replace function.""" - # create two config instances - cfg1 = BasicDemoCfg() - cfg2 = cfg1.replace(device_id=1) - - # alter configurations - cfg1.env.num_envs = 22 # immutable data: int - cfg1.env.viewer.eye[0] = 1.0 # mutable data: list - cfg1.env.viewer.lookat[2] = 12.0 # mutable data: list - - # check variables - # values should be different - self.assertNotEqual(cfg1.env.num_envs, cfg2.env.num_envs) - self.assertNotEqual(cfg1.env.viewer.eye, cfg2.env.viewer.eye) - self.assertNotEqual(cfg1.env.viewer.lookat, cfg2.env.viewer.lookat) - # mutable -- variables are different ids - self.assertNotEqual(id(cfg1.env.viewer.eye), id(cfg2.env.viewer.eye)) - self.assertNotEqual(id(cfg1.env.viewer.lookat), id(cfg2.env.viewer.lookat)) - # immutable -- altered variables are different ids - self.assertNotEqual(id(cfg1.env.num_envs), id(cfg2.env.num_envs)) - self.assertNotEqual(id(cfg1.device_id), id(cfg2.device_id)) - - def test_configclass_type_ordering(self): - """Checks ordering of config objects when no type annotation is provided.""" - - cfg_1 = TypeAnnotationOrderingDemoCfg() - cfg_2 = NonTypeAnnotationOrderingDemoCfg() - cfg_3 = InheritedNonTypeAnnotationOrderingDemoCfg() - - # check ordering - self.assertEqual(list(cfg_1.__dict__.keys()), list(cfg_2.__dict__.keys())) - self.assertEqual(list(cfg_3.__dict__.keys()), list(cfg_2.__dict__.keys())) - self.assertEqual(list(cfg_1.__dict__.keys()), list(cfg_3.__dict__.keys())) - - def test_functions_config(self): - """Tests having functions as values in the configuration instance.""" - cfg = FunctionsDemoCfg() - # check types - self.assertEqual(cfg.__annotations__["func"], type(dummy_function1)) - self.assertEqual(cfg.__annotations__["wrapped_func"], type(wrapped_dummy_function3)) - self.assertEqual(cfg.__annotations__["func_in_dict"], dict) - # check calling - self.assertEqual(cfg.func(), 1) - self.assertEqual(cfg.wrapped_func(), 4) - self.assertEqual(cfg.func_in_dict["func"](), 1) - - def test_function_impl_config(self): - """Tests having function defined in the class instance.""" - cfg = FunctionImplementedDemoCfg() - # change value - self.assertEqual(cfg.a, 5) - cfg.set_a(10) - self.assertEqual(cfg.a, 10) - - def test_class_function_impl_config(self): - """Tests having class function defined in the class instance.""" - cfg = ClassFunctionImplementedDemoCfg() - - # check that the annotations are correct - self.assertDictEqual(cfg.__annotations__, {"a": "int"}) - - # check all methods are callable - cfg.instance_method() - new_cfg1 = cfg.class_method(20) - # check value is correct - self.assertEqual(new_cfg1.a, 20) - - # create the same config instance using class method - new_cfg2 = ClassFunctionImplementedDemoCfg.class_method(20) - # check value is correct - self.assertEqual(new_cfg2.a, 20) - - def test_class_property_impl_config(self): - """Tests having class property defined in the class instance.""" - cfg = ClassFunctionImplementedDemoCfg() - - # check that the annotations are correct - self.assertDictEqual(cfg.__annotations__, {"a": "int"}) - - # check all methods are callable - cfg.instance_method() - - # check value is correct - self.assertEqual(cfg.a, 5) - self.assertEqual(cfg.a_proxy, 5) - - # set through property - cfg.a_proxy = 10 - self.assertEqual(cfg.a, 10) - self.assertEqual(cfg.a_proxy, 10) - - def test_dict_conversion_functions_config(self): - """Tests conversion of config with functions into dictionary.""" - cfg = FunctionsDemoCfg() - cfg_dict = class_to_dict(cfg) - self.assertEqual(cfg_dict["func"], functions_demo_cfg_correct["func"]) - self.assertEqual(cfg_dict["wrapped_func"], functions_demo_cfg_correct["wrapped_func"]) - self.assertEqual(cfg_dict["func_in_dict"]["func"], functions_demo_cfg_correct["func_in_dict"]["func"]) - - def test_update_functions_config_with_functions(self): - """Tests updating config with functions.""" - cfg = FunctionsDemoCfg() - # update config - update_class_from_dict(cfg, functions_demo_cfg_for_updating) - # check calling - self.assertEqual(cfg.func(), 2) - self.assertEqual(cfg.wrapped_func(), 5) - self.assertEqual(cfg.func_in_dict["func"](), 2) - - def test_missing_type_in_config(self): - """Tests missing type annotation in config. - - Should complain that 'c' is missing type annotation since it cannot be inferred - from 'MISSING' value. - """ - with self.assertRaises(TypeError): - - @configclass - class MissingTypeDemoCfg: - a: int = 1 - b = 2 - c = MISSING - - def test_missing_default_value_in_config(self): - """Tests missing default value in config. - - Should complain that 'a' is missing default value since it cannot be inferred - from type annotation. - """ - with self.assertRaises(ValueError): - - @configclass - class MissingTypeDemoCfg: - a: int - b = 2 - - def test_required_argument_for_missing_type_in_config(self): - """Tests required positional argument for missing type annotation in config creation.""" + +def test_multiple_instances(): + """Test multiple instances with twice instantiation.""" + # create two config instances + cfg1 = BasicDemoCfg() + cfg2 = BasicDemoCfg() + + # check variables + # mutable -- variables should be different + assert id(cfg1.env.viewer.eye) != id(cfg2.env.viewer.eye) + assert id(cfg1.env.viewer.lookat) != id(cfg2.env.viewer.lookat) + assert id(cfg1.robot_default_state) != id(cfg2.robot_default_state) + # immutable -- variables are the same + assert id(cfg1.robot_default_state.dof_pos) == id(cfg2.robot_default_state.dof_pos) + assert id(cfg1.env.num_envs) == id(cfg2.env.num_envs) + assert id(cfg1.device_id) == id(cfg2.device_id) + + # check values + assert cfg1.env.to_dict() == cfg2.env.to_dict() + assert cfg1.robot_default_state.to_dict() == cfg2.robot_default_state.to_dict() + + +def test_alter_values_multiple_instances(): + """Test alterations in multiple instances of the same configclass.""" + # create two config instances + cfg1 = BasicDemoCfg() + cfg2 = BasicDemoCfg() + + # alter configurations + cfg1.env.num_envs = 22 # immutable data: int + cfg1.env.viewer.eye[0] = 1.0 # mutable data: list + cfg1.env.viewer.lookat[2] = 12.0 # mutable data: list + + # check variables + # values should be different + assert cfg1.env.num_envs != cfg2.env.num_envs + assert cfg1.env.viewer.eye != cfg2.env.viewer.eye + assert cfg1.env.viewer.lookat != cfg2.env.viewer.lookat + # mutable -- variables are different ids + assert id(cfg1.env.viewer.eye) != id(cfg2.env.viewer.eye) + assert id(cfg1.env.viewer.lookat) != id(cfg2.env.viewer.lookat) + # immutable -- altered variables are different ids + assert id(cfg1.env.num_envs) != id(cfg2.env.num_envs) + + +def test_multiple_instances_with_replace(): + """Test multiple instances with creation through replace function.""" + # create two config instances + cfg1 = BasicDemoCfg() + cfg2 = cfg1.replace() + + # check variable IDs + # mutable -- variables should be different + assert id(cfg1.env.viewer.eye) != id(cfg2.env.viewer.eye) + assert id(cfg1.env.viewer.lookat) != id(cfg2.env.viewer.lookat) + assert id(cfg1.robot_default_state) != id(cfg2.robot_default_state) + # immutable -- variables are the same + assert id(cfg1.robot_default_state.dof_pos) == id(cfg2.robot_default_state.dof_pos) + assert id(cfg1.env.num_envs) == id(cfg2.env.num_envs) + assert id(cfg1.device_id) == id(cfg2.device_id) + + # check values + assert cfg1.to_dict() == cfg2.to_dict() + + +def test_alter_values_multiple_instances_wth_replace(): + """Test alterations in multiple instances through replace function.""" + # create two config instances + cfg1 = BasicDemoCfg() + cfg2 = cfg1.replace(device_id=1) + + # alter configurations + cfg1.env.num_envs = 22 # immutable data: int + cfg1.env.viewer.eye[0] = 1.0 # mutable data: list + cfg1.env.viewer.lookat[2] = 12.0 # mutable data: list + + # check variables + # values should be different + assert cfg1.env.num_envs != cfg2.env.num_envs + assert cfg1.env.viewer.eye != cfg2.env.viewer.eye + assert cfg1.env.viewer.lookat != cfg2.env.viewer.lookat + # mutable -- variables are different ids + assert id(cfg1.env.viewer.eye) != id(cfg2.env.viewer.eye) + assert id(cfg1.env.viewer.lookat) != id(cfg2.env.viewer.lookat) + # immutable -- altered variables are different ids + assert id(cfg1.env.num_envs) != id(cfg2.env.num_envs) + assert id(cfg1.device_id) != id(cfg2.device_id) + + +def test_configclass_type_ordering(): + """Checks ordering of config objects when no type annotation is provided.""" + + cfg_1 = TypeAnnotationOrderingDemoCfg() + cfg_2 = NonTypeAnnotationOrderingDemoCfg() + cfg_3 = InheritedNonTypeAnnotationOrderingDemoCfg() + + # check ordering + assert list(cfg_1.__dict__.keys()) == list(cfg_2.__dict__.keys()) + assert list(cfg_3.__dict__.keys()) == list(cfg_2.__dict__.keys()) + assert list(cfg_1.__dict__.keys()) == list(cfg_3.__dict__.keys()) + + +def test_functions_config(): + """Tests having functions as values in the configuration instance.""" + cfg = FunctionsDemoCfg() + # check types + assert cfg.__annotations__["func"] is type(dummy_function1) + assert cfg.__annotations__["wrapped_func"] is type(wrapped_dummy_function3) + assert cfg.__annotations__["func_in_dict"] is dict + # check calling + assert cfg.func() == 1 + assert cfg.wrapped_func() == 4 + assert cfg.func_in_dict["func"]() == 1 + + +def test_function_impl_config(): + """Tests having function defined in the class instance.""" + cfg = FunctionImplementedDemoCfg() + # change value + assert cfg.a == 5 + cfg.set_a(10) + assert cfg.a == 10 + + +def test_class_function_impl_config(): + """Tests having class function defined in the class instance.""" + cfg = ClassFunctionImplementedDemoCfg() + + # check that the annotations are correct + assert cfg.__annotations__ == {"a": "int"} + + # check all methods are callable + cfg.instance_method() + new_cfg1 = cfg.class_method(20) + # check value is correct + assert new_cfg1.a == 20 + + # create the same config instance using class method + new_cfg2 = ClassFunctionImplementedDemoCfg.class_method(20) + # check value is correct + assert new_cfg2.a == 20 + + +def test_class_property_impl_config(): + """Tests having class property defined in the class instance.""" + cfg = ClassFunctionImplementedDemoCfg() + + # check that the annotations are correct + assert cfg.__annotations__ == {"a": "int"} + + # check all methods are callable + cfg.instance_method() + + # check value is correct + assert cfg.a == 5 + assert cfg.a_proxy == 5 + + # set through property + cfg.a_proxy = 10 + assert cfg.a == 10 + assert cfg.a_proxy == 10 + + +def test_dict_conversion_functions_config(): + """Tests conversion of config with functions into dictionary.""" + cfg = FunctionsDemoCfg() + cfg_dict = class_to_dict(cfg) + assert cfg_dict["func"] == functions_demo_cfg_correct["func"] + assert cfg_dict["wrapped_func"] == functions_demo_cfg_correct["wrapped_func"] + assert cfg_dict["func_in_dict"]["func"] == functions_demo_cfg_correct["func_in_dict"]["func"] + + +def test_update_functions_config_with_functions(): + """Tests updating config with functions.""" + cfg = FunctionsDemoCfg() + # update config + update_class_from_dict(cfg, functions_demo_cfg_for_updating) + # check calling + assert cfg.func() == 2 + assert cfg.wrapped_func() == 5 + assert cfg.func_in_dict["func"]() == 2 + + +def test_missing_type_in_config(): + """Tests missing type annotation in config. + + Should complain that 'c' is missing type annotation since it cannot be inferred + from 'MISSING' value. + """ + with pytest.raises(TypeError): @configclass class MissingTypeDemoCfg: a: int = 1 b = 2 - c: int = MISSING - - # should complain that 'c' is missed in positional arguments - # TODO: Uncomment this when we move to 3.10. - # with self.assertRaises(TypeError): - # cfg = MissingTypeDemoCfg(a=1) - # should not complain - cfg = MissingTypeDemoCfg(a=1, c=3) - - self.assertEqual(cfg.a, 1) - self.assertEqual(cfg.b, 2) - - def test_config_inheritance(self): - """Tests that inheritance works properly.""" - # check variables - cfg_a = ChildADemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) - - self.assertEqual(cfg_a.func, dummy_function1) - self.assertEqual(cfg_a.a, 20) - self.assertEqual(cfg_a.d, 3) - self.assertEqual(cfg_a.j, ["c", "d"]) - - # check post init - self.assertEqual(cfg_a.b, 3) - self.assertEqual(cfg_a.i, ["a", "b"]) - self.assertEqual(cfg_a.m.rot, (2.0, 0.0, 0.0, 0.0)) - - def test_config_inheritance_independence(self): - """Tests that subclass instantions have fully unique members, - rather than references to members of the parent class""" - # instantiate two classes which inherit from a shared parent, - # but which will differently modify their members in their - # __init__ and __post_init__ - cfg_a = ChildADemoCfg() - cfg_b = ChildBDemoCfg() - - # Test various combinations of initialization - # and defaults across inherited members in - # instances to verify independence between the subclasses - self.assertIsInstance(cfg_a.a, type(MISSING)) - self.assertEqual(cfg_b.a, 100) - self.assertEqual(cfg_a.b, 3) - self.assertEqual(cfg_b.b, 8) - self.assertEqual(cfg_a.c, RobotDefaultStateCfg()) - self.assertIsInstance(cfg_b.c, type(MISSING)) - self.assertEqual(cfg_a.m.rot, (2.0, 0.0, 0.0, 0.0)) - self.assertEqual(cfg_b.m.rot, (1.0, 0.0, 0.0, 0.0)) - self.assertIsInstance(cfg_a.j, type(MISSING)) - self.assertEqual(cfg_b.j, ["3", "4"]) - self.assertEqual(cfg_a.i, ["a", "b"]) - self.assertEqual(cfg_b.i, ["1", "2"]) - self.assertEqual(cfg_a.func, dummy_function1) - self.assertIsInstance(cfg_b.func, type(MISSING)) - - # Explicitly assert that members are not the same object - # for different levels and kinds of data types - self.assertIsNot(cfg_a.m, cfg_b.m) - self.assertIsNot(cfg_a.m.rot, cfg_b.m.rot) - self.assertIsNot(cfg_a.i, cfg_b.i) - self.assertIsNot(cfg_a.b, cfg_b.b) - - def test_config_double_inheritance(self): - """Tests that inheritance works properly when inheriting twice.""" - # check variables - cfg = ChildChildDemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) - - self.assertEqual(cfg.func, dummy_function1) - self.assertEqual(cfg.func_2, dummy_function2) - self.assertEqual(cfg.a, 20) - self.assertEqual(cfg.d, 3) - self.assertEqual(cfg.j, ["c", "d"]) - - # check post init - self.assertEqual(cfg.b, 4) - self.assertEqual(cfg.f, "new") - self.assertEqual(cfg.i, ["a", "b"]) - - def test_config_with_class_type(self): - """Tests that configclass works properly with class type.""" - - cfg = DummyClassCfg() - - # since python 3.10, annotations are stored as strings - annotations = {k: eval(v) if isinstance(v, str) else v for k, v in cfg.__annotations__.items()} - # check types - self.assertEqual(annotations["class_name_1"], type) - self.assertEqual(annotations["class_name_2"], type[DummyClass]) - self.assertEqual(annotations["class_name_3"], type[DummyClass]) - self.assertEqual(annotations["class_name_4"], ClassVar[type[DummyClass]]) - # check values - self.assertEqual(cfg.class_name_1, DummyClass) - self.assertEqual(cfg.class_name_2, DummyClass) - self.assertEqual(cfg.class_name_3, DummyClass) - self.assertEqual(cfg.class_name_4, DummyClass) - self.assertEqual(cfg.b, "dummy") - - def test_nested_config_class_declarations(self): - """Tests that configclass works properly with nested class class declarations.""" - - cfg = OutsideClassCfg() - - # check types - self.assertNotIn("InsideClassCfg", cfg.__annotations__) - self.assertNotIn("InsideClassCfg", OutsideClassCfg.__annotations__) - self.assertNotIn("InsideInsideClassCfg", OutsideClassCfg.InsideClassCfg.__annotations__) - self.assertNotIn("InsideInsideClassCfg", cfg.inside.__annotations__) - # check values - self.assertEqual(cfg.inside.class_type, DummyClass) - self.assertEqual(cfg.inside.b, "dummy_changed") - self.assertEqual(cfg.x, 20) - - def test_config_dumping(self): - """Check that config dumping works properly.""" - - # file for dumping - dirname = os.path.dirname(os.path.abspath(__file__)) - filename = os.path.join(dirname, "output", "configclass", "test_config.yaml") - - # create config - cfg = ChildADemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) - - # save config - dump_yaml(filename, cfg) - # load config - cfg_loaded = load_yaml(filename) - # check dictionaries are the same - self.assertEqual(list(cfg.to_dict().keys()), list(cfg_loaded.keys())) - self.assertDictEqual(cfg.to_dict(), cfg_loaded) - - # save config with sorted order won't work! - # save config - dump_yaml(filename, cfg, sort_keys=True) - # load config - cfg_loaded = load_yaml(filename) - # check dictionaries are the same - self.assertNotEqual(list(cfg.to_dict().keys()), list(cfg_loaded.keys())) - self.assertDictEqual(cfg.to_dict(), cfg_loaded) - - def test_config_md5_hash(self): - """Check that config md5 hash generation works properly.""" - - # create config - cfg = ChildADemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) - - # generate md5 hash - md5_hash_1 = dict_to_md5_hash(cfg.to_dict()) - md5_hash_2 = dict_to_md5_hash(cfg.to_dict()) - - self.assertEqual(md5_hash_1, md5_hash_2) - - def test_validity(self): - """Check that invalid configurations raise errors.""" - - cfg = MissingChildDemoCfg() - - with self.assertRaises(TypeError) as context: - cfg.validate() - - # check that the expected missing fields are in the error message - error_message = str(context.exception) - for elem in validity_expected_fields: - self.assertIn(elem, error_message) - - # check that no more than the expected missing fields are in the error message - self.assertEqual(len(error_message.split("\n")) - 2, len(validity_expected_fields)) - - -if __name__ == "__main__": - run_tests() + c = MISSING + + +def test_missing_default_value_in_config(): + """Tests missing default value in config. + + Should complain that 'a' is missing default value since it cannot be inferred + from type annotation. + """ + with pytest.raises(ValueError): + + @configclass + class MissingTypeDemoCfg: + a: int + b = 2 + + +def test_required_argument_for_missing_type_in_config(): + """Tests required positional argument for missing type annotation in config creation.""" + + @configclass + class MissingTypeDemoCfg: + a: int = 1 + b = 2 + c: int = MISSING + + # should complain that 'c' is missed in positional arguments + # TODO: Uncomment this when we move to 3.10. + # with self.assertRaises(TypeError): + # cfg = MissingTypeDemoCfg(a=1) + # should not complain + cfg = MissingTypeDemoCfg(a=1, c=3) + + assert cfg.a == 1 + assert cfg.b == 2 + + +def test_config_inheritance(): + """Tests that inheritance works properly.""" + # check variables + cfg_a = ChildADemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) + + assert cfg_a.func == dummy_function1 + assert cfg_a.a == 20 + assert cfg_a.d == 3 + assert cfg_a.j == ["c", "d"] + + # check post init + assert cfg_a.b == 3 + assert cfg_a.i == ["a", "b"] + assert cfg_a.m.rot == (2.0, 0.0, 0.0, 0.0) + + +def test_config_inheritance_independence(): + """Tests that subclass instantions have fully unique members, + rather than references to members of the parent class""" + # instantiate two classes which inherit from a shared parent, + # but which will differently modify their members in their + # __init__ and __post_init__ + cfg_a = ChildADemoCfg() + cfg_b = ChildBDemoCfg() + + # Test various combinations of initialization + # and defaults across inherited members in + # instances to verify independence between the subclasses + assert isinstance(cfg_a.a, type(MISSING)) + assert cfg_b.a == 100 + assert cfg_a.b == 3 + assert cfg_b.b == 8 + assert cfg_a.c == RobotDefaultStateCfg() + assert isinstance(cfg_b.c, type(MISSING)) + assert cfg_a.m.rot == (2.0, 0.0, 0.0, 0.0) + assert cfg_b.m.rot == (1.0, 0.0, 0.0, 0.0) + assert isinstance(cfg_a.j, type(MISSING)) + assert cfg_b.j == ["3", "4"] + assert cfg_a.i == ["a", "b"] + assert cfg_b.i == ["1", "2"] + assert cfg_a.func == dummy_function1 + assert isinstance(cfg_b.func, type(MISSING)) + + # Explicitly assert that members are not the same object + # for different levels and kinds of data types + assert cfg_a.m != cfg_b.m + assert cfg_a.m.rot != cfg_b.m.rot + assert cfg_a.i != cfg_b.i + assert cfg_a.b != cfg_b.b + + +def test_config_double_inheritance(): + """Tests that inheritance works properly when inheriting twice.""" + # check variables + cfg = ChildChildDemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) + + assert cfg.func == dummy_function1 + assert cfg.func_2 == dummy_function2 + assert cfg.a == 20 + assert cfg.d == 3 + assert cfg.j == ["c", "d"] + + # check post init + assert cfg.b == 4 + assert cfg.f == "new" + assert cfg.i == ["a", "b"] + + +def test_config_with_class_type(): + """Tests that configclass works properly with class type.""" + + cfg = DummyClassCfg() + + # since python 3.10, annotations are stored as strings + annotations = {k: eval(v) if isinstance(v, str) else v for k, v in cfg.__annotations__.items()} + # check types + assert annotations["class_name_1"] is type + assert annotations["class_name_2"] == type[DummyClass] + assert annotations["class_name_3"] == type[DummyClass] + assert annotations["class_name_4"] is ClassVar[type[DummyClass]] + # check values + assert cfg.class_name_1 == DummyClass + assert cfg.class_name_2 == DummyClass + assert cfg.class_name_3 == DummyClass + assert cfg.class_name_4 == DummyClass + assert cfg.b == "dummy" + + +def test_nested_config_class_declarations(): + """Tests that configclass works properly with nested class class declarations.""" + + cfg = OutsideClassCfg() + + # check types + assert "InsideClassCfg" not in cfg.__annotations__ + assert "InsideClassCfg" not in OutsideClassCfg.__annotations__ + assert "InsideInsideClassCfg" not in OutsideClassCfg.InsideClassCfg.__annotations__ + assert "InsideInsideClassCfg" not in cfg.inside.__annotations__ + # check values + assert cfg.inside.class_type == DummyClass + assert cfg.inside.b == "dummy_changed" + assert cfg.x == 20 + + +def test_config_dumping(): + """Check that config dumping works properly.""" + + # file for dumping + dirname = os.path.dirname(os.path.abspath(__file__)) + filename = os.path.join(dirname, "output", "configclass", "test_config.yaml") + + # create config + cfg = ChildADemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) + + # save config + dump_yaml(filename, cfg) + # load config + cfg_loaded = load_yaml(filename) + # check dictionaries are the same + assert list(cfg.to_dict().keys()) == list(cfg_loaded.keys()) + assert cfg.to_dict() == cfg_loaded + + # save config with sorted order won't work! + # save config + dump_yaml(filename, cfg, sort_keys=True) + # load config + cfg_loaded = load_yaml(filename) + # check dictionaries are the same + assert list(cfg.to_dict().keys()) != list(cfg_loaded.keys()) + assert cfg.to_dict() == cfg_loaded + + +def test_config_md5_hash(): + """Check that config md5 hash generation works properly.""" + + # create config + cfg = ChildADemoCfg(a=20, d=3, e=ViewerCfg(), j=["c", "d"]) + + # generate md5 hash + md5_hash_1 = dict_to_md5_hash(cfg.to_dict()) + md5_hash_2 = dict_to_md5_hash(cfg.to_dict()) + + assert md5_hash_1 == md5_hash_2 + + +def test_validity(): + """Check that invalid configurations raise errors.""" + + cfg = MissingChildDemoCfg() + + with pytest.raises(TypeError) as context: + cfg.validate() + + # check that the expected missing fields are in the error message + error_message = str(context.value) + for elem in validity_expected_fields: + assert elem in error_message + + # check that no more than the expected missing fields are in the error message + assert len(error_message.split("\n")) - 2 == len(validity_expected_fields) diff --git a/source/isaaclab/test/utils/test_delay_buffer.py b/source/isaaclab/test/utils/test_delay_buffer.py index b1b9a56d424c..a66802e72978 100644 --- a/source/isaaclab/test/utils/test_delay_buffer.py +++ b/source/isaaclab/test/utils/test_delay_buffer.py @@ -1,99 +1,100 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app in headless mode simulation_app = AppLauncher(headless=True).app """Rest everything follows from here.""" -import torch -import unittest from collections.abc import Generator -from isaaclab.utils import DelayBuffer - - -class TestDelayBuffer(unittest.TestCase): - """Test fixture for checking the delay buffer implementation.""" - - def setUp(self): - self.device: str = "cpu" - self.batch_size: int = 10 - self.history_length: int = 4 - # create the buffer - self.buffer = DelayBuffer(self.history_length, batch_size=self.batch_size, device=self.device) +import pytest +import torch - def test_constant_time_lags(self): - """Test constant delay.""" - const_lag: int = 3 +from isaaclab.utils import DelayBuffer - self.buffer.set_time_lag(const_lag) - all_data = [] - for i, data in enumerate(self._generate_data(20)): - all_data.append(data) - # apply delay - delayed_data = self.buffer.compute(data) +@pytest.fixture +def delay_buffer(): + """Create a delay buffer for testing.""" + device: str = "cpu" + batch_size: int = 10 + history_length: int = 4 + return DelayBuffer(history_length, batch_size=batch_size, device=device) + + +def _generate_data(batch_size: int, length: int, device: str) -> Generator[torch.Tensor]: + """Data generator for testing the buffer.""" + for step in range(length): + yield torch.full((batch_size, 1), step, dtype=torch.int, device=device) + + +def test_constant_time_lags(delay_buffer): + """Test constant delay.""" + const_lag: int = 3 + batch_size: int = 10 + + delay_buffer.set_time_lag(const_lag) + + all_data = [] + for i, data in enumerate(_generate_data(batch_size, 20, delay_buffer.device)): + all_data.append(data) + # apply delay + delayed_data = delay_buffer.compute(data) + error = delayed_data - all_data[max(0, i - const_lag)] + assert torch.all(error == 0) + + +def test_reset(delay_buffer): + """Test resetting the last two batch indices after iteration `reset_itr`.""" + const_lag: int = 2 + reset_itr = 10 + batch_size: int = 10 + + delay_buffer.set_time_lag(const_lag) + + all_data = [] + for i, data in enumerate(_generate_data(batch_size, 20, delay_buffer.device)): + all_data.append(data) + # from 'reset_itr' iteration reset the last and second-to-last environments + if i == reset_itr: + delay_buffer.reset([-2, -1]) + # apply delay + delayed_data = delay_buffer.compute(data) + # before 'reset_itr' is is similar to test_constant_time_lags + # after that indices [-2, -1] should be treated separately + if i < reset_itr: error = delayed_data - all_data[max(0, i - const_lag)] - self.assertTrue(torch.all(error == 0)) - - def test_reset(self): - """Test resetting the last two batch indices after iteration `reset_itr`.""" - const_lag: int = 2 - reset_itr = 10 - - self.buffer.set_time_lag(const_lag) - - all_data = [] - for i, data in enumerate(self._generate_data(20)): - all_data.append(data) - # from 'reset_itr' iteration reset the last and second-to-last environments - if i == reset_itr: - self.buffer.reset([-2, -1]) - # apply delay - delayed_data = self.buffer.compute(data) - # before 'reset_itr' is is similar to test_constant_time_lags - # after that indices [-2, -1] should be treated separately - if i < reset_itr: - error = delayed_data - all_data[max(0, i - const_lag)] - self.assertTrue(torch.all(error == 0)) - else: - # error_regular = delayed_data[:-2] - all_data[max(0, i - const_lag)][:-2] - error2_reset = delayed_data[-2, -1] - all_data[max(reset_itr, i - const_lag)][-2, -1] - # self.assertTrue(torch.all(error_regular == 0)) - self.assertTrue(torch.all(error2_reset == 0)) - - def test_random_time_lags(self): - """Test random delays.""" - max_lag: int = 3 - time_lags = torch.randint(low=0, high=max_lag + 1, size=(self.batch_size,), dtype=torch.int, device=self.device) - - self.buffer.set_time_lag(time_lags) - - all_data = [] - for i, data in enumerate(self._generate_data(20)): - all_data.append(data) - # apply delay - delayed_data = self.buffer.compute(data) - true_delayed_index = torch.maximum(i - self.buffer.time_lags, torch.zeros_like(self.buffer.time_lags)) - true_delayed_index = true_delayed_index.tolist() - for i in range(self.batch_size): - error = delayed_data[i] - all_data[true_delayed_index[i]][i] - self.assertTrue(torch.all(error == 0)) - - """Helper functions.""" - - def _generate_data(self, length: int) -> Generator[torch.Tensor]: - """Data generator for testing the buffer.""" - for step in range(length): - yield torch.full((self.batch_size, 1), step, dtype=torch.int, device=self.device) - - -if __name__ == "__main__": - run_tests() + assert torch.all(error == 0) + else: + # error_regular = delayed_data[:-2] - all_data[max(0, i - const_lag)][:-2] + error2_reset = delayed_data[-2, -1] - all_data[max(reset_itr, i - const_lag)][-2, -1] + # assert torch.all(error_regular == 0) + assert torch.all(error2_reset == 0) + + +def test_random_time_lags(delay_buffer): + """Test random delays.""" + max_lag: int = 3 + time_lags = torch.randint( + low=0, high=max_lag + 1, size=(delay_buffer.batch_size,), dtype=torch.int, device=delay_buffer.device + ) + + delay_buffer.set_time_lag(time_lags) + + all_data = [] + for i, data in enumerate(_generate_data(delay_buffer.batch_size, 20, delay_buffer.device)): + all_data.append(data) + # apply delay + delayed_data = delay_buffer.compute(data) + true_delayed_index = torch.maximum(i - delay_buffer.time_lags, torch.zeros_like(delay_buffer.time_lags)) + true_delayed_index = true_delayed_index.tolist() + for i in range(delay_buffer.batch_size): + error = delayed_data[i] - all_data[true_delayed_index[i]][i] + assert torch.all(error == 0) diff --git a/source/isaaclab/test/utils/test_dict.py b/source/isaaclab/test/utils/test_dict.py index daf24b4d13ed..35ce35f26577 100644 --- a/source/isaaclab/test/utils/test_dict.py +++ b/source/isaaclab/test/utils/test_dict.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,98 +7,93 @@ # because warp is only available in the context of a running simulation """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import random -import unittest import isaaclab.utils.dict as dict_utils -def test_function(x): +def _test_function(x): """Test function for string <-> callable conversion.""" return x**2 -def test_lambda_function(x): +def _test_lambda_function(x): """Test function for string <-> callable conversion.""" return x**2 -class TestDictUtilities(unittest.TestCase): - """Test fixture for checking dictionary utilities in Isaac Lab.""" - - def test_print_dict(self): - """Test printing of dictionary.""" - # create a complex nested dictionary - test_dict = { - "a": 1, - "b": 2, - "c": {"d": 3, "e": 4, "f": {"g": 5, "h": 6}}, - "i": 7, - "j": lambda x: x**2, # noqa: E731 - "k": dict_utils.class_to_dict, - } - # print the dictionary - dict_utils.print_dict(test_dict) - - def test_string_callable_function_conversion(self): - """Test string <-> callable conversion for function.""" - - # convert function to string - test_string = dict_utils.callable_to_string(test_function) - # convert string to function - test_function_2 = dict_utils.string_to_callable(test_string) - # check that functions are the same - self.assertEqual(test_function(2), test_function_2(2)) - - def test_string_callable_function_with_lambda_in_name_conversion(self): - """Test string <-> callable conversion for function which has lambda in its name.""" - - # convert function to string - test_string = dict_utils.callable_to_string(test_lambda_function) - # convert string to function - test_function_2 = dict_utils.string_to_callable(test_string) - # check that functions are the same - self.assertEqual(test_function(2), test_function_2(2)) - - def test_string_callable_lambda_conversion(self): - """Test string <-> callable conversion for lambda expression.""" - - # create lambda function - func = lambda x: x**2 # noqa: E731 - # convert function to string - test_string = dict_utils.callable_to_string(func) - # convert string to function - func_2 = dict_utils.string_to_callable(test_string) - # check that functions are the same - self.assertEqual(test_string, "lambda x: x**2") - self.assertEqual(func(2), func_2(2)) - - def test_dict_to_md5(self): - """Test MD5 hash generation for dictionary.""" - # create a complex nested dictionary - test_dict = { - "a": 1, - "b": 2, - "c": {"d": 3, "e": 4, "f": {"g": 5, "h": 6}}, - "i": random.random(), - "k": dict_utils.callable_to_string(dict_utils.class_to_dict), - } - # generate the MD5 hash - md5_hash_1 = dict_utils.dict_to_md5_hash(test_dict) - - # check that the hash is correct even after multiple calls - for _ in range(200): - md5_hash_2 = dict_utils.dict_to_md5_hash(test_dict) - self.assertEqual(md5_hash_1, md5_hash_2) - - -if __name__ == "__main__": - run_tests() +def test_print_dict(): + """Test printing of dictionary.""" + # create a complex nested dictionary + test_dict = { + "a": 1, + "b": 2, + "c": {"d": 3, "e": 4, "f": {"g": 5, "h": 6}}, + "i": 7, + "j": lambda x: x**2, # noqa: E731 + "k": dict_utils.class_to_dict, + } + # print the dictionary + dict_utils.print_dict(test_dict) + + +def test_string_callable_function_conversion(): + """Test string <-> callable conversion for function.""" + + # convert function to string + test_string = dict_utils.callable_to_string(_test_function) + # convert string to function + test_function_2 = dict_utils.string_to_callable(test_string) + # check that functions are the same + assert _test_function(2) == test_function_2(2) + + +def test_string_callable_function_with_lambda_in_name_conversion(): + """Test string <-> callable conversion for function which has lambda in its name.""" + + # convert function to string + test_string = dict_utils.callable_to_string(_test_lambda_function) + # convert string to function + test_function_2 = dict_utils.string_to_callable(test_string) + # check that functions are the same + assert _test_function(2) == test_function_2(2) + + +def test_string_callable_lambda_conversion(): + """Test string <-> callable conversion for lambda expression.""" + + # create lambda function + func = lambda x: x**2 # noqa: E731 + # convert function to string + test_string = dict_utils.callable_to_string(func) + # convert string to function + func_2 = dict_utils.string_to_callable(test_string) + # check that functions are the same + assert test_string == "lambda x: x**2" + assert func(2) == func_2(2) + + +def test_dict_to_md5(): + """Test MD5 hash generation for dictionary.""" + # create a complex nested dictionary + test_dict = { + "a": 1, + "b": 2, + "c": {"d": 3, "e": 4, "f": {"g": 5, "h": 6}}, + "i": random.random(), + "k": dict_utils.callable_to_string(dict_utils.class_to_dict), + } + # generate the MD5 hash + md5_hash_1 = dict_utils.dict_to_md5_hash(test_dict) + + # check that the hash is correct even after multiple calls + for _ in range(200): + md5_hash_2 = dict_utils.dict_to_md5_hash(test_dict) + assert md5_hash_1 == md5_hash_2 diff --git a/source/isaaclab/test/utils/test_episode_data.py b/source/isaaclab/test/utils/test_episode_data.py index 9541cf1e6ec7..e7d14adc8aa6 100644 --- a/source/isaaclab/test/utils/test_episode_data.py +++ b/source/isaaclab/test/utils/test_episode_data.py @@ -1,140 +1,153 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app in headless mode simulation_app = AppLauncher(headless=True).app """Rest everything follows from here.""" +import pytest import torch -import unittest from isaaclab.utils.datasets import EpisodeData -class TestEpisodeData(unittest.TestCase): - """Test EpisodeData implementation.""" - - """ - Test cases for EpisodeData class. - """ - - def test_is_empty(self): - """Test checking whether the episode is empty.""" - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - episode = EpisodeData() - self.assertTrue(episode.is_empty()) - - episode.add("key", torch.tensor([1, 2, 3], device=device)) - self.assertFalse(episode.is_empty()) - - def test_add_tensors(self): - """Test appending tensor data to the episode.""" - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - dummy_data_0 = torch.tensor([0], device=device) - dummy_data_1 = torch.tensor([1], device=device) - expected_added_data = torch.cat((dummy_data_0.unsqueeze(0), dummy_data_1.unsqueeze(0))) - episode = EpisodeData() - - # test adding data to a key that does not exist - episode.add("key", dummy_data_0) - self.assertTrue(torch.equal(episode.data.get("key"), dummy_data_0.unsqueeze(0))) - - # test adding data to a key that exists - episode.add("key", dummy_data_1) - self.assertTrue(torch.equal(episode.data.get("key"), expected_added_data)) - - # test adding data to a key with "/" in the name - episode.add("first/second", dummy_data_0) - self.assertTrue(torch.equal(episode.data.get("first").get("second"), dummy_data_0.unsqueeze(0))) - - # test adding data to a key with "/" in the name that already exists - episode.add("first/second", dummy_data_1) - self.assertTrue(torch.equal(episode.data.get("first").get("second"), expected_added_data)) - - def test_add_dict_tensors(self): - """Test appending dict data to the episode.""" - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - dummy_dict_data_0 = { - "key_0": torch.tensor([0], device=device), - "key_1": {"key_1_0": torch.tensor([1], device=device), "key_1_1": torch.tensor([2], device=device)}, - } - dummy_dict_data_1 = { - "key_0": torch.tensor([3], device=device), - "key_1": {"key_1_0": torch.tensor([4], device=device), "key_1_1": torch.tensor([5], device=device)}, - } - - episode = EpisodeData() - - # test adding dict data to a key that does not exist - episode.add("key", dummy_dict_data_0) - self.assertTrue(torch.equal(episode.data.get("key").get("key_0"), torch.tensor([[0]], device=device))) - self.assertTrue( - torch.equal(episode.data.get("key").get("key_1").get("key_1_0"), torch.tensor([[1]], device=device)) - ) - self.assertTrue( - torch.equal(episode.data.get("key").get("key_1").get("key_1_1"), torch.tensor([[2]], device=device)) - ) - - # test adding dict data to a key that exists - episode.add("key", dummy_dict_data_1) - self.assertTrue( - torch.equal(episode.data.get("key").get("key_0"), torch.tensor([[0], [3]], device=device)) - ) - self.assertTrue( - torch.equal( - episode.data.get("key").get("key_1").get("key_1_0"), torch.tensor([[1], [4]], device=device) - ) - ) - self.assertTrue( - torch.equal( - episode.data.get("key").get("key_1").get("key_1_1"), torch.tensor([[2], [5]], device=device) - ) - ) - - def test_get_initial_state(self): - """Test getting the initial state of the episode.""" - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - dummy_initial_state = torch.tensor([1, 2, 3], device=device) - episode = EpisodeData() - - episode.add("initial_state", dummy_initial_state) - self.assertTrue(torch.equal(episode.get_initial_state(), dummy_initial_state.unsqueeze(0))) - - def test_get_next_action(self): - """Test getting next actions.""" - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - # dummy actions - action1 = torch.tensor([1, 2, 3], device=device) - action2 = torch.tensor([4, 5, 6], device=device) - action3 = torch.tensor([7, 8, 9], device=device) - - episode = EpisodeData() - self.assertIsNone(episode.get_next_action()) - - episode.add("actions", action1) - episode.add("actions", action2) - episode.add("actions", action3) - - # check if actions are returned in the correct order - self.assertTrue(torch.equal(episode.get_next_action(), action1)) - self.assertTrue(torch.equal(episode.get_next_action(), action2)) - self.assertTrue(torch.equal(episode.get_next_action(), action3)) - - # check if None is returned when all actions are exhausted - self.assertIsNone(episode.get_next_action()) - - -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_is_empty(device): + """Test checking whether the episode is empty.""" + episode = EpisodeData() + assert episode.is_empty() + + episode.add("key", torch.tensor([1, 2, 3], device=device)) + assert not episode.is_empty() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_add_tensors(device): + """Test appending tensor data to the episode.""" + dummy_data_0 = torch.tensor([0], device=device) + dummy_data_1 = torch.tensor([1], device=device) + expected_added_data = torch.cat((dummy_data_0.unsqueeze(0), dummy_data_1.unsqueeze(0))) + episode = EpisodeData() + + # test adding data to a key that does not exist + episode.add("key", dummy_data_0) + key_data = torch.stack(episode.data.get("key")) + assert key_data is not None + assert torch.equal(key_data, dummy_data_0.unsqueeze(0)) + + # test adding data to a key that exists + episode.add("key", dummy_data_1) + key_data = torch.stack(episode.data.get("key")) + assert key_data is not None + assert torch.equal(key_data, expected_added_data) + + # test adding data to a key with "/" in the name + episode.add("first/second", dummy_data_0) + first_data = episode.data.get("first") + assert first_data is not None + second_data = torch.stack(first_data.get("second")) + assert second_data is not None + assert torch.equal(second_data, dummy_data_0.unsqueeze(0)) + + # test adding data to a key with "/" in the name that already exists + episode.add("first/second", dummy_data_1) + first_data = episode.data.get("first") + assert first_data is not None + second_data = torch.stack(first_data.get("second")) + assert second_data is not None + assert torch.equal(second_data, expected_added_data) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_add_dict_tensors(device): + """Test appending dict data to the episode.""" + dummy_dict_data_0 = { + "key_0": torch.tensor([0], device=device), + "key_1": {"key_1_0": torch.tensor([1], device=device), "key_1_1": torch.tensor([2], device=device)}, + } + dummy_dict_data_1 = { + "key_0": torch.tensor([3], device=device), + "key_1": {"key_1_0": torch.tensor([4], device=device), "key_1_1": torch.tensor([5], device=device)}, + } + + episode = EpisodeData() + + # test adding dict data to a key that does not exist + episode.add("key", dummy_dict_data_0) + key_data = episode.data.get("key") + assert key_data is not None + key_0_data = torch.stack(key_data.get("key_0")) + assert key_0_data is not None + assert torch.equal(key_0_data, torch.tensor([[0]], device=device)) + key_1_data = key_data.get("key_1") + assert key_1_data is not None + key_1_0_data = torch.stack(key_1_data.get("key_1_0")) + assert key_1_0_data is not None + assert torch.equal(key_1_0_data, torch.tensor([[1]], device=device)) + key_1_1_data = torch.stack(key_1_data.get("key_1_1")) + assert key_1_1_data is not None + assert torch.equal(key_1_1_data, torch.tensor([[2]], device=device)) + + # test adding dict data to a key that exists + episode.add("key", dummy_dict_data_1) + key_data = episode.data.get("key") + assert key_data is not None + key_0_data = torch.stack(key_data.get("key_0")) + assert key_0_data is not None + assert torch.equal(key_0_data, torch.tensor([[0], [3]], device=device)) + key_1_data = key_data.get("key_1") + assert key_1_data is not None + key_1_0_data = torch.stack(key_1_data.get("key_1_0")) + assert key_1_0_data is not None + assert torch.equal(key_1_0_data, torch.tensor([[1], [4]], device=device)) + key_1_1_data = torch.stack(key_1_data.get("key_1_1")) + assert key_1_1_data is not None + assert torch.equal(key_1_1_data, torch.tensor([[2], [5]], device=device)) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_get_initial_state(device): + """Test getting the initial state of the episode.""" + dummy_initial_state = torch.tensor([1, 2, 3], device=device) + episode = EpisodeData() + + episode.add("initial_state", dummy_initial_state) + initial_state = torch.stack(episode.get_initial_state()) + assert initial_state is not None + assert torch.equal(initial_state, dummy_initial_state.unsqueeze(0)) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_get_next_action(device): + """Test getting next actions.""" + # dummy actions + action1 = torch.tensor([1, 2, 3], device=device) + action2 = torch.tensor([4, 5, 6], device=device) + action3 = torch.tensor([7, 8, 9], device=device) + + episode = EpisodeData() + assert episode.get_next_action() is None + + episode.add("actions", action1) + episode.add("actions", action2) + episode.add("actions", action3) + + # check if actions are returned in the correct order + next_action = episode.get_next_action() + assert next_action is not None + assert torch.equal(next_action, action1) + next_action = episode.get_next_action() + assert next_action is not None + assert torch.equal(next_action, action2) + next_action = episode.get_next_action() + assert next_action is not None + assert torch.equal(next_action, action3) + + # check if None is returned when all actions are exhausted + assert episode.get_next_action() is None diff --git a/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py b/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py index fa8538e90d17..123ee95a1157 100644 --- a/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py +++ b/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py @@ -1,11 +1,10 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app in headless mode simulation_app = AppLauncher(headless=True).app @@ -15,10 +14,11 @@ import os import shutil import tempfile -import torch -import unittest import uuid +import pytest +import torch + from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler @@ -42,88 +42,79 @@ def create_test_episode(device): return test_episode -class TestHDF5DatasetFileHandler(unittest.TestCase): - """Test HDF5 dataset filer handler implementation.""" - - """ - Test cases for HDF5DatasetFileHandler class. - """ - - def setUp(self): - # create a temporary directory to store the test datasets - self.temp_dir = tempfile.mkdtemp() - - def tearDown(self): - # delete the temporary directory after the test - shutil.rmtree(self.temp_dir) +@pytest.fixture +def temp_dir(): + """Create a temporary directory for test datasets.""" + temp_dir = tempfile.mkdtemp() + yield temp_dir + # cleanup after tests + shutil.rmtree(temp_dir) - def test_create_dataset_file(self): - """Test creating a new dataset file.""" - # create a dataset file given a file name with extension - dataset_file_path = os.path.join(self.temp_dir, f"{uuid.uuid4()}.hdf5") - dataset_file_handler = HDF5DatasetFileHandler() - dataset_file_handler.create(dataset_file_path, "test_env_name") - dataset_file_handler.close() - # check if the dataset is created - self.assertTrue(os.path.exists(dataset_file_path)) +def test_create_dataset_file(temp_dir): + """Test creating a new dataset file.""" + # create a dataset file given a file name with extension + dataset_file_path = os.path.join(temp_dir, f"{uuid.uuid4()}.hdf5") + dataset_file_handler = HDF5DatasetFileHandler() + dataset_file_handler.create(dataset_file_path, "test_env_name") + dataset_file_handler.close() - # create a dataset file given a file name without extension - dataset_file_path = os.path.join(self.temp_dir, f"{uuid.uuid4()}") - dataset_file_handler = HDF5DatasetFileHandler() - dataset_file_handler.create(dataset_file_path, "test_env_name") - dataset_file_handler.close() + # check if the dataset is created + assert os.path.exists(dataset_file_path) - # check if the dataset is created - self.assertTrue(os.path.exists(dataset_file_path + ".hdf5")) + # create a dataset file given a file name without extension + dataset_file_path = os.path.join(temp_dir, f"{uuid.uuid4()}") + dataset_file_handler = HDF5DatasetFileHandler() + dataset_file_handler.create(dataset_file_path, "test_env_name") + dataset_file_handler.close() - def test_write_and_load_episode(self): - """Test writing and loading an episode to and from the dataset file.""" - for device in ("cuda:0", "cpu"): - with self.subTest(device=device): - dataset_file_path = os.path.join(self.temp_dir, f"{uuid.uuid4()}.hdf5") - dataset_file_handler = HDF5DatasetFileHandler() - dataset_file_handler.create(dataset_file_path, "test_env_name") + # check if the dataset is created + assert os.path.exists(dataset_file_path + ".hdf5") - test_episode = create_test_episode(device) - # write the episode to the dataset - dataset_file_handler.write_episode(test_episode) - dataset_file_handler.flush() +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_write_and_load_episode(temp_dir, device): + """Test writing and loading an episode to and from the dataset file.""" + dataset_file_path = os.path.join(temp_dir, f"{uuid.uuid4()}.hdf5") + dataset_file_handler = HDF5DatasetFileHandler() + dataset_file_handler.create(dataset_file_path, "test_env_name") - self.assertEqual(dataset_file_handler.get_num_episodes(), 1) + test_episode = create_test_episode(device) - # write the episode again to test writing 2nd episode - dataset_file_handler.write_episode(test_episode) - dataset_file_handler.flush() + # write the episode to the dataset + test_episode.pre_export() + dataset_file_handler.write_episode(test_episode) + dataset_file_handler.flush() - self.assertEqual(dataset_file_handler.get_num_episodes(), 2) + assert dataset_file_handler.get_num_episodes() == 1 - # close the dataset file to prepare for testing the load function - dataset_file_handler.close() + # write the episode again to test writing 2nd episode + dataset_file_handler.write_episode(test_episode) + dataset_file_handler.flush() - # load the episode from the dataset - dataset_file_handler = HDF5DatasetFileHandler() - dataset_file_handler.open(dataset_file_path) + assert dataset_file_handler.get_num_episodes() == 2 - self.assertEqual(dataset_file_handler.get_env_name(), "test_env_name") + # close the dataset file to prepare for testing the load function + dataset_file_handler.close() - loaded_episode_names = dataset_file_handler.get_episode_names() - self.assertEqual(len(list(loaded_episode_names)), 2) + # load the episode from the dataset + dataset_file_handler = HDF5DatasetFileHandler() + dataset_file_handler.open(dataset_file_path) - for episode_name in loaded_episode_names: - loaded_episode = dataset_file_handler.load_episode(episode_name, device=device) - self.assertEqual(loaded_episode.env_id, "test_env_name") - self.assertEqual(loaded_episode.seed, test_episode.seed) - self.assertEqual(loaded_episode.success, test_episode.success) + assert dataset_file_handler.get_env_name() == "test_env_name" - self.assertTrue(torch.equal(loaded_episode.get_initial_state(), test_episode.get_initial_state())) + loaded_episode_names = dataset_file_handler.get_episode_names() + assert len(list(loaded_episode_names)) == 2 - for action in test_episode.data["actions"]: - self.assertTrue(torch.equal(loaded_episode.get_next_action(), action)) + for episode_name in loaded_episode_names: + loaded_episode = dataset_file_handler.load_episode(episode_name, device=device) + assert loaded_episode.env_id == "test_env_name" + assert loaded_episode.seed == test_episode.seed + assert loaded_episode.success == test_episode.success - dataset_file_handler.close() + assert torch.equal(loaded_episode.get_initial_state(), test_episode.get_initial_state()) + for action in test_episode.data["actions"]: + assert torch.equal(loaded_episode.get_next_action(), action) -if __name__ == "__main__": - run_tests() + dataset_file_handler.close() diff --git a/source/isaaclab/test/utils/test_logger.py b/source/isaaclab/test/utils/test_logger.py new file mode 100644 index 000000000000..69df76f4c660 --- /dev/null +++ b/source/isaaclab/test/utils/test_logger.py @@ -0,0 +1,725 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for logging utilities.""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import logging +import os +import re +import tempfile +import time + +import pytest + +from isaaclab.utils.logger import ColoredFormatter, RateLimitFilter, configure_logging + + +# Fixtures +@pytest.fixture +def formatter(): + """Fixture providing a ColoredFormatter instance.""" + return ColoredFormatter("%(levelname)s: %(message)s") + + +@pytest.fixture +def test_message(): + """Fixture providing a test message string.""" + return "Test message" + + +@pytest.fixture +def rate_limit_filter(): + """Fixture providing a RateLimitFilter instance with 2 second interval.""" + return RateLimitFilter(interval_seconds=2) + + +""" +Tests for the ColoredFormatter class. +""" + + +def test_info_formatting(formatter, test_message): + """Test INFO level message formatting.""" + record = logging.LogRecord( + name="test", + level=logging.INFO, + pathname="test.py", + lineno=1, + msg=test_message, + args=(), + exc_info=None, + ) + formatted = formatter.format(record) + + # INFO should use reset color (no color) + assert "\033[0m" in formatted + assert test_message in formatted + assert "INFO" in formatted + + +def test_debug_formatting(formatter, test_message): + """Test DEBUG level message formatting.""" + record = logging.LogRecord( + name="test", + level=logging.DEBUG, + pathname="test.py", + lineno=1, + msg=test_message, + args=(), + exc_info=None, + ) + formatted = formatter.format(record) + + # DEBUG should use reset color (no color) + assert "\033[0m" in formatted + assert test_message in formatted + assert "DEBUG" in formatted + + +def test_warning_formatting(formatter, test_message): + """Test WARNING level message formatting.""" + record = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg=test_message, + args=(), + exc_info=None, + ) + formatted = formatter.format(record) + + # WARNING should use yellow/orange color + assert "\033[33m" in formatted + assert test_message in formatted + assert "WARNING" in formatted + # Should end with reset + assert formatted.endswith("\033[0m") + + +def test_error_formatting(formatter, test_message): + """Test ERROR level message formatting.""" + record = logging.LogRecord( + name="test", + level=logging.ERROR, + pathname="test.py", + lineno=1, + msg=test_message, + args=(), + exc_info=None, + ) + formatted = formatter.format(record) + + # ERROR should use red color + assert "\033[31m" in formatted + assert test_message in formatted + assert "ERROR" in formatted + # Should end with reset + assert formatted.endswith("\033[0m") + + +def test_critical_formatting(formatter, test_message): + """Test CRITICAL level message formatting.""" + record = logging.LogRecord( + name="test", + level=logging.CRITICAL, + pathname="test.py", + lineno=1, + msg=test_message, + args=(), + exc_info=None, + ) + formatted = formatter.format(record) + + # CRITICAL should use bold red color + assert "\033[1;31m" in formatted + assert test_message in formatted + assert "CRITICAL" in formatted + # Should end with reset + assert formatted.endswith("\033[0m") + + +def test_color_codes_are_ansi(): + """Test that color codes are valid ANSI escape sequences.""" + # Test all defined colors + for level_name, color_code in ColoredFormatter.COLORS.items(): + # ANSI color codes should match pattern \033[m or \033[;m (for bold, etc.) + assert re.match(r"\033\[[\d;]+m", color_code), f"Invalid ANSI color code for {level_name}" + + # Test reset code + assert re.match(r"\033\[[\d;]+m", ColoredFormatter.RESET), "Invalid ANSI reset code" + + +def test_custom_format_string(test_message): + """Test that custom format strings work correctly.""" + custom_formatter = ColoredFormatter("%(name)s - %(levelname)s - %(message)s") + record = logging.LogRecord( + name="custom.logger", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg=test_message, + args=(), + exc_info=None, + ) + formatted = custom_formatter.format(record) + + assert "custom.logger" in formatted + assert "WARNING" in formatted + assert test_message in formatted + assert "\033[33m" in formatted # Warning color + + +""" +Tests for the RateLimitFilter class. +""" + + +def test_non_warning_messages_pass_through(rate_limit_filter): + """Test that non-WARNING messages always pass through the filter.""" + # Test INFO + info_record = logging.LogRecord( + name="test", + level=logging.INFO, + pathname="test.py", + lineno=1, + msg="Info message", + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(info_record) is True + + # Test ERROR + error_record = logging.LogRecord( + name="test", + level=logging.ERROR, + pathname="test.py", + lineno=1, + msg="Error message", + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(error_record) is True + + # Test DEBUG + debug_record = logging.LogRecord( + name="test", + level=logging.DEBUG, + pathname="test.py", + lineno=1, + msg="Debug message", + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(debug_record) is True + + +def test_first_warning_passes(rate_limit_filter): + """Test that the first WARNING message passes through.""" + record = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg="First warning", + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(record) is True + + +def test_duplicate_warning_within_interval_blocked(rate_limit_filter): + """Test that duplicate WARNING messages within interval are blocked.""" + message = "Duplicate warning" + + # First warning should pass + record1 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg=message, + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(record1) is True + + # Immediate duplicate should be blocked + record2 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=2, + msg=message, + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(record2) is False + + +def test_warning_after_interval_passes(): + """Test that WARNING messages pass after the rate limit interval.""" + message = "Rate limited warning" + filter_short = RateLimitFilter(interval_seconds=1) + + # First warning should pass + record1 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg=message, + args=(), + exc_info=None, + ) + assert filter_short.filter(record1) is True + + # Immediate duplicate should be blocked + record2 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=2, + msg=message, + args=(), + exc_info=None, + ) + assert filter_short.filter(record2) is False + + # Wait for interval to pass + time.sleep(1.1) + + # After interval, same message should pass again + record3 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=3, + msg=message, + args=(), + exc_info=None, + ) + assert filter_short.filter(record3) is True + + +def test_different_warnings_not_rate_limited(rate_limit_filter): + """Test that different WARNING messages are not rate limited together.""" + # First warning + record1 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg="Warning A", + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(record1) is True + + # Different warning should also pass + record2 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=2, + msg="Warning B", + args=(), + exc_info=None, + ) + assert rate_limit_filter.filter(record2) is True + + +def test_custom_interval(): + """Test that custom interval seconds work correctly.""" + custom_filter = RateLimitFilter(interval_seconds=1) + assert custom_filter.interval == 1 + + long_filter = RateLimitFilter(interval_seconds=10) + assert long_filter.interval == 10 + + +def test_last_emitted_tracking(rate_limit_filter): + """Test that the filter correctly tracks last emission times.""" + message1 = "Message 1" + message2 = "Message 2" + + # Emit first message + record1 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg=message1, + args=(), + exc_info=None, + ) + rate_limit_filter.filter(record1) + + # Check that message1 is tracked + assert message1 in rate_limit_filter.last_emitted + + # Emit second message + record2 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=2, + msg=message2, + args=(), + exc_info=None, + ) + rate_limit_filter.filter(record2) + + # Check that both messages are tracked + assert message1 in rate_limit_filter.last_emitted + assert message2 in rate_limit_filter.last_emitted + + # Timestamps should be different (though very close) + assert rate_limit_filter.last_emitted[message1] <= rate_limit_filter.last_emitted[message2] + + +def test_formatted_message_warnings(rate_limit_filter): + """Test rate limiting with formatted WARNING messages.""" + # Test with string formatting + record1 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=1, + msg="Warning: value=%d", + args=(42,), + exc_info=None, + ) + assert rate_limit_filter.filter(record1) is True + + # Same formatted message should be blocked + record2 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=2, + msg="Warning: value=%d", + args=(42,), + exc_info=None, + ) + assert rate_limit_filter.filter(record2) is False + + # Different args create different message, should pass + record3 = logging.LogRecord( + name="test", + level=logging.WARNING, + pathname="test.py", + lineno=3, + msg="Warning: value=%d", + args=(99,), + exc_info=None, + ) + assert rate_limit_filter.filter(record3) is True + + +""" +Integration Tests. + +Tests that the filter and formatter work together in a logger. +""" + + +def test_filter_and_formatter_together(): + """Test that filter and formatter work together in a logger.""" + # Create a logger with both filter and formatter + test_logger = logging.getLogger("test_integration") + test_logger.setLevel(logging.DEBUG) + + # Remove any existing handlers + test_logger.handlers.clear() + + # Create handler with colored formatter + handler = logging.StreamHandler() + handler.setFormatter(ColoredFormatter("%(levelname)s: %(message)s")) + + # Add rate limit filter + rate_filter = RateLimitFilter(interval_seconds=1) + handler.addFilter(rate_filter) + + test_logger.addHandler(handler) + + # Test that logger is set up correctly + assert len(test_logger.handlers) == 1 + assert isinstance(test_logger.handlers[0].formatter, ColoredFormatter) + + # Clean up + test_logger.handlers.clear() + + +def test_default_initialization(): + """Test that classes can be initialized with default parameters.""" + # ColoredFormatter with default format + formatter = ColoredFormatter() + assert formatter is not None + + # RateLimitFilter with default interval + filter_obj = RateLimitFilter() + assert filter_obj.interval == 5 # default is 5 seconds + + +""" +Tests for the configure_logging function. +""" + + +def test_configure_logging_basic(): + """Test basic configure_logging functionality without file logging.""" + # Setup logger without file logging + logger = configure_logging(logging_level="INFO", save_logs_to_file=False) + + # Should return root logger + assert logger is not None + assert logger is logging.getLogger() + # Root logger is always set to DEBUG to ensure all messages are logged + assert logger.level == logging.DEBUG + + # Should have exactly one handler (stream handler) + assert len(logger.handlers) == 1 + + # Stream handler should have ColoredFormatter + stream_handler = logger.handlers[0] + assert isinstance(stream_handler, logging.StreamHandler) + assert isinstance(stream_handler.formatter, ColoredFormatter) + assert stream_handler.level == logging.INFO + + # Should have RateLimitFilter + assert len(stream_handler.filters) > 0 + rate_filter = stream_handler.filters[0] + assert isinstance(rate_filter, RateLimitFilter) + assert rate_filter.interval == 5 + + +def test_configure_logging_with_file(): + """Test configure_logging with file logging enabled.""" + # Setup logger with file logging + with tempfile.TemporaryDirectory() as temp_dir: + logger = configure_logging(logging_level="DEBUG", save_logs_to_file=True, log_dir=temp_dir) + + # Should return root logger + assert logger is not None + # Root logger is always set to DEBUG + assert logger.level == logging.DEBUG + + # Should have two handlers (stream + file) + assert len(logger.handlers) == 2 + + # Check stream handler + stream_handler = logger.handlers[0] + assert isinstance(stream_handler, logging.StreamHandler) + assert isinstance(stream_handler.formatter, ColoredFormatter) + assert stream_handler.level == logging.DEBUG + + # Check file handler + file_handler = logger.handlers[1] + assert isinstance(file_handler, logging.FileHandler) + assert file_handler.level == logging.DEBUG + + # Verify log file was created + log_files = [f for f in os.listdir(temp_dir) if f.startswith("isaaclab_")] + assert len(log_files) == 1 + + +def test_configure_logging_levels(): + """Test configure_logging with different logging levels.""" + from typing import Literal + + levels: list[Literal["DEBUG", "INFO", "WARNING", "ERROR", "CRITICAL"]] = [ + "DEBUG", + "INFO", + "WARNING", + "ERROR", + "CRITICAL", + ] + level_values = { + "DEBUG": logging.DEBUG, + "INFO": logging.INFO, + "WARNING": logging.WARNING, + "ERROR": logging.ERROR, + "CRITICAL": logging.CRITICAL, + } + + for level_str in levels: + logger = configure_logging(logging_level=level_str, save_logs_to_file=False) + # Root logger is always set to DEBUG to ensure all messages are logged + assert logger.level == logging.DEBUG + # Handler level should match the requested level + assert logger.handlers[0].level == level_values[level_str] + + +def test_configure_logging_removes_existing_handlers(): + """Test that configure_logging removes existing handlers.""" + # Get root logger and add a dummy handler + root_logger = logging.getLogger() + dummy_handler = logging.StreamHandler() + root_logger.addHandler(dummy_handler) + + initial_handler_count = len(root_logger.handlers) + assert initial_handler_count > 0 + + # Setup logger should remove existing handlers + logger = configure_logging(logging_level="INFO", save_logs_to_file=False) + + # Should only have the new handler + assert len(logger.handlers) == 1 + assert dummy_handler not in logger.handlers + + +def test_configure_logging_default_log_dir(): + """Test configure_logging uses temp directory when log_dir is None.""" + + logger = configure_logging(logging_level="INFO", save_logs_to_file=True, log_dir=None) + + # Root logger is always set to DEBUG + assert logger.level == logging.DEBUG + + # Should have file handler + assert len(logger.handlers) == 2 + file_handler = logger.handlers[1] + assert isinstance(file_handler, logging.FileHandler) + + # File should be in temp directory + log_file_path = file_handler.baseFilename + assert os.path.dirname(log_file_path) == os.path.join(tempfile.gettempdir(), "isaaclab", "logs") + assert os.path.basename(log_file_path).startswith("isaaclab_") + + # Cleanup + if os.path.exists(log_file_path): + os.remove(log_file_path) + + +def test_configure_logging_custom_log_dir(): + """Test configure_logging with custom log directory.""" + with tempfile.TemporaryDirectory() as temp_dir: + custom_log_dir = os.path.join(temp_dir, "custom_logs") + + logger = configure_logging(logging_level="INFO", save_logs_to_file=True, log_dir=custom_log_dir) + + # Custom directory should be created + assert os.path.exists(custom_log_dir) + assert os.path.isdir(custom_log_dir) + + # Root logger is always set to DEBUG + assert logger.level == logging.DEBUG + + # Log file should be in custom directory + file_handler = logger.handlers[1] + assert isinstance(file_handler, logging.FileHandler) + log_file_path = file_handler.baseFilename + assert os.path.dirname(log_file_path) == custom_log_dir + + +def test_configure_logging_log_file_format(): + """Test that log file has correct timestamp format.""" + with tempfile.TemporaryDirectory() as temp_dir: + logger = configure_logging(logging_level="INFO", save_logs_to_file=True, log_dir=temp_dir) + + # Root logger is always set to DEBUG + assert logger.level == logging.DEBUG + + # Get log file name + file_handler = logger.handlers[1] + assert isinstance(file_handler, logging.FileHandler) + log_file_path = file_handler.baseFilename + log_filename = os.path.basename(log_file_path) + + # Check filename format: isaaclab_YYYY-MM-DD_HH-MM-SS.log + pattern = r"isaaclab_\d{4}-\d{2}-\d{2}_\d{2}-\d{2}-\d{2}\.log" + assert re.match(pattern, log_filename), f"Log filename {log_filename} doesn't match expected pattern" + + +def test_configure_logging_file_formatter(): + """Test that file handler has more detailed formatter than stream handler.""" + with tempfile.TemporaryDirectory() as temp_dir: + logger = configure_logging(logging_level="INFO", save_logs_to_file=True, log_dir=temp_dir) + + # Root logger is always set to DEBUG + assert logger.level == logging.DEBUG + + stream_handler = logger.handlers[0] + file_handler = logger.handlers[1] + + # Stream formatter should exist and be ColoredFormatter + assert stream_handler.formatter is not None + assert isinstance(stream_handler.formatter, ColoredFormatter) + stream_format = stream_handler.formatter._fmt # type: ignore + assert stream_format is not None + assert "%(asctime)s" in stream_format + assert "%(filename)s" in stream_format + + # File formatter should exist and include line numbers + assert file_handler.formatter is not None + assert isinstance(file_handler.formatter, logging.Formatter) + file_format = file_handler.formatter._fmt # type: ignore + assert file_format is not None + assert "%(asctime)s" in file_format + assert "%(lineno)d" in file_format + + # File handler should always use DEBUG level + assert file_handler.level == logging.DEBUG + + +def test_configure_logging_multiple_calls(): + """Test that multiple configure_logging calls properly cleanup.""" + # First setup + logger1 = configure_logging(logging_level="INFO", save_logs_to_file=False) + handler_count_1 = len(logger1.handlers) + + # Second setup should remove previous handlers + logger2 = configure_logging(logging_level="DEBUG", save_logs_to_file=False) + handler_count_2 = len(logger2.handlers) + + # Should be same logger (root logger) + assert logger1 is logger2 + + # Should have same number of handlers (old ones removed) + assert handler_count_1 == handler_count_2 == 1 + + +def test_configure_logging_actual_logging(): + """Test that logger actually logs messages correctly.""" + import io + + # Capture stdout + captured_output = io.StringIO() + + # Setup logger + logger = configure_logging(logging_level="INFO", save_logs_to_file=False) + + # Temporarily redirect handler to captured output + stream_handler = logger.handlers[0] + assert isinstance(stream_handler, logging.StreamHandler) + original_stream = stream_handler.stream # type: ignore + stream_handler.stream = captured_output # type: ignore + + # Log some messages + test_logger = logging.getLogger("test_module") + test_logger.info("Test info message") + test_logger.warning("Test warning message") + test_logger.debug("Test debug message") # Should not appear (level is INFO) + + # Restore original stream + stream_handler.stream = original_stream # type: ignore + + # Check output + output = captured_output.getvalue() + assert "Test info message" in output + assert "Test warning message" in output + assert "Test debug message" not in output # DEBUG < INFO + assert "INFO" in output + assert "WARNING" in output diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index d08feb1cb685..2f256728e9ee 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/source/isaaclab/test/utils/test_math.py @@ -1,16 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import unittest - """Launch Isaac Sim Simulator first. This is only needed because of warp dependency. """ -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app in headless mode simulation_app = AppLauncher(headless=True).app @@ -19,11 +17,13 @@ """Rest everything follows.""" import math +from math import pi as PI + import numpy as np +import pytest import scipy.spatial.transform as scipy_tf import torch import torch.utils.benchmark as benchmark -from math import pi as PI import isaaclab.utils.math as math_utils @@ -35,569 +35,1286 @@ """ -class TestMathUtilities(unittest.TestCase): - """Test fixture for checking math utilities in Isaac Lab.""" +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +@pytest.mark.parametrize("size", ((5, 4, 3), (10, 2))) +def test_scale_unscale_transform(device, size): + """Test scale_transform and unscale_transform.""" + + inputs = torch.tensor(range(math.prod(size)), device=device, dtype=torch.float32).reshape(size) + + # test with same size + scale_same = 2.0 + lower_same = -scale_same * torch.ones(size, device=device) + upper_same = scale_same * torch.ones(size, device=device) + output_same = math_utils.scale_transform(inputs, lower_same, upper_same) + expected_output_same = inputs / scale_same + torch.testing.assert_close(output_same, expected_output_same) + output_unscale_same = math_utils.unscale_transform(output_same, lower_same, upper_same) + torch.testing.assert_close(output_unscale_same, inputs) + + # test with broadcasting + scale_per_batch = 3.0 + lower_per_batch = -scale_per_batch * torch.ones(size[1:], device=device) + upper_per_batch = scale_per_batch * torch.ones(size[1:], device=device) + output_per_batch = math_utils.scale_transform(inputs, lower_per_batch, upper_per_batch) + expected_output_per_batch = inputs / scale_per_batch + torch.testing.assert_close(output_per_batch, expected_output_per_batch) + output_unscale_per_batch = math_utils.unscale_transform(output_per_batch, lower_per_batch, upper_per_batch) + torch.testing.assert_close(output_unscale_per_batch, inputs) + + # test offset between lower and upper + lower_offset = -3.0 * torch.ones(size[1:], device=device) + upper_offset = 2.0 * torch.ones(size[1:], device=device) + output_offset = math_utils.scale_transform(inputs, lower_offset, upper_offset) + expected_output_offset = (inputs + 0.5) / 2.5 + torch.testing.assert_close(output_offset, expected_output_offset) + output_unscale_offset = math_utils.unscale_transform(output_offset, lower_offset, upper_offset) + torch.testing.assert_close(output_unscale_offset, inputs) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +@pytest.mark.parametrize("size", ((5, 4, 3), (10, 2))) +def test_saturate(device, size): + "Test saturate of a tensor of differed shapes and device." + + num_elements = math.prod(size) + input = torch.tensor(range(num_elements), device=device, dtype=torch.float32).reshape(size) + + # testing with same size + lower_same = -2.0 * torch.ones(size, device=device) + upper_same = 2.0 * torch.ones(size, device=device) + output_same = math_utils.saturate(input, lower_same, upper_same) + assert torch.all(torch.greater_equal(output_same, lower_same)).item() + assert torch.all(torch.less_equal(output_same, upper_same)).item() + # testing with broadcasting + lower_per_batch = -2.0 * torch.ones(size[1:], device=device) + upper_per_batch = 3.0 * torch.ones(size[1:], device=device) + output_per_batch = math_utils.saturate(input, lower_per_batch, upper_per_batch) + assert torch.all(torch.greater_equal(output_per_batch, lower_per_batch)).item() + assert torch.all(torch.less_equal(output_per_batch, upper_per_batch)).item() + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +@pytest.mark.parametrize("size", ((5, 4, 3), (10, 2))) +def test_normalize(device, size): + """Test normalize of a tensor along its last dimension and check the norm of that dimension is close to 1.0.""" + + num_elements = math.prod(size) + input = torch.tensor(range(num_elements), device=device, dtype=torch.float32).reshape(size) + output = math_utils.normalize(input) + norm = torch.linalg.norm(output, dim=-1) + torch.testing.assert_close(norm, torch.ones(size[0:-1], device=device)) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_copysign(device): + """Test copysign by copying a sign from both a negative and positive value and + verify that the new sign is the same. + """ + + size = (10, 2) + + input_mag_pos = 2.0 + input_mag_neg = -3.0 + + input = torch.tensor(range(20), device=device, dtype=torch.float32).reshape(size) + value_pos = math_utils.copysign(input_mag_pos, input) + value_neg = math_utils.copysign(input_mag_neg, input) + torch.testing.assert_close(abs(input_mag_pos) * torch.ones_like(input), value_pos) + torch.testing.assert_close(abs(input_mag_neg) * torch.ones_like(input), value_neg) + + input_neg_dim1 = input.clone() + input_neg_dim1[:, 1] = -input_neg_dim1[:, 1] + value_neg_dim1_pos = math_utils.copysign(input_mag_pos, input_neg_dim1) + value_neg_dim1_neg = math_utils.copysign(input_mag_neg, input_neg_dim1) + expected_value_neg_dim1_pos = abs(input_mag_pos) * torch.ones_like(input_neg_dim1) + expected_value_neg_dim1_pos[:, 1] = -expected_value_neg_dim1_pos[:, 1] + expected_value_neg_dim1_neg = abs(input_mag_neg) * torch.ones_like(input_neg_dim1) + expected_value_neg_dim1_neg[:, 1] = -expected_value_neg_dim1_neg[:, 1] + + torch.testing.assert_close(expected_value_neg_dim1_pos, value_neg_dim1_pos) + torch.testing.assert_close(expected_value_neg_dim1_neg, value_neg_dim1_neg) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_is_identity_pose(device): + """Test is_identity_pose method.""" + # Single row identity pose + identity_pos = torch.zeros(3, device=device) + identity_rot = torch.tensor((1.0, 0.0, 0.0, 0.0), device=device) + assert math_utils.is_identity_pose(identity_pos, identity_rot) is True + + # Modified single row pose + identity_pos = torch.tensor([1.0, 0.0, 0.0], device=device) + identity_rot = torch.tensor((1.0, 1.0, 0.0, 0.0), device=device) + assert math_utils.is_identity_pose(identity_pos, identity_rot) is False + + # Multi-row identity pose + identity_pos = torch.zeros(3, 3, device=device) + identity_rot = torch.tensor([[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]], device=device) + assert math_utils.is_identity_pose(identity_pos, identity_rot) is True + + # Modified multi-row pose + identity_pos = torch.tensor([[1.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], device=device) + identity_rot = torch.tensor([[1.0, 1.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]], device=device) + assert math_utils.is_identity_pose(identity_pos, identity_rot) is False + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_axis_angle_from_quat(device): + """Test axis_angle_from_quat method.""" + # Quaternions of the form (2,4) and (2,2,4) + quats = [ + torch.Tensor([[1.0, 0.0, 0.0, 0.0], [0.8418536, 0.142006, 0.0, 0.5206887]]).to(device), + torch.Tensor( + [ + [[1.0, 0.0, 0.0, 0.0], [0.8418536, 0.142006, 0.0, 0.5206887]], + [[1.0, 0.0, 0.0, 0.0], [0.9850375, 0.0995007, 0.0995007, 0.0995007]], + ] + ).to(device), + ] + + # Angles of the form (2,3) and (2,2,3) + angles = [ + torch.Tensor([[0.0, 0.0, 0.0], [0.3, 0.0, 1.1]]).to(device), + torch.Tensor([[[0.0, 0.0, 0.0], [0.3, 0.0, 1.1]], [[0.0, 0.0, 0.0], [0.2, 0.2, 0.2]]]).to(device), + ] + + for quat, angle in zip(quats, angles): + torch.testing.assert_close(math_utils.axis_angle_from_quat(quat), angle) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_axis_angle_from_quat_approximation(device): + """Test the Taylor approximation from axis_angle_from_quat method. + + This test checks for unstable conversions where theta is very small. + """ + # Generate a small rotation quaternion + # Small angle + theta = torch.Tensor([0.0000001]).to(device) + # Arbitrary normalized axis of rotation in rads, (x,y,z) + axis = [-0.302286, 0.205494, -0.930803] + # Generate quaternion + qw = torch.cos(theta / 2) + quat_vect = [qw] + [d * torch.sin(theta / 2) for d in axis] + quaternion = torch.tensor(quat_vect, dtype=torch.float32, device=device) + + # Convert quaternion to axis-angle + axis_angle_computed = math_utils.axis_angle_from_quat(quaternion) + + # Expected axis-angle representation + axis_angle_expected = torch.tensor([theta * d for d in axis], dtype=torch.float32, device=device) + + # Assert that the computed values are close to the expected values + torch.testing.assert_close(axis_angle_computed, axis_angle_expected) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_error_magnitude(device): + """Test quat_error_magnitude method.""" + # No rotation + q1 = torch.Tensor([1, 0, 0, 0]).to(device) + q2 = torch.Tensor([1, 0, 0, 0]).to(device) + expected_diff = torch.Tensor([0.0]).to(device) + q12_diff = math_utils.quat_error_magnitude(q1, q2) + assert math.isclose(q12_diff.item(), expected_diff.item(), rel_tol=1e-5) + + # PI/2 rotation + q1 = torch.Tensor([1.0, 0, 0.0, 0]).to(device) + q2 = torch.Tensor([0.7071068, 0.7071068, 0, 0]).to(device) + expected_diff = torch.Tensor([PI / 2]).to(device) + q12_diff = math_utils.quat_error_magnitude(q1, q2) + assert math.isclose(q12_diff.item(), expected_diff.item(), rel_tol=1e-5) + + # PI rotation + q1 = torch.Tensor([1.0, 0, 0.0, 0]).to(device) + q2 = torch.Tensor([0.0, 0.0, 1.0, 0]).to(device) + expected_diff = torch.Tensor([PI]).to(device) + q12_diff = math_utils.quat_error_magnitude(q1, q2) + assert math.isclose(q12_diff.item(), expected_diff.item(), rel_tol=1e-5) + + # Batched inputs + q1 = torch.stack( + [torch.Tensor([1, 0, 0, 0]), torch.Tensor([1.0, 0, 0.0, 0]), torch.Tensor([1.0, 0, 0.0, 0])], dim=0 + ).to(device) + q2 = torch.stack( + [torch.Tensor([1, 0, 0, 0]), torch.Tensor([0.7071068, 0.7071068, 0, 0]), torch.Tensor([0.0, 0.0, 1.0, 0])], + dim=0, + ).to(device) + expected_diff = ( + torch.stack([torch.Tensor([0.0]), torch.Tensor([PI / 2]), torch.Tensor([PI])], dim=0).flatten().to(device) + ) + q12_diff = math_utils.quat_error_magnitude(q1, q2) + torch.testing.assert_close(q12_diff, expected_diff) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_unique(device): + """Test quat_unique method.""" + # Define test cases + quats = math_utils.random_orientation(num=1024, device=device) + + # Test positive real quaternion + pos_real_quats = math_utils.quat_unique(quats) + + # Test that the real part is positive + assert torch.all(pos_real_quats[:, 0] > 0).item() + + non_pos_indices = quats[:, 0] < 0 + # Check imaginary part have sign flipped if real part is negative + torch.testing.assert_close(pos_real_quats[non_pos_indices], -quats[non_pos_indices]) + torch.testing.assert_close(pos_real_quats[~non_pos_indices], quats[~non_pos_indices]) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_mul_with_quat_unique(device): + """Test quat_mul method with different quaternions. + + This test checks that the quaternion multiplication is consistent when using positive real quaternions + and regular quaternions. It makes sure that the result is the same regardless of the input quaternion sign + (i.e. q and -q are same quaternion in the context of rotations). + """ + + quats_1 = math_utils.random_orientation(num=1024, device=device) + quats_2 = math_utils.random_orientation(num=1024, device=device) + # Make quats positive real + quats_1_pos_real = math_utils.quat_unique(quats_1) + quats_2_pos_real = math_utils.quat_unique(quats_2) + + # Option 1: Direct computation on quaternions + quat_result_1 = math_utils.quat_mul(quats_1, math_utils.quat_conjugate(quats_2)) + quat_result_1 = math_utils.quat_unique(quat_result_1) + + # Option 2: Computation on positive real quaternions + quat_result_2 = math_utils.quat_mul(quats_1_pos_real, math_utils.quat_conjugate(quats_2_pos_real)) + quat_result_2 = math_utils.quat_unique(quat_result_2) + + # Option 3: Mixed computation + quat_result_3 = math_utils.quat_mul(quats_1, math_utils.quat_conjugate(quats_2_pos_real)) + quat_result_3 = math_utils.quat_unique(quat_result_3) + + # Check that the result is close to the expected value + torch.testing.assert_close(quat_result_1, quat_result_2) + torch.testing.assert_close(quat_result_2, quat_result_3) + torch.testing.assert_close(quat_result_3, quat_result_1) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_error_mag_with_quat_unique(device): + """Test quat_error_magnitude method with positive real quaternions.""" + + quats_1 = math_utils.random_orientation(num=1024, device=device) + quats_2 = math_utils.random_orientation(num=1024, device=device) + # Make quats positive real + quats_1_pos_real = math_utils.quat_unique(quats_1) + quats_2_pos_real = math_utils.quat_unique(quats_2) + + # Compute the error + error_1 = math_utils.quat_error_magnitude(quats_1, quats_2) + error_2 = math_utils.quat_error_magnitude(quats_1_pos_real, quats_2_pos_real) + error_3 = math_utils.quat_error_magnitude(quats_1, quats_2_pos_real) + error_4 = math_utils.quat_error_magnitude(quats_1_pos_real, quats_2) + + # Check that the error is close to the expected value + torch.testing.assert_close(error_1, error_2) + torch.testing.assert_close(error_2, error_3) + torch.testing.assert_close(error_3, error_4) + torch.testing.assert_close(error_4, error_1) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_convention_converter(device): + """Test convert_camera_frame_orientation_convention to and from ros, opengl, and world conventions.""" + quat_ros = torch.tensor([[-0.17591989, 0.33985114, 0.82047325, -0.42470819]], device=device) + quat_opengl = torch.tensor([[0.33985113, 0.17591988, 0.42470818, 0.82047324]], device=device) + quat_world = torch.tensor([[-0.3647052, -0.27984815, -0.1159169, 0.88047623]], device=device) + + # from ROS + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "opengl"), quat_opengl + ) + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "world"), quat_world + ) + torch.testing.assert_close(math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "ros"), quat_ros) + # from OpenGL + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "ros"), quat_ros + ) + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world"), quat_world + ) + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "opengl"), quat_opengl + ) + # from World + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "ros"), quat_ros + ) + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "opengl"), quat_opengl + ) + torch.testing.assert_close( + math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "world"), quat_world + ) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +@pytest.mark.parametrize("size", ((10, 4), (5, 3, 4))) +def test_convert_quat(device, size): + """Test convert_quat from "xyzw" to "wxyz" and back to "xyzw" and verify the correct rolling of the tensor. + + Also check the correct exceptions are raised for bad inputs for the quaternion and the 'to'. + """ + + quat = torch.zeros(size, device=device) + quat[..., 0] = 1.0 + + value_default = math_utils.convert_quat(quat) + expected_default = torch.zeros(size, device=device) + expected_default[..., -1] = 1.0 + torch.testing.assert_close(expected_default, value_default) + + value_to_xyzw = math_utils.convert_quat(quat, to="xyzw") + expected_to_xyzw = torch.zeros(size, device=device) + expected_to_xyzw[..., -1] = 1.0 + torch.testing.assert_close(expected_to_xyzw, value_to_xyzw) + + value_to_wxyz = math_utils.convert_quat(quat, to="wxyz") + expected_to_wxyz = torch.zeros(size, device=device) + expected_to_wxyz[..., 1] = 1.0 + torch.testing.assert_close(expected_to_wxyz, value_to_wxyz) + + bad_quat = torch.zeros((10, 5), device=device) + + with pytest.raises(ValueError): + math_utils.convert_quat(bad_quat) + + with pytest.raises(ValueError): + math_utils.convert_quat(quat, to="xwyz") + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_quat_conjugate(device): + """Test quat_conjugate by checking the sign of the imaginary part changes but the magnitudes stay the same.""" + + quat = math_utils.random_orientation(1000, device=device) + + value = math_utils.quat_conjugate(quat) + expected_real = quat[..., 0] + expected_imag = -quat[..., 1:] + torch.testing.assert_close(expected_real, value[..., 0]) + torch.testing.assert_close(expected_imag, value[..., 1:]) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +@pytest.mark.parametrize("num_envs", (1, 10)) +@pytest.mark.parametrize( + "euler_angles", + [ + [0.0, 0.0, 0.0], + [math.pi / 2.0, 0.0, 0.0], + [0.0, math.pi / 2.0, 0.0], + [0.0, 0.0, math.pi / 2.0], + [1.5708, -2.75, 0.1], + [0.1, math.pi, math.pi / 2], + ], +) +def test_quat_from_euler_xyz(device, num_envs, euler_angles): + """Test quat_from_euler_xyz against scipy.""" + + angles = torch.tensor(euler_angles, device=device).unsqueeze(0).repeat((num_envs, 1)) + quat_value = math_utils.quat_unique(math_utils.quat_from_euler_xyz(angles[:, 0], angles[:, 1], angles[:, 2])) + expected_quat = math_utils.convert_quat( + torch.tensor( + scipy_tf.Rotation.from_euler("xyz", euler_angles, degrees=False).as_quat(), + device=device, + dtype=torch.float, + ) + .unsqueeze(0) + .repeat((num_envs, 1)), + to="wxyz", + ) + torch.testing.assert_close(expected_quat, quat_value) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_wrap_to_pi(device): + """Test wrap_to_pi method.""" + # No wrapping needed + angle = torch.Tensor([0.0]).to(device) + expected_angle = torch.Tensor([0.0]).to(device) + wrapped_angle = math_utils.wrap_to_pi(angle) + torch.testing.assert_close(wrapped_angle, expected_angle) + + # Positive angle + angle = torch.Tensor([PI]).to(device) + expected_angle = torch.Tensor([PI]).to(device) + wrapped_angle = math_utils.wrap_to_pi(angle) + torch.testing.assert_close(wrapped_angle, expected_angle) + + # Negative angle + angle = torch.Tensor([-PI]).to(device) + expected_angle = torch.Tensor([-PI]).to(device) + wrapped_angle = math_utils.wrap_to_pi(angle) + torch.testing.assert_close(wrapped_angle, expected_angle) + + # Multiple angles + angle = torch.Tensor([3 * PI, -3 * PI, 4 * PI, -4 * PI]).to(device) + expected_angle = torch.Tensor([PI, -PI, 0.0, 0.0]).to(device) + wrapped_angle = math_utils.wrap_to_pi(angle) + torch.testing.assert_close(wrapped_angle, expected_angle) + + # Multiple angles from MATLAB docs + # fmt: off + angle = torch.Tensor([-2 * PI, - PI - 0.1, -PI, -2.8, 3.1, PI, PI + 0.001, PI + 1, 2 * PI, 2 * PI + 0.1]).to(device) + expected_angle = torch.Tensor([0.0, PI - 0.1, -PI, -2.8, 3.1 , PI, -PI + 0.001, -PI + 1 , 0.0, 0.1]).to(device) + # fmt: on + wrapped_angle = math_utils.wrap_to_pi(angle) + torch.testing.assert_close(wrapped_angle, expected_angle) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +@pytest.mark.parametrize("shape", ((3,), (1024, 3))) +def test_skew_symmetric_matrix(device, shape): + """Test skew_symmetric_matrix.""" + + vec_rand = torch.zeros(shape, device=device) + vec_rand.uniform_(-1000.0, 1000.0) + + if vec_rand.ndim == 1: + vec_rand_resized = vec_rand.clone().unsqueeze(0) + else: + vec_rand_resized = vec_rand.clone() + + mat_value = math_utils.skew_symmetric_matrix(vec_rand) + if len(shape) == 1: + expected_shape = (1, 3, 3) + else: + expected_shape = (shape[0], 3, 3) + + torch.testing.assert_close( + torch.zeros((expected_shape[0], 3), device=device), torch.diagonal(mat_value, dim1=-2, dim2=-1) + ) + torch.testing.assert_close(-vec_rand_resized[:, 2], mat_value[:, 0, 1]) + torch.testing.assert_close(vec_rand_resized[:, 1], mat_value[:, 0, 2]) + torch.testing.assert_close(-vec_rand_resized[:, 0], mat_value[:, 1, 2]) + torch.testing.assert_close(vec_rand_resized[:, 2], mat_value[:, 1, 0]) + torch.testing.assert_close(-vec_rand_resized[:, 1], mat_value[:, 2, 0]) + torch.testing.assert_close(vec_rand_resized[:, 0], mat_value[:, 2, 1]) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_orthogonalize_perspective_depth(device): + """Test for converting perspective depth to orthogonal depth.""" + # Create a sample perspective depth image (N, H, W) + perspective_depth = torch.tensor([[[10.0, 0.0, 100.0], [0.0, 3000.0, 0.0], [100.0, 0.0, 100.0]]], device=device) + + # Create sample intrinsic matrix (3, 3) + intrinsics = torch.tensor([[500.0, 0.0, 5.0], [0.0, 500.0, 5.0], [0.0, 0.0, 1.0]], device=device) + + # Convert perspective depth to orthogonal depth + orthogonal_depth = math_utils.orthogonalize_perspective_depth(perspective_depth, intrinsics) + + # Manually compute expected orthogonal depth based on the formula for comparison + expected_orthogonal_depth = torch.tensor( + [[[9.9990, 0.0000, 99.9932], [0.0000, 2999.8079, 0.0000], [99.9932, 0.0000, 99.9964]]], device=device + ) + + # Assert that the output is close to the expected result + torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_combine_frame_transform(device): + """Test combine_frame_transforms function.""" + # create random poses + pose01 = torch.rand(1, 7, device=device) + pose01[:, 3:7] = torch.nn.functional.normalize(pose01[..., 3:7], dim=-1) + + pose12 = torch.rand(1, 7, device=device) + pose12[:, 3:7] = torch.nn.functional.normalize(pose12[..., 3:7], dim=-1) + + # apply combination of poses + pos02, quat02 = math_utils.combine_frame_transforms( + pose01[..., :3], pose01[..., 3:7], pose12[:, :3], pose12[:, 3:7] + ) + # apply combination of poses w.r.t. inverse to get original frame + pos01, quat01 = math_utils.combine_frame_transforms( + pos02, + quat02, + math_utils.quat_rotate(math_utils.quat_inv(pose12[:, 3:7]), -pose12[:, :3]), + math_utils.quat_inv(pose12[:, 3:7]), + ) + + torch.testing.assert_close(pose01, torch.cat((pos01, quat01), dim=-1)) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_interpolate_poses(device): + """Test interpolate_poses function. + + This test checks the output from the :meth:`~isaaclab.utils.math_utils.interpolate_poses` function against + the output from :func:`scipy.spatial.transform.Slerp` and :func:`np.linspace`. + """ + for _ in range(100): + mat1 = math_utils.generate_random_transformation_matrix() + mat2 = math_utils.generate_random_transformation_matrix() + pos_1, rmat1 = math_utils.unmake_pose(mat1) + pos_2, rmat2 = math_utils.unmake_pose(mat2) + + # Compute expected results using scipy's Slerp + key_rots = scipy_tf.Rotation.from_matrix(np.array([rmat1, rmat2])) + + # Create a Slerp object and interpolate create the interpolated rotation matrices + num_steps = np.random.randint(3, 51) + key_times = [0, 1] + slerp = scipy_tf.Slerp(key_times, key_rots) + interp_times = np.linspace(0, 1, num_steps) + expected_quat = slerp(interp_times).as_matrix() + + # Test interpolation against expected result using np.linspace + expected_pos = np.linspace(pos_1, pos_2, num_steps) + + # interpolate_poses using interpolate_poses and quat_slerp + interpolated_poses, _ = math_utils.interpolate_poses( + math_utils.make_pose(pos_1, rmat1), math_utils.make_pose(pos_2, rmat2), num_steps - 2 + ) + result_pos, result_quat = math_utils.unmake_pose(interpolated_poses) - def test_is_identity_pose(self): - """Test is_identity_pose method.""" - identity_pos_one_row = torch.zeros(3) - identity_rot_one_row = torch.tensor((1.0, 0.0, 0.0, 0.0)) + # Assert that the result is almost equal to the expected quaternion + np.testing.assert_array_almost_equal(result_quat, expected_quat, decimal=DECIMAL_PRECISION) + np.testing.assert_array_almost_equal(result_pos, expected_pos, decimal=DECIMAL_PRECISION) - self.assertTrue(math_utils.is_identity_pose(identity_pos_one_row, identity_rot_one_row)) - identity_pos_one_row[0] = 1.0 - identity_rot_one_row[1] = 1.0 +def test_pose_inv(): + """Test pose_inv function. - self.assertFalse(math_utils.is_identity_pose(identity_pos_one_row, identity_rot_one_row)) + This test checks the output from the :meth:`~isaaclab.utils.math_utils.pose_inv` function against + the output from :func:`np.linalg.inv`. Two test cases are performed: - identity_pos_multi_row = torch.zeros(3, 3) - identity_rot_multi_row = torch.zeros(3, 4) - identity_rot_multi_row[:, 0] = 1.0 + 1. Checking the inverse of a random transformation matrix matches Numpy's built-in inverse. + 2. Checking the inverse of a batch of random transformation matrices matches Numpy's built-in inverse. + """ + # Check against a single matrix + for _ in range(100): + test_mat = math_utils.generate_random_transformation_matrix(pos_boundary=10, rot_boundary=(2 * np.pi)) + result = np.array(math_utils.pose_inv(test_mat)) + expected = np.linalg.inv(np.array(test_mat)) + np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION) - self.assertTrue(math_utils.is_identity_pose(identity_pos_multi_row, identity_rot_multi_row)) + # Check against a batch of matrices + test_mats = torch.stack( + [ + math_utils.generate_random_transformation_matrix(pos_boundary=10, rot_boundary=(2 * math.pi)) + for _ in range(100) + ] + ) + result = np.array(math_utils.pose_inv(test_mats)) + expected = np.linalg.inv(np.array(test_mats)) + np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_to_and_from_angle_axis(device): + """Test that axis_angle_from_quat against scipy and that quat_from_angle_axis are the inverse of each other.""" + n = 1024 + q_rand = math_utils.quat_unique(math_utils.random_orientation(num=n, device=device)) + rot_vec_value = math_utils.axis_angle_from_quat(q_rand) + rot_vec_scipy = torch.tensor( + scipy_tf.Rotation.from_quat( + math_utils.convert_quat(quat=q_rand.to(device="cpu").numpy(), to="xyzw") + ).as_rotvec(), + device=device, + dtype=torch.float32, + ) + torch.testing.assert_close(rot_vec_scipy, rot_vec_value) + axis = math_utils.normalize(rot_vec_value.clone()) + angle = torch.norm(rot_vec_value.clone(), dim=-1) + q_value = math_utils.quat_unique(math_utils.quat_from_angle_axis(angle, axis)) + torch.testing.assert_close(q_rand, q_value) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_box_minus(device): + """Test quat_box_minus method. + + Ensures that quat_box_minus correctly computes the axis-angle difference + between two quaternions representing rotations around the same axis. + """ + axis_angles = torch.tensor([0.0, 0.0, 1.0], device=device) + angle_a = math.pi - 0.1 + angle_b = -math.pi + 0.1 + quat_a = math_utils.quat_from_angle_axis(torch.tensor([angle_a], device=device), axis_angles) + quat_b = math_utils.quat_from_angle_axis(torch.tensor([angle_b], device=device), axis_angles) + + axis_diff = math_utils.quat_box_minus(quat_a, quat_b).squeeze(0) + expected_diff = axis_angles * math_utils.wrap_to_pi(torch.tensor(angle_a - angle_b, device=device)) + torch.testing.assert_close(expected_diff, axis_diff, atol=1e-06, rtol=1e-06) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_box_minus_and_quat_box_plus(device): + """Test consistency of quat_box_plus and quat_box_minus. + + Checks that applying quat_box_plus to accumulate rotations and then using + quat_box_minus to retrieve differences results in expected values. + """ + + # Perform closed-loop integration using quat_box_plus to accumulate rotations, + # and then use quat_box_minus to compute the incremental differences between quaternions. + # NOTE: Accuracy may decrease for very small angle increments due to numerical precision limits. + for n in (2, 10, 100, 1000): + # Define small incremental rotations around principal axes + delta_angle = torch.tensor( + [ + [0, 0, -math.pi / n], + [0, -math.pi / n, 0], + [-math.pi / n, 0, 0], + [0, 0, math.pi / n], + [0, math.pi / n, 0], + [math.pi / n, 0, 0], + ], + device=device, + ) - identity_pos_multi_row[0, 0] = 1.0 - identity_rot_multi_row[0, 1] = 1.0 + # Initialize quaternion trajectory starting from identity quaternion + quat_trajectory = torch.zeros((len(delta_angle), 2 * n + 1, 4), device=device) + quat_trajectory[:, 0, :] = torch.tensor([[1.0, 0.0, 0.0, 0.0]], device=device).repeat(len(delta_angle), 1) + + # Integrate incremental rotations forward to form a closed loop trajectory + for i in range(1, 2 * n + 1): + quat_trajectory[:, i] = math_utils.quat_box_plus(quat_trajectory[:, i - 1], delta_angle) + + # Validate the loop closure: start and end quaternions should be approximately equal + torch.testing.assert_close(quat_trajectory[:, 0], quat_trajectory[:, -1], atol=1e-04, rtol=1e-04) + + # Validate that the differences between consecutive quaternions match the original increments + for i in range(2 * n): + delta_result = math_utils.quat_box_minus(quat_trajectory[:, i + 1], quat_trajectory[:, i]) + torch.testing.assert_close(delta_result, delta_angle, atol=1e-04, rtol=1e-04) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("t12_inputs", ["True", "False"]) +@pytest.mark.parametrize("q12_inputs", ["True", "False"]) +def test_combine_frame_transforms(device, t12_inputs, q12_inputs): + """Test combine_frame_transforms such that inputs for delta translation and delta rotation + can be :obj:`None` or specified. + """ + n = 1024 + t01 = torch.zeros((n, 3), device=device) + t01.uniform_(-1000.0, 1000.0) + q01 = math_utils.quat_unique(math_utils.random_orientation(n, device=device)) + + mat_01 = torch.eye(4, 4, device=device).unsqueeze(0).repeat(n, 1, 1) + mat_01[:, 0:3, 3] = t01 + mat_01[:, 0:3, 0:3] = math_utils.matrix_from_quat(q01) + + mat_12 = torch.eye(4, 4, device=device).unsqueeze(0).repeat(n, 1, 1) + if t12_inputs: + t12 = torch.zeros((n, 3), device=device) + t12.uniform_(-1000.0, 1000.0) + mat_12[:, 0:3, 3] = t12 + else: + t12 = None + + if q12_inputs: + q12 = math_utils.quat_unique(math_utils.random_orientation(n, device=device)) + mat_12[:, 0:3, 0:3] = math_utils.matrix_from_quat(q12) + else: + q12 = None + + mat_expect = torch.einsum("bij,bjk->bik", mat_01, mat_12) + expected_translation = mat_expect[:, 0:3, 3] + expected_quat = math_utils.quat_from_matrix(mat_expect[:, 0:3, 0:3]) + translation_value, quat_value = math_utils.combine_frame_transforms(t01, q01, t12, q12) + + torch.testing.assert_close(expected_translation, translation_value, atol=1e-3, rtol=1e-5) + torch.testing.assert_close(math_utils.quat_unique(expected_quat), math_utils.quat_unique(quat_value)) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("t02_inputs", ["True", "False"]) +@pytest.mark.parametrize("q02_inputs", ["True", "False"]) +def test_subtract_frame_transforms(device, t02_inputs, q02_inputs): + """Test subtract_frame_transforms with specified and unspecified inputs for t02 and q02. + + This test verifies that :meth:`~isaaclab.utils.math_utils.subtract_frame_transforms` is the inverse operation + to :meth:`~isaaclab.utils.math_utils.combine_frame_transforms`. + .""" + n = 1024 + t01 = torch.zeros((n, 3), device=device) + t01.uniform_(-1000.0, 1000.0) + q01 = math_utils.quat_unique(math_utils.random_orientation(n, device=device)) + + mat_01 = torch.eye(4, 4, device=device).unsqueeze(0).repeat(n, 1, 1) + mat_01[:, 0:3, 3] = t01 + mat_01[:, 0:3, 0:3] = math_utils.matrix_from_quat(q01) + + if t02_inputs: + t02 = torch.zeros((n, 3), device=device) + t02.uniform_(-1000.0, 1000.0) + t02_expected = t02.clone() + else: + t02 = None + t02_expected = torch.zeros((n, 3), device=device) + + if q02_inputs: + q02 = math_utils.quat_unique(math_utils.random_orientation(n, device=device)) + q02_expected = q02.clone() + else: + q02 = None + q02_expected = math_utils.default_orientation(n, device=device) + + t12_value, q12_value = math_utils.subtract_frame_transforms(t01, q01, t02, q02) + t02_compare, q02_compare = math_utils.combine_frame_transforms(t01, q01, t12_value, q12_value) + + torch.testing.assert_close(t02_expected, t02_compare, atol=1e-3, rtol=1e-4) + torch.testing.assert_close(math_utils.quat_unique(q02_expected), math_utils.quat_unique(q02_compare)) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("rot_error_type", ("quat", "axis_angle")) +def test_compute_pose_error(device, rot_error_type): + """Test compute_pose_error for different rot_error_type.""" + n = 1000 + t01 = torch.zeros((n, 3), device=device) + t01.uniform_(-1000.0, 1000.0) + t02 = torch.zeros((n, 3), device=device) + t02.uniform_(-1000.0, 1000.0) + q01 = math_utils.quat_unique(math_utils.random_orientation(n, device=device)) + q02 = math_utils.quat_unique(math_utils.random_orientation(n, device=device)) + + diff_pos, diff_rot = math_utils.compute_pose_error(t01, q01, t02, q02, rot_error_type=rot_error_type) + + torch.testing.assert_close(t02 - t01, diff_pos) + if rot_error_type == "axis_angle": + torch.testing.assert_close(math_utils.quat_box_minus(q02, q01), diff_rot) + else: + axis_angle = math_utils.quat_box_minus(q02, q01) + axis = math_utils.normalize(axis_angle) + angle = torch.norm(axis_angle, dim=-1) - self.assertFalse(math_utils.is_identity_pose(identity_pos_multi_row, identity_rot_multi_row)) + torch.testing.assert_close( + math_utils.quat_unique(math_utils.quat_from_angle_axis(angle, axis)), + math_utils.quat_unique(diff_rot), + ) - def test_axis_angle_from_quat(self): - """Test axis_angle_from_quat method.""" - # Quaternions of the form (2,4) and (2,2,4) - quats = [ - torch.Tensor([[1.0, 0.0, 0.0, 0.0], [0.8418536, 0.142006, 0.0, 0.5206887]]), - torch.Tensor([ - [[1.0, 0.0, 0.0, 0.0], [0.8418536, 0.142006, 0.0, 0.5206887]], - [[1.0, 0.0, 0.0, 0.0], [0.9850375, 0.0995007, 0.0995007, 0.0995007]], - ]), - ] - # Angles of the form (2,3) and (2,2,3) - angles = [ - torch.Tensor([[0.0, 0.0, 0.0], [0.3, 0.0, 1.1]]), - torch.Tensor([[[0.0, 0.0, 0.0], [0.3, 0.0, 1.1]], [[0.0, 0.0, 0.0], [0.2, 0.2, 0.2]]]), - ] +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_rigid_body_twist_transform(device): + """Test rigid_body_twist_transform method. - for quat, angle in zip(quats, angles): - with self.subTest(quat=quat, angle=angle): - torch.testing.assert_close(math_utils.axis_angle_from_quat(quat), angle) - - def test_axis_angle_from_quat_approximation(self): - """Test the Taylor approximation from axis_angle_from_quat method. - - This test checks for unstable conversions where theta is very small. - """ - # Generate a small rotation quaternion - # Small angle - theta = torch.Tensor([0.0000001]) - # Arbitrary normalized axis of rotation in rads, (x,y,z) - axis = [-0.302286, 0.205494, -0.930803] - # Generate quaternion - qw = torch.cos(theta / 2) - quat_vect = [qw] + [d * torch.sin(theta / 2) for d in axis] - quaternion = torch.tensor(quat_vect, dtype=torch.float32) - - # Convert quaternion to axis-angle - axis_angle_computed = math_utils.axis_angle_from_quat(quaternion) - - # Expected axis-angle representation - axis_angle_expected = torch.tensor([theta * d for d in axis], dtype=torch.float32) - - # Assert that the computed values are close to the expected values - torch.testing.assert_close(axis_angle_computed, axis_angle_expected) - - def test_quat_error_magnitude(self): - """Test quat_error_magnitude method.""" - # Define test cases - # Each tuple contains: q1, q2, expected error - test_cases = [ - # No rotation - (torch.Tensor([1, 0, 0, 0]), torch.Tensor([1, 0, 0, 0]), torch.Tensor([0.0])), - # PI/2 rotation - (torch.Tensor([1.0, 0, 0.0, 0]), torch.Tensor([0.7071068, 0.7071068, 0, 0]), torch.Tensor([PI / 2])), - # PI rotation - (torch.Tensor([1.0, 0, 0.0, 0]), torch.Tensor([0.0, 0.0, 1.0, 0]), torch.Tensor([PI])), - ] - # Test higher dimension (batched) inputs - q1_list = torch.stack([t[0] for t in test_cases], dim=0) - q2_list = torch.stack([t[1] for t in test_cases], dim=0) - expected_diff_list = torch.stack([t[2] for t in test_cases], dim=0).flatten() - test_cases += [(q1_list, q2_list, expected_diff_list)] - - # Iterate over test cases - for q1, q2, expected_diff in test_cases: - with self.subTest(q1=q1, q2=q2): - # Compute the error - q12_diff = math_utils.quat_error_magnitude(q1, q2) - - # Check that the error is close to the expected value - if len(q1.shape) > 1: - torch.testing.assert_close(q12_diff, expected_diff) - else: - self.assertAlmostEqual(q12_diff.item(), expected_diff.item(), places=5) - - def test_quat_unique(self): - """Test quat_unique method.""" - # Define test cases - quats = math_utils.random_orientation(num=1024, device="cpu") - - # Test positive real quaternion - pos_real_quats = math_utils.quat_unique(quats) - - # Test that the real part is positive - self.assertTrue(torch.all(pos_real_quats[:, 0] > 0).item()) - - non_pos_indices = quats[:, 0] < 0 - # Check imaginary part have sign flipped if real part is negative - torch.testing.assert_close(pos_real_quats[non_pos_indices], -quats[non_pos_indices]) - torch.testing.assert_close(pos_real_quats[~non_pos_indices], quats[~non_pos_indices]) - - def test_quat_mul_with_quat_unique(self): - """Test quat_mul method with different quaternions. - - This test checks that the quaternion multiplication is consistent when using positive real quaternions - and regular quaternions. It makes sure that the result is the same regardless of the input quaternion sign - (i.e. q and -q are same quaternion in the context of rotations). - """ - - quats_1 = math_utils.random_orientation(num=1024, device="cpu") - quats_2 = math_utils.random_orientation(num=1024, device="cpu") - # Make quats positive real - quats_1_pos_real = math_utils.quat_unique(quats_1) - quats_2_pos_real = math_utils.quat_unique(quats_2) - - # Option 1: Direct computation on quaternions - quat_result_1 = math_utils.quat_mul(quats_1, math_utils.quat_conjugate(quats_2)) - quat_result_1 = math_utils.quat_unique(quat_result_1) - - # Option 2: Computation on positive real quaternions - quat_result_2 = math_utils.quat_mul(quats_1_pos_real, math_utils.quat_conjugate(quats_2_pos_real)) - quat_result_2 = math_utils.quat_unique(quat_result_2) - - # Option 3: Mixed computation - quat_result_3 = math_utils.quat_mul(quats_1, math_utils.quat_conjugate(quats_2_pos_real)) - quat_result_3 = math_utils.quat_unique(quat_result_3) - - # Check that the result is close to the expected value - torch.testing.assert_close(quat_result_1, quat_result_2) - torch.testing.assert_close(quat_result_2, quat_result_3) - torch.testing.assert_close(quat_result_3, quat_result_1) - - def test_quat_error_mag_with_quat_unique(self): - """Test quat_error_magnitude method with positive real quaternions.""" - - quats_1 = math_utils.random_orientation(num=1024, device="cpu") - quats_2 = math_utils.random_orientation(num=1024, device="cpu") - # Make quats positive real - quats_1_pos_real = math_utils.quat_unique(quats_1) - quats_2_pos_real = math_utils.quat_unique(quats_2) - - # Compute the error - error_1 = math_utils.quat_error_magnitude(quats_1, quats_2) - error_2 = math_utils.quat_error_magnitude(quats_1_pos_real, quats_2_pos_real) - error_3 = math_utils.quat_error_magnitude(quats_1, quats_2_pos_real) - error_4 = math_utils.quat_error_magnitude(quats_1_pos_real, quats_2) - - # Check that the error is close to the expected value - torch.testing.assert_close(error_1, error_2) - torch.testing.assert_close(error_2, error_3) - torch.testing.assert_close(error_3, error_4) - torch.testing.assert_close(error_4, error_1) - - def test_convention_converter(self): - """Test convert_camera_frame_orientation_convention to and from ros, opengl, and world conventions.""" - quat_ros = torch.tensor([[-0.17591989, 0.33985114, 0.82047325, -0.42470819]]) - quat_opengl = torch.tensor([[0.33985113, 0.17591988, 0.42470818, 0.82047324]]) - quat_world = torch.tensor([[-0.3647052, -0.27984815, -0.1159169, 0.88047623]]) - - # from ROS - torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "opengl"), quat_opengl + Verifies correct transformation of twists (linear and angular velocity) between coordinate frames. + """ + num_bodies = 100 + # Frame A to B + t_AB = torch.randn((num_bodies, 3), device=device) + q_AB = math_utils.random_orientation(num=num_bodies, device=device) + + # Twists in A in frame A + v_AA = torch.randn((num_bodies, 3), device=device) + w_AA = torch.randn((num_bodies, 3), device=device) + + # Get twists in B in frame B + v_BB, w_BB = math_utils.rigid_body_twist_transform(v_AA, w_AA, t_AB, q_AB) + + # Get back twists in A in frame A + t_BA = -math_utils.quat_rotate_inverse(q_AB, t_AB) + q_BA = math_utils.quat_conjugate(q_AB) + v_AA_, w_AA_ = math_utils.rigid_body_twist_transform(v_BB, w_BB, t_BA, q_BA) + + # Check + torch.testing.assert_close(v_AA_, v_AA) + torch.testing.assert_close(w_AA_, w_AA) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_yaw_quat(device): + """ + Test for yaw_quat methods. + """ + # 90-degree (n/2 radians) rotations about the Y-axis + quat_input = torch.tensor([0.7071, 0, 0.7071, 0], device=device) + cloned_quat_input = quat_input.clone() + + # Calculated output that the function should return + expected_output = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device) + + # Compute the result using the existing implementation + result = math_utils.yaw_quat(quat_input) + + # Verify original quat is not being modified + torch.testing.assert_close(quat_input, cloned_quat_input) + + # check that the output is equivalent to the expected output + torch.testing.assert_close(result, expected_output) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_slerp(device): + """Test quat_slerp function. + + This test checks the output from the :meth:`~isaaclab.utils.math_utils.quat_slerp` function against + the output from :func:`scipy.spatial.transform.Slerp`. + """ + # Generate 100 random rotation matrices + random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(100)] + random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(100)] + + tau_values = np.random.rand(10) # Random values in the range [0, 1] + + for rmat1, rmat2 in zip(random_rotation_matrices_1, random_rotation_matrices_2): + # Convert the rotation matrices to quaternions + q1 = scipy_tf.Rotation.from_matrix(rmat1).as_quat() # (x, y, z, w) + q2 = scipy_tf.Rotation.from_matrix(rmat2).as_quat() # (x, y, z, w) + + # Compute expected results using scipy's Slerp + key_rots = scipy_tf.Rotation.from_quat(np.array([q1, q2])) + key_times = [0, 1] + slerp = scipy_tf.Slerp(key_times, key_rots) + + for tau in tau_values: + expected = slerp(tau).as_quat() # (x, y, z, w) + result = math_utils.quat_slerp(torch.tensor(q1, device=device), torch.tensor(q2, device=device), tau) + # Assert that the result is almost equal to the expected quaternion + np.testing.assert_array_almost_equal(result.cpu(), expected, decimal=DECIMAL_PRECISION) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_matrix_from_quat(device): + """test matrix_from_quat against scipy.""" + # prepare random quaternions and vectors + n = 1024 + # prepare random quaternions and vectors + q_rand = math_utils.quat_unique(math_utils.random_orientation(num=n, device=device)) + rot_mat = math_utils.matrix_from_quat(quaternions=q_rand) + rot_mat_scipy = torch.tensor( + scipy_tf.Rotation.from_quat(math_utils.convert_quat(quat=q_rand.to(device="cpu"), to="xyzw")).as_matrix(), + device=device, + dtype=torch.float32, + ) + torch.testing.assert_close(rot_mat_scipy.to(device=device), rot_mat) + q_value = math_utils.quat_unique(math_utils.quat_from_matrix(rot_mat)) + torch.testing.assert_close(q_rand, q_value) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize( + "euler_angles", + [ + [0.0, 0.0, 0.0], + [math.pi / 2.0, 0.0, 0.0], + [0.0, math.pi / 2.0, 0.0], + [0.0, 0.0, math.pi / 2.0], + [1.5708, -2.75, 0.1], + [0.1, math.pi, math.pi / 2], + ], +) +@pytest.mark.parametrize( + "convention", ("XYZ", "XZY", "YXZ", "YZX", "ZXY", "ZYX", "ZYZ", "YZY", "XYX", "XZX", "ZXZ", "YXY") +) +def test_matrix_from_euler(device, euler_angles, convention): + """Test matrix_from_euler against scipy for different permutations of the X,Y,Z euler angle conventions.""" + + num_envs = 1024 + angles = torch.tensor(euler_angles, device=device).unsqueeze(0).repeat((num_envs, 1)) + mat_value = math_utils.matrix_from_euler(angles, convention=convention) + expected_mag = ( + torch.tensor( + scipy_tf.Rotation.from_euler(convention, euler_angles, degrees=False).as_matrix(), + device=device, + dtype=torch.float, ) - torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "world"), quat_world + .unsqueeze(0) + .repeat((num_envs, 1, 1)) + ) + torch.testing.assert_close(expected_mag, mat_value) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_apply(device): + """Test for quat_apply against scipy.""" + # prepare random quaternions and vectors + n = 1024 + q_rand = math_utils.random_orientation(num=n, device=device) + Rotation = scipy_tf.Rotation.from_quat(math_utils.convert_quat(quat=q_rand.to(device="cpu").numpy(), to="xyzw")) + + v_rand = math_utils.sample_uniform(-1000, 1000, (n, 3), device=device) + + # compute the result using the new implementation + scipy_result = torch.tensor(Rotation.apply(v_rand.to(device="cpu").numpy()), device=device, dtype=torch.float) + apply_result = math_utils.quat_apply(q_rand, v_rand) + torch.testing.assert_close(scipy_result.to(device=device), apply_result, atol=2e-4, rtol=2e-4) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_apply_inverse(device): + """Test for quat_apply against scipy.""" + + # prepare random quaternions and vectors + n = 1024 + q_rand = math_utils.random_orientation(num=n, device=device) + Rotation = scipy_tf.Rotation.from_quat(math_utils.convert_quat(quat=q_rand.to(device="cpu").numpy(), to="xyzw")) + + v_rand = math_utils.sample_uniform(-1000, 1000, (n, 3), device=device) + + # compute the result using the new implementation + scipy_result = torch.tensor( + Rotation.apply(v_rand.to(device="cpu").numpy(), inverse=True), device=device, dtype=torch.float + ) + apply_result = math_utils.quat_apply_inverse(q_rand, v_rand) + torch.testing.assert_close(scipy_result.to(device=device), apply_result, atol=2e-4, rtol=2e-4) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_quat_inv(device): + """Test for quat_inv method. + + For random unit and non-unit quaternions q, the Hamilton products + q โŠ— qโปยน and qโปยน โŠ— q must both equal the identity quaternion (1,0,0,0) + within numerical precision. + """ + num = 2048 + + # -------- non-unit sample (average โ€–qโ€– โ‰ˆ 10) -------- + q_nonunit = torch.randn(num, 4, device=device) * 5.0 + + # -------- unit sample (โ€–qโ€– = 1) -------- + q_unit = torch.randn(num, 4, device=device) + q_unit = q_unit / q_unit.norm(dim=-1, keepdim=True) + + identity = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device) + + for q in (q_nonunit, q_unit): + q_inv = math_utils.quat_inv(q) + + id_batch = identity.expand_as(q) + + # left and right products must both be identity + torch.testing.assert_close(math_utils.quat_mul(q, q_inv), id_batch, atol=1e-4, rtol=1e-4) + torch.testing.assert_close(math_utils.quat_mul(q_inv, q), id_batch, atol=1e-4, rtol=1e-4) + + +def test_quat_apply_benchmarks(): + """Test for quat_apply and quat_apply_inverse methods compared to old methods using torch.bmm and torch.einsum. + The new implementation uses :meth:`torch.einsum` instead of `torch.bmm` which allows + for more flexibility in the input dimensions and is faster than `torch.bmm`. + """ + + # define old implementation for quat_rotate and quat_rotate_inverse + # Based on commit: cdfa954fcc4394ca8daf432f61994e25a7b8e9e2 + + @torch.jit.script + def bmm_quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + shape = q.shape + q_w = q[:, 0] + q_vec = q[:, 1:] + a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) + b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 + c = q_vec * torch.bmm(q_vec.view(shape[0], 1, 3), v.view(shape[0], 3, 1)).squeeze(-1) * 2.0 + return a + b + c + + @torch.jit.script + def bmm_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + shape = q.shape + q_w = q[:, 0] + q_vec = q[:, 1:] + a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) + b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 + c = q_vec * torch.bmm(q_vec.view(shape[0], 1, 3), v.view(shape[0], 3, 1)).squeeze(-1) * 2.0 + return a - b + c + + @torch.jit.script + def einsum_quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + q_w = q[..., 0] + q_vec = q[..., 1:] + a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) + b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 + c = q_vec * torch.einsum("...i,...i->...", q_vec, v).unsqueeze(-1) * 2.0 + return a + b + c + + @torch.jit.script + def einsum_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + q_w = q[..., 0] + q_vec = q[..., 1:] + a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) + b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 + c = q_vec * torch.einsum("...i,...i->...", q_vec, v).unsqueeze(-1) * 2.0 + return a - b + c + + # check that implementation produces the same result as the new implementation + for device in ["cpu", "cuda:0"]: + # prepare random quaternions and vectors + q_rand = math_utils.random_orientation(num=1024, device=device) + v_rand = math_utils.sample_uniform(-1000, 1000, (1024, 3), device=device) + + # compute the result using the old implementation + bmm_result = bmm_quat_rotate(q_rand, v_rand) + bmm_result_inv = bmm_quat_rotate_inverse(q_rand, v_rand) + + # compute the result using the old implementation + einsum_result = einsum_quat_rotate(q_rand, v_rand) + einsum_result_inv = einsum_quat_rotate_inverse(q_rand, v_rand) + + # compute the result using the new implementation + new_result = math_utils.quat_apply(q_rand, v_rand) + new_result_inv = math_utils.quat_apply_inverse(q_rand, v_rand) + + # check that the result is close to the expected value + torch.testing.assert_close(bmm_result, new_result, atol=1e-3, rtol=1e-3) + torch.testing.assert_close(bmm_result_inv, new_result_inv, atol=1e-3, rtol=1e-3) + torch.testing.assert_close(einsum_result, new_result, atol=1e-3, rtol=1e-3) + torch.testing.assert_close(einsum_result_inv, new_result_inv, atol=1e-3, rtol=1e-3) + + # check the performance of the new implementation + for device in ["cpu", "cuda:0"]: + # prepare random quaternions and vectors + # new implementation supports batched inputs + q_shape = (1024, 2, 5, 4) + v_shape = (1024, 2, 5, 3) + # sample random quaternions and vectors + num_quats = math.prod(q_shape[:-1]) + q_rand = math_utils.random_orientation(num=num_quats, device=device).reshape(q_shape) + v_rand = math_utils.sample_uniform(-1000, 1000, v_shape, device=device) + + # create functions to test + def iter_quat_apply(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Iterative implementation of new quat_apply.""" + out = torch.empty_like(v) + for i in range(q.shape[1]): + for j in range(q.shape[2]): + out[:, i, j] = math_utils.quat_apply(q_rand[:, i, j], v_rand[:, i, j]) + return out + + def iter_quat_apply_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Iterative implementation of new quat_apply_inverse.""" + out = torch.empty_like(v) + for i in range(q.shape[1]): + for j in range(q.shape[2]): + out[:, i, j] = math_utils.quat_apply_inverse(q_rand[:, i, j], v_rand[:, i, j]) + return out + + def iter_bmm_quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Iterative implementation of old quat_rotate using torch.bmm.""" + out = torch.empty_like(v) + for i in range(q.shape[1]): + for j in range(q.shape[2]): + out[:, i, j] = bmm_quat_rotate(q_rand[:, i, j], v_rand[:, i, j]) + return out + + def iter_bmm_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Iterative implementation of old quat_rotate_inverse using torch.bmm.""" + out = torch.empty_like(v) + for i in range(q.shape[1]): + for j in range(q.shape[2]): + out[:, i, j] = bmm_quat_rotate_inverse(q_rand[:, i, j], v_rand[:, i, j]) + return out + + def iter_einsum_quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Iterative implementation of old quat_rotate using torch.einsum.""" + out = torch.empty_like(v) + for i in range(q.shape[1]): + for j in range(q.shape[2]): + out[:, i, j] = einsum_quat_rotate(q_rand[:, i, j], v_rand[:, i, j]) + return out + + def iter_einsum_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Iterative implementation of old quat_rotate_inverse using torch.einsum.""" + out = torch.empty_like(v) + for i in range(q.shape[1]): + for j in range(q.shape[2]): + out[:, i, j] = einsum_quat_rotate_inverse(q_rand[:, i, j], v_rand[:, i, j]) + return out + + # benchmarks for iterative calls + timer_iter_quat_apply = benchmark.Timer( + stmt="iter_quat_apply(q_rand, v_rand)", + globals={"iter_quat_apply": iter_quat_apply, "q_rand": q_rand, "v_rand": v_rand}, ) - torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "ros"), quat_ros + timer_iter_quat_apply_inverse = benchmark.Timer( + stmt="iter_quat_apply_inverse(q_rand, v_rand)", + globals={"iter_quat_apply_inverse": iter_quat_apply_inverse, "q_rand": q_rand, "v_rand": v_rand}, ) - # from OpenGL - torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "ros"), quat_ros + + timer_iter_bmm_quat_rotate = benchmark.Timer( + stmt="iter_bmm_quat_rotate(q_rand, v_rand)", + globals={"iter_bmm_quat_rotate": iter_bmm_quat_rotate, "q_rand": q_rand, "v_rand": v_rand}, ) - torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world"), quat_world + timer_iter_bmm_quat_rotate_inverse = benchmark.Timer( + stmt="iter_bmm_quat_rotate_inverse(q_rand, v_rand)", + globals={ + "iter_bmm_quat_rotate_inverse": iter_bmm_quat_rotate_inverse, + "q_rand": q_rand, + "v_rand": v_rand, + }, ) - torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "opengl"), quat_opengl + + timer_iter_einsum_quat_rotate = benchmark.Timer( + stmt="iter_einsum_quat_rotate(q_rand, v_rand)", + globals={"iter_einsum_quat_rotate": iter_einsum_quat_rotate, "q_rand": q_rand, "v_rand": v_rand}, + ) + timer_iter_einsum_quat_rotate_inverse = benchmark.Timer( + stmt="iter_einsum_quat_rotate_inverse(q_rand, v_rand)", + globals={ + "iter_einsum_quat_rotate_inverse": iter_einsum_quat_rotate_inverse, + "q_rand": q_rand, + "v_rand": v_rand, + }, + ) + + # create benchmaks for size independent calls + timer_quat_apply = benchmark.Timer( + stmt="math_utils.quat_apply(q_rand, v_rand)", + globals={"math_utils": math_utils, "q_rand": q_rand, "v_rand": v_rand}, + ) + timer_quat_apply_inverse = benchmark.Timer( + stmt="math_utils.quat_apply_inverse(q_rand, v_rand)", + globals={"math_utils": math_utils, "q_rand": q_rand, "v_rand": v_rand}, + ) + timer_einsum_quat_rotate = benchmark.Timer( + stmt="einsum_quat_rotate(q_rand, v_rand)", + globals={"einsum_quat_rotate": einsum_quat_rotate, "q_rand": q_rand, "v_rand": v_rand}, ) - # from World + timer_einsum_quat_rotate_inverse = benchmark.Timer( + stmt="einsum_quat_rotate_inverse(q_rand, v_rand)", + globals={"einsum_quat_rotate_inverse": einsum_quat_rotate_inverse, "q_rand": q_rand, "v_rand": v_rand}, + ) + + # run the benchmark + print("--------------------------------") + print(f"Device: {device}") + print("Time for quat_apply:", timer_quat_apply.timeit(number=1000)) + print("Time for einsum_quat_rotate:", timer_einsum_quat_rotate.timeit(number=1000)) + print("Time for iter_quat_apply:", timer_iter_quat_apply.timeit(number=1000)) + print("Time for iter_bmm_quat_rotate:", timer_iter_bmm_quat_rotate.timeit(number=1000)) + print("Time for iter_einsum_quat_rotate:", timer_iter_einsum_quat_rotate.timeit(number=1000)) + print("--------------------------------") + print("Time for quat_apply_inverse:", timer_quat_apply_inverse.timeit(number=1000)) + print("Time for einsum_quat_rotate_inverse:", timer_einsum_quat_rotate_inverse.timeit(number=1000)) + print("Time for iter_quat_apply_inverse:", timer_iter_quat_apply_inverse.timeit(number=1000)) + print("Time for iter_bmm_quat_rotate_inverse:", timer_iter_bmm_quat_rotate_inverse.timeit(number=1000)) + print("Time for iter_einsum_quat_rotate_inverse:", timer_iter_einsum_quat_rotate_inverse.timeit(number=1000)) + print("--------------------------------") + + # check output values are the same + torch.testing.assert_close(math_utils.quat_apply(q_rand, v_rand), iter_quat_apply(q_rand, v_rand)) torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "ros"), quat_ros + math_utils.quat_apply(q_rand, v_rand), iter_bmm_quat_rotate(q_rand, v_rand), atol=1e-3, rtol=1e-3 ) torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "opengl"), quat_opengl + math_utils.quat_apply_inverse(q_rand, v_rand), iter_quat_apply_inverse(q_rand, v_rand) ) torch.testing.assert_close( - math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "world"), quat_world + math_utils.quat_apply_inverse(q_rand, v_rand), + iter_bmm_quat_rotate_inverse(q_rand, v_rand), + atol=1e-3, + rtol=1e-3, ) - def test_wrap_to_pi(self): - """Test wrap_to_pi method.""" - # Define test cases - # Each tuple contains: angle, expected wrapped angle - test_cases = [ - # No wrapping needed - (torch.Tensor([0.0]), torch.Tensor([0.0])), - # Positive angle - (torch.Tensor([PI]), torch.Tensor([PI])), - # Negative angle - (torch.Tensor([-PI]), torch.Tensor([-PI])), - # Multiple angles - (torch.Tensor([3 * PI, -3 * PI, 4 * PI, -4 * PI]), torch.Tensor([PI, -PI, 0.0, 0.0])), - # Multiple angles from MATLAB docs - # fmt: off - ( - torch.Tensor([-2 * PI, - PI - 0.1, -PI, -2.8, 3.1, PI, PI + 0.001, PI + 1, 2 * PI, 2 * PI + 0.1]), - torch.Tensor([0.0, PI - 0.1, -PI, -2.8, 3.1 , PI, -PI + 0.001, -PI + 1 , 0.0, 0.1]) - ), - # fmt: on - ] - - # Iterate over test cases - for device in ["cpu", "cuda:0"]: - for angle, expected_angle in test_cases: - with self.subTest(angle=angle, device=device): - # move to the device - angle = angle.to(device) - expected_angle = expected_angle.to(device) - # Compute the wrapped angle - wrapped_angle = math_utils.wrap_to_pi(angle) - # Check that the wrapped angle is close to the expected value - torch.testing.assert_close(wrapped_angle, expected_angle) - - def test_quat_rotate_and_quat_rotate_inverse(self): - """Test for quat_rotate and quat_rotate_inverse methods. - - The new implementation uses :meth:`torch.einsum` instead of `torch.bmm` which allows - for more flexibility in the input dimensions and is faster than `torch.bmm`. - """ - - # define old implementation for quat_rotate and quat_rotate_inverse - # Based on commit: cdfa954fcc4394ca8daf432f61994e25a7b8e9e2 - - @torch.jit.script - def old_quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: - shape = q.shape - q_w = q[:, 0] - q_vec = q[:, 1:] - a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) - b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 - c = q_vec * torch.bmm(q_vec.view(shape[0], 1, 3), v.view(shape[0], 3, 1)).squeeze(-1) * 2.0 - return a + b + c - - @torch.jit.script - def old_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: - shape = q.shape - q_w = q[:, 0] - q_vec = q[:, 1:] - a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) - b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 - c = q_vec * torch.bmm(q_vec.view(shape[0], 1, 3), v.view(shape[0], 3, 1)).squeeze(-1) * 2.0 - return a - b + c - - # check that implementation produces the same result as the new implementation - for device in ["cpu", "cuda:0"]: - # prepare random quaternions and vectors - q_rand = math_utils.random_orientation(num=1024, device=device) - v_rand = math_utils.sample_uniform(-1000, 1000, (1024, 3), device=device) - - # compute the result using the old implementation - old_result = old_quat_rotate(q_rand, v_rand) - old_result_inv = old_quat_rotate_inverse(q_rand, v_rand) - - # compute the result using the new implementation - new_result = math_utils.quat_rotate(q_rand, v_rand) - new_result_inv = math_utils.quat_rotate_inverse(q_rand, v_rand) - - # check that the result is close to the expected value - torch.testing.assert_close(old_result, new_result) - torch.testing.assert_close(old_result_inv, new_result_inv) - - # check the performance of the new implementation - for device in ["cpu", "cuda:0"]: - # prepare random quaternions and vectors - # new implementation supports batched inputs - q_shape = (1024, 2, 5, 4) - v_shape = (1024, 2, 5, 3) - # sample random quaternions and vectors - num_quats = math.prod(q_shape[:-1]) - q_rand = math_utils.random_orientation(num=num_quats, device=device).reshape(q_shape) - v_rand = math_utils.sample_uniform(-1000, 1000, v_shape, device=device) - - # create functions to test - def iter_quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: - """Iterative implementation of new quat_rotate.""" - out = torch.empty_like(v) - for i in range(q.shape[1]): - for j in range(q.shape[2]): - out[:, i, j] = math_utils.quat_rotate(q_rand[:, i, j], v_rand[:, i, j]) - return out - - def iter_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: - """Iterative implementation of new quat_rotate_inverse.""" - out = torch.empty_like(v) - for i in range(q.shape[1]): - for j in range(q.shape[2]): - out[:, i, j] = math_utils.quat_rotate_inverse(q_rand[:, i, j], v_rand[:, i, j]) - return out - - def iter_old_quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: - """Iterative implementation of old quat_rotate.""" - out = torch.empty_like(v) - for i in range(q.shape[1]): - for j in range(q.shape[2]): - out[:, i, j] = old_quat_rotate(q_rand[:, i, j], v_rand[:, i, j]) - return out - - def iter_old_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: - """Iterative implementation of old quat_rotate_inverse.""" - out = torch.empty_like(v) - for i in range(q.shape[1]): - for j in range(q.shape[2]): - out[:, i, j] = old_quat_rotate_inverse(q_rand[:, i, j], v_rand[:, i, j]) - return out - - # create benchmark - timer_iter_quat_rotate = benchmark.Timer( - stmt="iter_quat_rotate(q_rand, v_rand)", - globals={"iter_quat_rotate": iter_quat_rotate, "q_rand": q_rand, "v_rand": v_rand}, - ) - timer_iter_quat_rotate_inverse = benchmark.Timer( - stmt="iter_quat_rotate_inverse(q_rand, v_rand)", - globals={"iter_quat_rotate_inverse": iter_quat_rotate_inverse, "q_rand": q_rand, "v_rand": v_rand}, - ) - - timer_iter_old_quat_rotate = benchmark.Timer( - stmt="iter_old_quat_rotate(q_rand, v_rand)", - globals={"iter_old_quat_rotate": iter_old_quat_rotate, "q_rand": q_rand, "v_rand": v_rand}, - ) - timer_iter_old_quat_rotate_inverse = benchmark.Timer( - stmt="iter_old_quat_rotate_inverse(q_rand, v_rand)", - globals={ - "iter_old_quat_rotate_inverse": iter_old_quat_rotate_inverse, - "q_rand": q_rand, - "v_rand": v_rand, - }, - ) - - timer_quat_rotate = benchmark.Timer( - stmt="math_utils.quat_rotate(q_rand, v_rand)", - globals={"math_utils": math_utils, "q_rand": q_rand, "v_rand": v_rand}, - ) - timer_quat_rotate_inverse = benchmark.Timer( - stmt="math_utils.quat_rotate_inverse(q_rand, v_rand)", - globals={"math_utils": math_utils, "q_rand": q_rand, "v_rand": v_rand}, - ) - - # run the benchmark - print("--------------------------------") - print(f"Device: {device}") - print("Time for quat_rotate:", timer_quat_rotate.timeit(number=1000)) - print("Time for iter_quat_rotate:", timer_iter_quat_rotate.timeit(number=1000)) - print("Time for iter_old_quat_rotate:", timer_iter_old_quat_rotate.timeit(number=1000)) - print("--------------------------------") - print("Time for quat_rotate_inverse:", timer_quat_rotate_inverse.timeit(number=1000)) - print("Time for iter_quat_rotate_inverse:", timer_iter_quat_rotate_inverse.timeit(number=1000)) - print("Time for iter_old_quat_rotate_inverse:", timer_iter_old_quat_rotate_inverse.timeit(number=1000)) - print("--------------------------------") - - # check output values are the same - torch.testing.assert_close(math_utils.quat_rotate(q_rand, v_rand), iter_quat_rotate(q_rand, v_rand)) - torch.testing.assert_close(math_utils.quat_rotate(q_rand, v_rand), iter_old_quat_rotate(q_rand, v_rand)) - torch.testing.assert_close( - math_utils.quat_rotate_inverse(q_rand, v_rand), iter_quat_rotate_inverse(q_rand, v_rand) - ) - torch.testing.assert_close( - math_utils.quat_rotate_inverse(q_rand, v_rand), - iter_old_quat_rotate_inverse(q_rand, v_rand), - ) - - def test_orthogonalize_perspective_depth(self): - """Test for converting perspective depth to orthogonal depth.""" - for device in ["cpu", "cuda:0"]: - # Create a sample perspective depth image (N, H, W) - perspective_depth = torch.tensor( - [[[10.0, 0.0, 100.0], [0.0, 3000.0, 0.0], [100.0, 0.0, 100.0]]], device=device - ) - - # Create sample intrinsic matrix (3, 3) - intrinsics = torch.tensor([[500.0, 0.0, 5.0], [0.0, 500.0, 5.0], [0.0, 0.0, 1.0]], device=device) - - # Convert perspective depth to orthogonal depth - orthogonal_depth = math_utils.orthogonalize_perspective_depth(perspective_depth, intrinsics) - - # Manually compute expected orthogonal depth based on the formula for comparison - expected_orthogonal_depth = torch.tensor( - [[[9.9990, 0.0000, 99.9932], [0.0000, 2999.8079, 0.0000], [99.9932, 0.0000, 99.9964]]], device=device - ) - - # Assert that the output is close to the expected result - torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth) - - def test_combine_frame_transform(self): - """Test combine_frame_transforms function.""" - for device in ["cpu", "cuda:0"]: - # create random poses - pose01 = torch.rand(1, 7, device=device) - pose01[:, 3:7] = torch.nn.functional.normalize(pose01[..., 3:7], dim=-1) - - pose12 = torch.rand(1, 7, device=device) - pose12[:, 3:7] = torch.nn.functional.normalize(pose12[..., 3:7], dim=-1) - - # apply combination of poses - pos02, quat02 = math_utils.combine_frame_transforms( - pose01[..., :3], pose01[..., 3:7], pose12[:, :3], pose12[:, 3:7] - ) - # apply combination of poses w.r.t. inverse to get original frame - pos01, quat01 = math_utils.combine_frame_transforms( - pos02, - quat02, - math_utils.quat_rotate(math_utils.quat_inv(pose12[:, 3:7]), -pose12[:, :3]), - math_utils.quat_inv(pose12[:, 3:7]), - ) - - torch.testing.assert_close(pose01, torch.cat((pos01, quat01), dim=-1)) - - def test_pose_inv(self): - """Test pose_inv function. - - This test checks the output from the :meth:`~isaaclab.utils.math_utils.pose_inv` function against - the output from :func:`np.linalg.inv`. Two test cases are performed: - - 1. Checking the inverse of a random transformation matrix matches Numpy's built-in inverse. - 2. Checking the inverse of a batch of random transformation matrices matches Numpy's built-in inverse. - """ - # Check against a single matrix - for _ in range(100): - test_mat = math_utils.generate_random_transformation_matrix(pos_boundary=10, rot_boundary=(2 * np.pi)) - result = np.array(math_utils.pose_inv(test_mat)) - expected = np.linalg.inv(np.array(test_mat)) - np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION) - - # Check against a batch of matrices - test_mats = torch.stack([ - math_utils.generate_random_transformation_matrix(pos_boundary=10, rot_boundary=(2 * math.pi)) - for _ in range(100) - ]) - result = np.array(math_utils.pose_inv(test_mats)) - expected = np.linalg.inv(np.array(test_mats)) - np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION) - - def test_quat_slerp(self): - """Test quat_slerp function. - - This test checks the output from the :meth:`~isaaclab.utils.math_utils.quat_slerp` function against - the output from :func:`scipy.spatial.transform.Slerp`. - """ - # Generate 100 random rotation matrices - random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(100)] - random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(100)] - - tau_values = np.random.rand(10) # Random values in the range [0, 1] - - for rmat1, rmat2 in zip(random_rotation_matrices_1, random_rotation_matrices_2): - # Convert the rotation matrices to quaternions - q1 = scipy_tf.Rotation.from_matrix(rmat1).as_quat() # (x, y, z, w) - q2 = scipy_tf.Rotation.from_matrix(rmat2).as_quat() # (x, y, z, w) - - # Compute expected results using scipy's Slerp - key_rots = scipy_tf.Rotation.from_quat(np.array([q1, q2])) - key_times = [0, 1] - slerp = scipy_tf.Slerp(key_times, key_rots) - - for tau in tau_values: - expected = slerp(tau).as_quat() # (x, y, z, w) - result = math_utils.quat_slerp(torch.tensor(q1), torch.tensor(q2), tau) - # Assert that the result is almost equal to the expected quaternion - np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION) - - def test_interpolate_rotations(self): - """Test interpolate_rotations function. - - This test checks the output from the :meth:`~isaaclab.utils.math_utils.interpolate_rotations` function against - the output from :func:`scipy.spatial.transform.Slerp`. - """ - # Generate NUM_ITERS random rotation matrices - random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(100)] - random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(100)] - - for rmat1, rmat2 in zip(random_rotation_matrices_1, random_rotation_matrices_2): - # Compute expected results using scipy's Slerp - key_rots = scipy_tf.Rotation.from_matrix(np.array([rmat1, rmat2])) - - # Create a Slerp object and interpolate create the interpolated matrices - # Minimum 2 required because Interpolate_rotations returns one extra rotation matrix - num_steps = np.random.randint(2, 51) - key_times = [0, 1] - slerp = scipy_tf.Slerp(key_times, key_rots) - interp_times = np.linspace(0, 1, num_steps) - expected = slerp(interp_times).as_matrix() - - # Test 1: - # Interpolate_rotations using interpolate_rotations and quat_slerp - # interpolate_rotations returns one extra rotation matrix hence num_steps-1 - result_quat = math_utils.interpolate_rotations(rmat1, rmat2, num_steps - 1) - - # Assert that the result is almost equal to the expected quaternion - np.testing.assert_array_almost_equal(result_quat, expected, decimal=DECIMAL_PRECISION) - - # Test 2: - # Interpolate_rotations using axis_angle and ensure the result is still the same - # interpolate_rotations returns one extra rotation matrix hence num_steps-1 - result_axis_angle = math_utils.interpolate_rotations(rmat1, rmat2, num_steps - 1, axis_angle=True) - - # Assert that the result is almost equal to the expected quaternion - np.testing.assert_array_almost_equal(result_axis_angle, expected, decimal=DECIMAL_PRECISION) - - def test_interpolate_poses(self): - """Test interpolate_poses function. - - This test checks the output from the :meth:`~isaaclab.utils.math_utils.interpolate_poses` function against - the output from :func:`scipy.spatial.transform.Slerp` and :func:`np.linspace`. - """ - # Generate 100 random transformation matrices - random_mat_1 = [math_utils.generate_random_transformation_matrix() for _ in range(100)] - random_mat_2 = [math_utils.generate_random_transformation_matrix() for _ in range(100)] - - for mat1, mat2 in zip(random_mat_1, random_mat_2): - pos_1, rmat1 = math_utils.unmake_pose(mat1) - pos_2, rmat2 = math_utils.unmake_pose(mat2) - - # Compute expected results using scipy's Slerp - key_rots = scipy_tf.Rotation.from_matrix(np.array([rmat1, rmat2])) - - # Create a Slerp object and interpolate create the interpolated rotation matrices - # Minimum 3 required because interpolate_poses returns extra staring and ending pose matrices - num_steps = np.random.randint(3, 51) - key_times = [0, 1] - slerp = scipy_tf.Slerp(key_times, key_rots) - interp_times = np.linspace(0, 1, num_steps) - expected_quat = slerp(interp_times).as_matrix() - - # Test interpolation against expected result using np.linspace - expected_pos = np.linspace(pos_1, pos_2, num_steps) - - # interpolate_poses using interpolate_poses and quat_slerp - # interpolate_poses returns extra staring and ending pose matrices hence num_steps-2 - interpolated_poses, _ = math_utils.interpolate_poses( - math_utils.make_pose(pos_1, rmat1), math_utils.make_pose(pos_2, rmat2), num_steps - 2 - ) - result_pos, result_quat = math_utils.unmake_pose(interpolated_poses) - - # Assert that the result is almost equal to the expected quaternion - np.testing.assert_array_almost_equal(result_quat, expected_quat, decimal=DECIMAL_PRECISION) - np.testing.assert_array_almost_equal(result_pos, expected_pos, decimal=DECIMAL_PRECISION) - -if __name__ == "__main__": - run_tests() +def test_interpolate_rotations(): + """Test interpolate_rotations function. + + This test checks the output from the :meth:`~isaaclab.utils.math_utils.interpolate_rotations` function against + the output from :func:`scipy.spatial.transform.Slerp`. + """ + # Generate NUM_ITERS random rotation matrices + random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(100)] + random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(100)] + + for rmat1, rmat2 in zip(random_rotation_matrices_1, random_rotation_matrices_2): + # Compute expected results using scipy's Slerp + key_rots = scipy_tf.Rotation.from_matrix(np.array([rmat1, rmat2])) + + # Create a Slerp object and interpolate create the interpolated matrices + # Minimum 2 required because Interpolate_rotations returns one extra rotation matrix + num_steps = np.random.randint(2, 51) + key_times = [0, 1] + slerp = scipy_tf.Slerp(key_times, key_rots) + interp_times = np.linspace(0, 1, num_steps) + expected = slerp(interp_times).as_matrix() + + # Test 1: + # Interpolate_rotations using interpolate_rotations and quat_slerp + # interpolate_rotations returns one extra rotation matrix hence num_steps-1 + result_quat = math_utils.interpolate_rotations(rmat1, rmat2, num_steps - 1) + + # Assert that the result is almost equal to the expected quaternion + np.testing.assert_array_almost_equal(result_quat.cpu(), expected, decimal=DECIMAL_PRECISION) + + # Test 2: + # Interpolate_rotations using axis_angle and ensure the result is still the same + # interpolate_rotations returns one extra rotation matrix hence num_steps-1 + result_axis_angle = math_utils.interpolate_rotations(rmat1, rmat2, num_steps - 1, axis_angle=True) + + # Assert that the result is almost equal to the expected quaternion + np.testing.assert_array_almost_equal(result_axis_angle.cpu(), expected, decimal=DECIMAL_PRECISION) + + +def test_euler_xyz_from_quat(): + """Test euler_xyz_from_quat function. + + This test checks the output from the :meth:`~isaaclab.utils.math_utils.euler_xyz_from_quat` function + against the expected output for various quaternions. + The test includes quaternions representing different rotations around the x, y, and z axes. + The test is performed for both the default output range (-ฯ€, ฯ€] and the wrapped output range [0, 2ฯ€). + """ + quats = [ + torch.Tensor([[1.0, 0.0, 0.0, 0.0]]), # 0ยฐ around x, y, z + torch.Tensor( + [ + [0.9238795, 0.3826834, 0.0, 0.0], # 45ยฐ around x + [0.9238795, 0.0, -0.3826834, 0.0], # -45ยฐ around y + [0.9238795, 0.0, 0.0, -0.3826834], # -45ยฐ around z + ] + ), + torch.Tensor( + [ + [0.7071068, -0.7071068, 0.0, 0.0], # -90ยฐ around x + [0.7071068, 0.0, 0.0, -0.7071068], # -90ยฐ around z + ] + ), + torch.Tensor( + [ + [0.3826834, -0.9238795, 0.0, 0.0], # -135ยฐ around x + [0.3826834, 0.0, 0.0, -0.9238795], # -135ยฐ around y + ] + ), + ] + + expected_euler_angles = [ + torch.Tensor([[0.0, 0.0, 0.0]]), # identity + torch.Tensor( + [ + [torch.pi / 4, 0.0, 0.0], # 45ยฐ about x + [0.0, -torch.pi / 4, 0.0], # -45ยฐ about y + [0.0, 0.0, -torch.pi / 4], # -45ยฐ about z + ] + ), + torch.Tensor( + [ + [-torch.pi / 2, 0.0, 0.0], # -90ยฐ about x + [0.0, 0.0, -torch.pi / 2], # -90ยฐ about z + ] + ), + torch.Tensor( + [ + [-3 * torch.pi / 4, 0.0, 0.0], # -135ยฐ about x + [0.0, 0.0, -3 * torch.pi / 4], # -135ยฐ about y + ] + ), + ] + + # Test 1: default no-wrap range from (-ฯ€, ฯ€] + for quat, expected in zip(quats, expected_euler_angles): + output = torch.stack(math_utils.euler_xyz_from_quat(quat), dim=-1) + torch.testing.assert_close(output, expected) + + # Test 2: wrap to [0, 2ฯ€) + for quat, expected in zip(quats, expected_euler_angles): + wrapped = expected % (2 * torch.pi) + output = torch.stack(math_utils.euler_xyz_from_quat(quat, wrap_to_2pi=True), dim=-1) + torch.testing.assert_close(output, wrapped) diff --git a/source/isaaclab/test/utils/test_modifiers.py b/source/isaaclab/test/utils/test_modifiers.py index 85fe0200fc16..9cdd9a5d6631 100644 --- a/source/isaaclab/test/utils/test_modifiers.py +++ b/source/isaaclab/test/utils/test_modifiers.py @@ -1,22 +1,22 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" -import torch -import unittest from dataclasses import MISSING +import pytest +import torch + import isaaclab.utils.modifiers as modifiers from isaaclab.utils import configclass @@ -31,170 +31,196 @@ class ModifierTestCfg: num_iter: int = 10 -class TestModifiers(unittest.TestCase): - """Test different modifiers implementations.""" - - def test_scale_modifier(self): - """Test for scale modifier.""" - # create a random tensor - data = torch.rand(128, 128, device="cuda") - - # create a modifier configuration - cfg = modifiers.ModifierCfg(func=modifiers.scale, params={"multiplier": 2.0}) - # apply the modifier - processed_data = cfg.func(data, **cfg.params) - - # check if the shape of the modified data is the same as the original data - self.assertEqual(data.shape, processed_data.shape, msg="Modified data shape does not equal original") - torch.testing.assert_close(processed_data, data * cfg.params["multiplier"]) - - def test_bias_modifier(self): - """Test for bias modifier.""" - # create a random tensor - data = torch.rand(128, 128, device="cuda") - # create a modifier configuration - cfg = modifiers.ModifierCfg(func=modifiers.bias, params={"value": 0.5}) - # apply the modifier - processed_data = cfg.func(data, **cfg.params) - - # check if the shape of the modified data is the same as the original data - self.assertEqual(data.shape, processed_data.shape, msg="Modified data shape does not equal original") - torch.testing.assert_close(processed_data - data, torch.ones_like(data) * cfg.params["value"]) - - def test_clip_modifier(self): - """Test for clip modifier.""" - # create a random tensor - data = torch.rand(128, 128, device="cuda") - - # create a modifier configuration - cfg = modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (0.5, 2.5)}) - # apply the modifier - processed_data = cfg.func(data, **cfg.params) - - # check if the shape of the modified data is the same as the original data - self.assertEqual(data.shape, processed_data.shape, msg="Modified data shape does not equal original") - self.assertTrue(torch.min(processed_data) >= cfg.params["bounds"][0]) - self.assertTrue(torch.max(processed_data) <= cfg.params["bounds"][1]) - - def test_clip_no_upper_bound_modifier(self): - """Test for clip modifier with no upper bound.""" - # create a random tensor - data = torch.rand(128, 128, device="cuda") - - # create a modifier configuration - cfg = modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (0.0, None)}) - # apply the modifier - processed_data = cfg.func(data, **cfg.params) - - # check if the shape of the modified data is the same as the original data - self.assertEqual(data.shape, processed_data.shape, msg="Modified data shape does not equal original") - self.assertTrue(torch.min(processed_data) >= cfg.params["bounds"][0]) - - def test_clip_no_lower_bound_modifier(self): - """Test for clip modifier with no lower bound.""" - # create a random tensor - data = torch.rand(128, 128, device="cuda") - - # create a modifier configuration - cfg = modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (None, 0.0)}) - # apply the modifier - processed_data = cfg.func(data, **cfg.params) - - # check if the shape of the modified data is the same as the original data - self.assertEqual(data.shape, processed_data.shape, msg="Modified data shape does not equal original") - self.assertTrue(torch.min(processed_data) <= cfg.params["bounds"][1]) - - def test_torch_relu_modifier(self): - """Test for torch relu modifier.""" - # create a random tensor - data = torch.rand(128, 128, device="cuda") - - # create a modifier configuration - cfg = modifiers.ModifierCfg(func=torch.nn.functional.relu) - # apply the modifier - processed_data = cfg.func(data) - - # check if the shape of the modified data is the same as the original data - self.assertEqual(data.shape, processed_data.shape, msg="modified data shape does not equal original") - self.assertTrue(torch.all(processed_data >= 0.0)) - - def test_digital_filter(self): - """Test for digital filter modifier.""" - for device in ["cpu", "cuda"]: - with self.subTest(device=device): - # create a modifier configuration - modifier_cfg = modifiers.DigitalFilterCfg(A=[0.0, 0.1], B=[0.5, 0.5]) - - # create a test configuration - test_cfg = ModifierTestCfg( - cfg=modifier_cfg, - init_data=torch.tensor([0.0, 0.0, 0.0], device=device).unsqueeze(1), - result=torch.tensor([-0.45661893, -0.45661893, -0.45661893], device=device).unsqueeze(1), - num_iter=16, - ) - - # create a modifier instance - modifier_obj = modifier_cfg.func(modifier_cfg, test_cfg.init_data.shape, device=device) - - # test the modifier - theta = torch.tensor([0.0], device=device) - delta = torch.pi / torch.tensor([8.0, 8.0, 8.0], device=device).unsqueeze(1) - - for _ in range(5): - # reset the modifier - modifier_obj.reset() - - # apply the modifier multiple times - for i in range(test_cfg.num_iter): - data = torch.sin(theta + i * delta) - processed_data = modifier_obj(data) - - self.assertEqual( - data.shape, processed_data.shape, msg="Modified data shape does not equal original" - ) - - # check if the modified data is close to the expected result - torch.testing.assert_close(processed_data, test_cfg.result) - - def test_integral(self): - """Test for integral modifier.""" - for device in ["cpu", "cuda"]: - with self.subTest(device=device): - # create a modifier configuration - modifier_cfg = modifiers.IntegratorCfg(dt=1.0) - - # create a test configuration - test_cfg = ModifierTestCfg( - cfg=modifier_cfg, - init_data=torch.tensor([0.0], device=device).unsqueeze(1), - result=torch.tensor([12.5], device=device).unsqueeze(1), - num_iter=6, - ) - - # create a modifier instance - modifier_obj = modifier_cfg.func(modifier_cfg, test_cfg.init_data.shape, device=device) - - # test the modifier - delta = torch.tensor(1.0, device=device) - - for _ in range(5): - # reset the modifier - modifier_obj.reset() - - # clone the data to avoid modifying the original - data = test_cfg.init_data.clone() - # apply the modifier multiple times - for _ in range(test_cfg.num_iter): - processed_data = modifier_obj(data) - data = data + delta - - self.assertEqual( - data.shape, processed_data.shape, msg="Modified data shape does not equal original" - ) - - # check if the modified data is close to the expected result - torch.testing.assert_close(processed_data, test_cfg.result) - - -if __name__ == "__main__": - run_tests() +def test_scale_modifier(): + """Test scale modifier.""" + # create test data + init_data = torch.tensor([1.0, 2.0, 3.0]) + scale = 2.0 + result = torch.tensor([2.0, 4.0, 6.0]) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.ModifierCfg(func=modifiers.scale, params={"multiplier": scale}), + init_data=init_data, + result=result, + ) + + # test modifier + for _ in range(test_cfg.num_iter): + output = test_cfg.cfg.func(test_cfg.init_data, **test_cfg.cfg.params) + assert torch.allclose(output, test_cfg.result) + + +def test_bias_modifier(): + """Test bias modifier.""" + # create test data + init_data = torch.tensor([1.0, 2.0, 3.0]) + bias = 1.0 + result = torch.tensor([2.0, 3.0, 4.0]) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.ModifierCfg(func=modifiers.bias, params={"value": bias}), + init_data=init_data, + result=result, + ) + + # test modifier + for _ in range(test_cfg.num_iter): + output = test_cfg.cfg.func(test_cfg.init_data, **test_cfg.cfg.params) + assert torch.allclose(output, test_cfg.result) + + +def test_clip_modifier(): + """Test clip modifier.""" + # create test data + init_data = torch.tensor([1.0, 2.0, 3.0]) + min_val = 1.5 + max_val = 2.5 + result = torch.tensor([1.5, 2.0, 2.5]) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (min_val, max_val)}), + init_data=init_data, + result=result, + ) + + # test modifier + for _ in range(test_cfg.num_iter): + output = test_cfg.cfg.func(test_cfg.init_data, **test_cfg.cfg.params) + assert torch.allclose(output, test_cfg.result) + + +def test_clip_no_upper_bound_modifier(): + """Test clip modifier with no upper bound.""" + # create test data + init_data = torch.tensor([1.0, 2.0, 3.0]) + min_val = 1.5 + result = torch.tensor([1.5, 2.0, 3.0]) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (min_val, None)}), + init_data=init_data, + result=result, + ) + + # test modifier + for _ in range(test_cfg.num_iter): + output = test_cfg.cfg.func(test_cfg.init_data, **test_cfg.cfg.params) + assert torch.allclose(output, test_cfg.result) + + +def test_clip_no_lower_bound_modifier(): + """Test clip modifier with no lower bound.""" + # create test data + init_data = torch.tensor([1.0, 2.0, 3.0]) + max_val = 2.5 + result = torch.tensor([1.0, 2.0, 2.5]) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (None, max_val)}), + init_data=init_data, + result=result, + ) + + # test modifier + for _ in range(test_cfg.num_iter): + output = test_cfg.cfg.func(test_cfg.init_data, **test_cfg.cfg.params) + assert torch.allclose(output, test_cfg.result) + + +def test_torch_relu_modifier(): + """Test torch relu modifier.""" + # create test data + init_data = torch.tensor([-1.0, 0.0, 1.0]) + result = torch.tensor([0.0, 0.0, 1.0]) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.ModifierCfg(func=torch.nn.functional.relu), + init_data=init_data, + result=result, + ) + + # test modifier + for _ in range(test_cfg.num_iter): + output = test_cfg.cfg.func(test_cfg.init_data) + assert torch.allclose(output, test_cfg.result) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_digital_filter(device): + """Test digital filter modifier.""" + # create test data + init_data = torch.tensor([0.0, 0.0, 0.0], device=device) + A = [0.0, 0.1] + B = [0.5, 0.5] + result = torch.tensor([-0.45661893, -0.45661893, -0.45661893], device=device) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.DigitalFilterCfg(A=A, B=B), init_data=init_data, result=result, num_iter=16 + ) + + # create a modifier instance + modifier_obj = test_cfg.cfg.func(test_cfg.cfg, test_cfg.init_data.shape, device=device) + + # test the modifier + theta = torch.tensor([0.0], device=device) + delta = torch.pi / torch.tensor([8.0, 8.0, 8.0], device=device) + + for _ in range(5): + # reset the modifier + modifier_obj.reset() + + # apply the modifier multiple times + for i in range(test_cfg.num_iter): + data = torch.sin(theta + i * delta) + processed_data = modifier_obj(data) + + assert data.shape == processed_data.shape, "Modified data shape does not equal original" + + # check if the modified data is close to the expected result + torch.testing.assert_close(processed_data, test_cfg.result) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_integral(device): + """Test integral modifier.""" + # create test data + init_data = torch.tensor([0.0], device=device) + dt = 1.0 + result = torch.tensor([12.5], device=device) + + # create test config + test_cfg = ModifierTestCfg( + cfg=modifiers.IntegratorCfg(dt=dt), + init_data=init_data, + result=result, + num_iter=6, + ) + + # create a modifier instance + modifier_obj = test_cfg.cfg.func(test_cfg.cfg, test_cfg.init_data.shape, device=device) + + # test the modifier + delta = torch.tensor(1.0, device=device) + + for _ in range(5): + # reset the modifier + modifier_obj.reset() + + # clone the data to avoid modifying the original + data = test_cfg.init_data.clone() + # apply the modifier multiple times + for _ in range(test_cfg.num_iter): + processed_data = modifier_obj(data) + data = data + delta + + assert data.shape == processed_data.shape, "Modified data shape does not equal original" + + # check if the modified data is close to the expected result + torch.testing.assert_close(processed_data, test_cfg.result) diff --git a/source/isaaclab/test/utils/test_noise.py b/source/isaaclab/test/utils/test_noise.py index 0bbc3bcbd6a8..176371d381f6 100644 --- a/source/isaaclab/test/utils/test_noise.py +++ b/source/isaaclab/test/utils/test_noise.py @@ -1,119 +1,110 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" +import pytest import torch -import unittest import isaaclab.utils.noise as noise -class TestNoise(unittest.TestCase): - """Test different noise implementations.""" - - def test_gaussian_noise(self): - """Test guassian_noise function.""" - - for device in ["cpu", "cuda"]: - for noise_device in ["cpu", "cuda"]: - for op in ["add", "scale", "abs"]: - with self.subTest(device=device, noise_device=noise_device, operation=op): - # create random data set - data = torch.rand(10000, 3, device=device) - # define standard deviation and mean - std = torch.tensor([0.1, 0.2, 0.3], device=noise_device) - mean = torch.tensor([0.4, 0.5, 0.6], device=noise_device) - # create noise config - noise_cfg = noise.GaussianNoiseCfg(std=std, mean=mean, operation=op) - - for i in range(10): - # apply noise - noisy_data = noise_cfg.func(data, cfg=noise_cfg) - # calculate resulting noise compared to original data set - if op == "add": - std_result, mean_result = torch.std_mean(noisy_data - data, dim=0) - elif op == "scale": - std_result, mean_result = torch.std_mean(noisy_data / data, dim=0) - elif op == "abs": - std_result, mean_result = torch.std_mean(noisy_data, dim=0) - - self.assertTrue(noise_cfg.mean.device, device) - self.assertTrue(noise_cfg.std.device, device) - torch.testing.assert_close(noise_cfg.std, std_result, atol=1e-2, rtol=1e-2) - torch.testing.assert_close(noise_cfg.mean, mean_result, atol=1e-2, rtol=1e-2) - - def test_uniform_noise(self): - """Test uniform_noise function.""" - for device in ["cpu", "cuda"]: - for noise_device in ["cpu", "cuda"]: - for op in ["add", "scale", "abs"]: - with self.subTest(device=device, noise_device=noise_device, operation=op): - # create random data set - data = torch.rand(10000, 3, device=device) - # define uniform minimum and maximum - n_min = torch.tensor([0.1, 0.2, 0.3], device=noise_device) - n_max = torch.tensor([0.4, 0.5, 0.6], device=noise_device) - # create noise config - noise_cfg = noise.UniformNoiseCfg(n_max=n_max, n_min=n_min, operation=op) - - for i in range(10): - # apply noise - noisy_data = noise_cfg.func(data, cfg=noise_cfg) - # calculate resulting noise compared to original data set - if op == "add": - min_result, _ = torch.min(noisy_data - data, dim=0) - max_result, _ = torch.max(noisy_data - data, dim=0) - elif op == "scale": - min_result, _ = torch.min(torch.div(noisy_data, data), dim=0) - max_result, _ = torch.max(torch.div(noisy_data, data), dim=0) - elif op == "abs": - min_result, _ = torch.min(noisy_data, dim=0) - max_result, _ = torch.max(noisy_data, dim=0) - - self.assertTrue(noise_cfg.n_min.device, device) - self.assertTrue(noise_cfg.n_max.device, device) - # add a small epsilon to accommodate for floating point error - self.assertTrue(all(torch.le(noise_cfg.n_min - 1e-5, min_result).tolist())) - self.assertTrue(all(torch.ge(noise_cfg.n_max + 1e-5, max_result).tolist())) - - def test_constant_noise(self): - """Test constant_noise""" - for device in ["cpu", "cuda"]: - for noise_device in ["cpu", "cuda"]: - for op in ["add", "scale", "abs"]: - with self.subTest(device=device, noise_device=noise_device, operation=op): - # create random data set - data = torch.rand(10000, 3, device=device) - # define a bias - bias = torch.tensor([0.1, 0.2, 0.3], device=noise_device) - # create noise config - noise_cfg = noise.ConstantNoiseCfg(bias=bias, operation=op) - - for i in range(10): - # apply noise - noisy_data = noise_cfg.func(data, cfg=noise_cfg) - # calculate resulting noise compared to original data set - if op == "add": - bias_result = noisy_data - data - elif op == "scale": - bias_result = noisy_data / data - elif op == "abs": - bias_result = noisy_data - - self.assertTrue(noise_cfg.bias.device, device) - torch.testing.assert_close(noise_cfg.bias.repeat(data.shape[0], 1), bias_result) - - -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("noise_device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("op", ["add", "scale", "abs"]) +def test_gaussian_noise(device, noise_device, op): + """Test guassian_noise function.""" + + # create random data set + data = torch.rand(10000, 3, device=device) + # define standard deviation and mean + std = torch.tensor([0.1, 0.2, 0.3], device=noise_device) + mean = torch.tensor([0.4, 0.5, 0.6], device=noise_device) + # create noise config + noise_cfg = noise.GaussianNoiseCfg(std=std, mean=mean, operation=op) + + for i in range(10): + # apply noise + noisy_data = noise_cfg.func(data, cfg=noise_cfg) + # calculate resulting noise compared to original data set + if op == "add": + std_result, mean_result = torch.std_mean(noisy_data - data, dim=0) + elif op == "scale": + std_result, mean_result = torch.std_mean(noisy_data / data, dim=0) + elif op == "abs": + std_result, mean_result = torch.std_mean(noisy_data, dim=0) + + assert str(noise_cfg.mean.device) == device + assert str(noise_cfg.std.device) == device + torch.testing.assert_close(noise_cfg.std, std_result, atol=1e-2, rtol=1e-2) + torch.testing.assert_close(noise_cfg.mean, mean_result, atol=1e-2, rtol=1e-2) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("noise_device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("op", ["add", "scale", "abs"]) +def test_uniform_noise(device, noise_device, op): + """Test uniform_noise function.""" + # create random data set + data = torch.rand(10000, 3, device=device) + # define uniform minimum and maximum + n_min = torch.tensor([0.1, 0.2, 0.3], device=noise_device) + n_max = torch.tensor([0.4, 0.5, 0.6], device=noise_device) + # create noise config + noise_cfg = noise.UniformNoiseCfg(n_max=n_max, n_min=n_min, operation=op) + + for i in range(10): + # apply noise + noisy_data = noise_cfg.func(data, cfg=noise_cfg) + # calculate resulting noise compared to original data set + if op == "add": + min_result, _ = torch.min(noisy_data - data, dim=0) + max_result, _ = torch.max(noisy_data - data, dim=0) + elif op == "scale": + min_result, _ = torch.min(torch.div(noisy_data, data), dim=0) + max_result, _ = torch.max(torch.div(noisy_data, data), dim=0) + elif op == "abs": + min_result, _ = torch.min(noisy_data, dim=0) + max_result, _ = torch.max(noisy_data, dim=0) + + assert str(noise_cfg.n_min.device) == device + assert str(noise_cfg.n_max.device) == device + # add a small epsilon to accommodate for floating point error + assert all(torch.le(noise_cfg.n_min - 1e-5, min_result).tolist()) + assert all(torch.ge(noise_cfg.n_max + 1e-5, max_result).tolist()) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("noise_device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("op", ["add", "scale", "abs"]) +def test_constant_noise(device, noise_device, op): + """Test constant_noise""" + # create random data set + data = torch.rand(10000, 3, device=device) + # define a bias + bias = torch.tensor([0.1, 0.2, 0.3], device=noise_device) + # create noise config + noise_cfg = noise.ConstantNoiseCfg(bias=bias, operation=op) + + for i in range(10): + # apply noise + noisy_data = noise_cfg.func(data, cfg=noise_cfg) + # calculate resulting noise compared to original data set + if op == "add": + bias_result = noisy_data - data + elif op == "scale": + bias_result = noisy_data / data + elif op == "abs": + bias_result = noisy_data + + assert str(noise_cfg.bias.device) == device + torch.testing.assert_close(noise_cfg.bias.repeat(data.shape[0], 1), bias_result) diff --git a/source/isaaclab/test/utils/test_string.py b/source/isaaclab/test/utils/test_string.py index e71810a19dd3..d171a3885e10 100644 --- a/source/isaaclab/test/utils/test_string.py +++ b/source/isaaclab/test/utils/test_string.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,189 +7,209 @@ # because warp is only available in the context of a running simulation """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import random -import unittest + +import pytest import isaaclab.utils.string as string_utils -class TestStringUtilities(unittest.TestCase): - """Test fixture for checking string utilities.""" - - def test_case_conversion(self): - """Test case conversion between camel case and snake case.""" - # test camel case to snake case - self.assertEqual(string_utils.to_snake_case("CamelCase"), "camel_case") - self.assertEqual(string_utils.to_snake_case("camelCase"), "camel_case") - self.assertEqual(string_utils.to_snake_case("CamelCaseString"), "camel_case_string") - # test snake case to camel case - self.assertEqual(string_utils.to_camel_case("snake_case", to="CC"), "SnakeCase") - self.assertEqual(string_utils.to_camel_case("snake_case_string", to="CC"), "SnakeCaseString") - self.assertEqual(string_utils.to_camel_case("snake_case_string", to="cC"), "snakeCaseString") - - def test_resolve_matching_names_with_basic_strings(self): - """Test resolving matching names with a basic expression.""" - # list of strings - target_names = ["a", "b", "c", "d", "e"] - # test matching names - query_names = ["a|c", "b"] - index_list, names_list = string_utils.resolve_matching_names(query_names, target_names) - self.assertEqual(index_list, [0, 1, 2]) - self.assertEqual(names_list, ["a", "b", "c"]) - # test matching names with regex - query_names = ["a.*", "b"] - index_list, names_list = string_utils.resolve_matching_names(query_names, target_names) - self.assertEqual(index_list, [0, 1]) - self.assertEqual(names_list, ["a", "b"]) - # test duplicate names - query_names = ["a|c", "b", "a|c"] - with self.assertRaises(ValueError): - _ = string_utils.resolve_matching_names(query_names, target_names) - # test no regex match - query_names = ["a|c", "b", "f"] - with self.assertRaises(ValueError): - _ = string_utils.resolve_matching_names(query_names, target_names) - - def test_resolve_matching_names_with_joint_name_strings(self): - """Test resolving matching names with joint names.""" - # list of strings - robot_joint_names = [] - for i in ["hip", "thigh", "calf"]: - for j in ["FL", "FR", "RL", "RR"]: - robot_joint_names.append(f"{j}_{i}_joint") - # test matching names - index_list, names_list = string_utils.resolve_matching_names(".*", robot_joint_names) - self.assertEqual(index_list, list(range(len(robot_joint_names)))) - self.assertEqual(names_list, robot_joint_names) - # test matching names with regex - index_list, names_list = string_utils.resolve_matching_names(".*_joint", robot_joint_names) - self.assertEqual(index_list, list(range(len(robot_joint_names)))) - self.assertEqual(names_list, robot_joint_names) - # test matching names with regex - index_list, names_list = string_utils.resolve_matching_names(["FL.*", "FR.*"], robot_joint_names) - ground_truth_index_list = [0, 1, 4, 5, 8, 9] - self.assertEqual(index_list, ground_truth_index_list) - self.assertEqual(names_list, [robot_joint_names[i] for i in ground_truth_index_list]) - # test matching names with regex - query_list = [ - "FL_hip_joint", - "FL_thigh_joint", - "FR_hip_joint", - "FR_thigh_joint", - "FL_calf_joint", - "FR_calf_joint", - ] - index_list, names_list = string_utils.resolve_matching_names(query_list, robot_joint_names) - ground_truth_index_list = [0, 1, 4, 5, 8, 9] - self.assertNotEqual(names_list, query_list) - self.assertEqual(index_list, ground_truth_index_list) - self.assertEqual(names_list, [robot_joint_names[i] for i in ground_truth_index_list]) - # test matching names with regex but shuffled - # randomize order of previous query list - random.shuffle(query_list) - index_list, names_list = string_utils.resolve_matching_names(query_list, robot_joint_names) - ground_truth_index_list = [0, 1, 4, 5, 8, 9] - self.assertNotEqual(names_list, query_list) - self.assertEqual(index_list, ground_truth_index_list) - self.assertEqual(names_list, [robot_joint_names[i] for i in ground_truth_index_list]) - - def test_resolve_matching_names_with_preserved_order(self): - # list of strings and query list - robot_joint_names = [] - for i in ["hip", "thigh", "calf"]: - for j in ["FL", "FR", "RL", "RR"]: - robot_joint_names.append(f"{j}_{i}_joint") - query_list = [ - "FL_hip_joint", - "FL_thigh_joint", - "FR_hip_joint", - "FR_thigh_joint", - "FL_calf_joint", - "FR_calf_joint", - ] - # test return in target ordering with sublist - query_list.reverse() - index_list, names_list = string_utils.resolve_matching_names(query_list, robot_joint_names, preserve_order=True) - ground_truth_index_list = [9, 8, 5, 1, 4, 0] - self.assertEqual(names_list, query_list) - self.assertEqual(index_list, ground_truth_index_list) - # test return in target ordering with regex expression - index_list, names_list = string_utils.resolve_matching_names( - ["FR.*", "FL.*"], robot_joint_names, preserve_order=True - ) - ground_truth_index_list = [1, 5, 9, 0, 4, 8] - self.assertEqual(index_list, ground_truth_index_list) - self.assertEqual(names_list, [robot_joint_names[i] for i in ground_truth_index_list]) - # test return in target ordering with a mix of regex and non-regex expression - index_list, names_list = string_utils.resolve_matching_names( - ["FR.*", "FL_calf_joint", "FL_thigh_joint", "FL_hip_joint"], robot_joint_names, preserve_order=True - ) - ground_truth_index_list = [1, 5, 9, 8, 4, 0] - self.assertEqual(index_list, ground_truth_index_list) - self.assertEqual(names_list, [robot_joint_names[i] for i in ground_truth_index_list]) - - def test_resolve_matching_names_values_with_basic_strings(self): - """Test resolving matching names with a basic expression.""" - # list of strings - target_names = ["a", "b", "c", "d", "e"] - # test matching names - data = {"a|c": 1, "b": 2} - index_list, names_list, values_list = string_utils.resolve_matching_names_values(data, target_names) - self.assertEqual(index_list, [0, 1, 2]) - self.assertEqual(names_list, ["a", "b", "c"]) - self.assertEqual(values_list, [1, 2, 1]) - # test matching names with regex - data = {"a|d|e": 1, "b|c": 2} - index_list, names_list, values_list = string_utils.resolve_matching_names_values(data, target_names) - self.assertEqual(index_list, [0, 1, 2, 3, 4]) - self.assertEqual(names_list, ["a", "b", "c", "d", "e"]) - self.assertEqual(values_list, [1, 2, 2, 1, 1]) - # test matching names with regex - data = {"a|d|e|b": 1, "b|c": 2} - with self.assertRaises(ValueError): - _ = string_utils.resolve_matching_names_values(data, target_names) - # test no regex match - query_names = {"a|c": 1, "b": 0, "f": 2} - with self.assertRaises(ValueError): - _ = string_utils.resolve_matching_names_values(query_names, target_names) - - def test_resolve_matching_names_values_with_basic_strings_and_preserved_order(self): - """Test resolving matching names with a basic expression.""" - # list of strings - target_names = ["a", "b", "c", "d", "e"] - # test matching names - data = {"a|c": 1, "b": 2} - index_list, names_list, values_list = string_utils.resolve_matching_names_values( - data, target_names, preserve_order=True - ) - self.assertEqual(index_list, [0, 2, 1]) - self.assertEqual(names_list, ["a", "c", "b"]) - self.assertEqual(values_list, [1, 1, 2]) - # test matching names with regex - data = {"a|d|e": 1, "b|c": 2} - index_list, names_list, values_list = string_utils.resolve_matching_names_values( - data, target_names, preserve_order=True - ) - self.assertEqual(index_list, [0, 3, 4, 1, 2]) - self.assertEqual(names_list, ["a", "d", "e", "b", "c"]) - self.assertEqual(values_list, [1, 1, 1, 2, 2]) - # test matching names with regex - data = {"a|d|e|b": 1, "b|c": 2} - with self.assertRaises(ValueError): - _ = string_utils.resolve_matching_names_values(data, target_names, preserve_order=True) - # test no regex match - query_names = {"a|c": 1, "b": 0, "f": 2} - with self.assertRaises(ValueError): - _ = string_utils.resolve_matching_names_values(query_names, target_names, preserve_order=True) - - -if __name__ == "__main__": - run_tests() +def test_case_conversion(): + """Test case conversion between camel case and snake case.""" + # test camel case to snake case + assert string_utils.to_snake_case("CamelCase") == "camel_case" + assert string_utils.to_snake_case("camelCase") == "camel_case" + assert string_utils.to_snake_case("CamelCaseString") == "camel_case_string" + # test snake case to camel case + assert string_utils.to_camel_case("snake_case", to="CC") == "SnakeCase" + assert string_utils.to_camel_case("snake_case_string", to="CC") == "SnakeCaseString" + assert string_utils.to_camel_case("snake_case_string", to="cC") == "snakeCaseString" + + +def test_resolve_matching_names_with_basic_strings(): + """Test resolving matching names with a basic expression.""" + # list of strings + target_names = ["a", "b", "c", "d", "e"] + # test matching names + query_names = ["a|c", "b"] + index_list, names_list = string_utils.resolve_matching_names(query_names, target_names) + assert index_list == [0, 1, 2] + assert names_list == ["a", "b", "c"] + # test matching names with regex + query_names = ["a.*", "b"] + index_list, names_list = string_utils.resolve_matching_names(query_names, target_names) + assert index_list == [0, 1] + assert names_list == ["a", "b"] + # test duplicate names + query_names = ["a|c", "b", "a|c"] + with pytest.raises(ValueError): + _ = string_utils.resolve_matching_names(query_names, target_names) + # test no regex match + query_names = ["a|c", "b", "f"] + with pytest.raises(ValueError): + _ = string_utils.resolve_matching_names(query_names, target_names) + + +def test_resolve_matching_names_with_joint_name_strings(): + """Test resolving matching names with joint names.""" + # list of strings + robot_joint_names = [] + for i in ["hip", "thigh", "calf"]: + for j in ["FL", "FR", "RL", "RR"]: + robot_joint_names.append(f"{j}_{i}_joint") + # test matching names + index_list, names_list = string_utils.resolve_matching_names(".*", robot_joint_names) + assert index_list == list(range(len(robot_joint_names))) + assert names_list == robot_joint_names + # test matching names with regex + index_list, names_list = string_utils.resolve_matching_names(".*_joint", robot_joint_names) + assert index_list == list(range(len(robot_joint_names))) + assert names_list == robot_joint_names + # test matching names with regex + index_list, names_list = string_utils.resolve_matching_names(["FL.*", "FR.*"], robot_joint_names) + ground_truth_index_list = [0, 1, 4, 5, 8, 9] + assert index_list == ground_truth_index_list + assert names_list == [robot_joint_names[i] for i in ground_truth_index_list] + # test matching names with regex + query_list = [ + "FL_hip_joint", + "FL_thigh_joint", + "FR_hip_joint", + "FR_thigh_joint", + "FL_calf_joint", + "FR_calf_joint", + ] + index_list, names_list = string_utils.resolve_matching_names(query_list, robot_joint_names) + ground_truth_index_list = [0, 1, 4, 5, 8, 9] + assert names_list != query_list + assert index_list == ground_truth_index_list + assert names_list == [robot_joint_names[i] for i in ground_truth_index_list] + # test matching names with regex but shuffled + # randomize order of previous query list + random.shuffle(query_list) + index_list, names_list = string_utils.resolve_matching_names(query_list, robot_joint_names) + ground_truth_index_list = [0, 1, 4, 5, 8, 9] + assert names_list != query_list + assert index_list == ground_truth_index_list + assert names_list == [robot_joint_names[i] for i in ground_truth_index_list] + + +def test_resolve_matching_names_with_preserved_order(): + """Test resolving matching names with preserved order.""" + # list of strings and query list + robot_joint_names = [] + for i in ["hip", "thigh", "calf"]: + for j in ["FL", "FR", "RL", "RR"]: + robot_joint_names.append(f"{j}_{i}_joint") + query_list = [ + "FL_hip_joint", + "FL_thigh_joint", + "FR_hip_joint", + "FR_thigh_joint", + "FL_calf_joint", + "FR_calf_joint", + ] + # test return in target ordering with sublist + query_list.reverse() + index_list, names_list = string_utils.resolve_matching_names(query_list, robot_joint_names, preserve_order=True) + ground_truth_index_list = [9, 8, 5, 1, 4, 0] + assert names_list == query_list + assert index_list == ground_truth_index_list + # test return in target ordering with regex expression + index_list, names_list = string_utils.resolve_matching_names( + ["FR.*", "FL.*"], robot_joint_names, preserve_order=True + ) + ground_truth_index_list = [1, 5, 9, 0, 4, 8] + assert index_list == ground_truth_index_list + assert names_list == [robot_joint_names[i] for i in ground_truth_index_list] + # test return in target ordering with a mix of regex and non-regex expression + index_list, names_list = string_utils.resolve_matching_names( + ["FR.*", "FL_calf_joint", "FL_thigh_joint", "FL_hip_joint"], robot_joint_names, preserve_order=True + ) + ground_truth_index_list = [1, 5, 9, 8, 4, 0] + assert index_list == ground_truth_index_list + assert names_list == [robot_joint_names[i] for i in ground_truth_index_list] + + +def test_resolve_matching_names_values_with_basic_strings(): + """Test resolving matching names with a basic expression.""" + # list of strings + target_names = ["a", "b", "c", "d", "e"] + # test matching names + data = {"a|c": 1, "b": 2} + index_list, names_list, values_list = string_utils.resolve_matching_names_values(data, target_names) + assert index_list == [0, 1, 2] + assert names_list == ["a", "b", "c"] + assert values_list == [1, 2, 1] + # test matching names with regex + data = {"a|d|e": 1, "b|c": 2} + index_list, names_list, values_list = string_utils.resolve_matching_names_values(data, target_names) + assert index_list == [0, 1, 2, 3, 4] + assert names_list == ["a", "b", "c", "d", "e"] + assert values_list == [1, 2, 2, 1, 1] + # test matching names with regex + data = {"a|d|e|b": 1, "b|c": 2} + with pytest.raises(ValueError): + _ = string_utils.resolve_matching_names_values(data, target_names) + # test no regex match + query_names = {"a|c": 1, "b": 0, "f": 2} + with pytest.raises(ValueError): + _ = string_utils.resolve_matching_names_values(query_names, target_names) + + +def test_resolve_matching_names_values_with_strict_false(): + """Test resolving matching names with strict=False parameter.""" + # list of strings + target_names = ["a", "b", "c", "d", "e"] + # test strict=False + data = {"a|c": 1, "b": 2, "f": 3} + index_list, names_list, values_list = string_utils.resolve_matching_names_values(data, target_names, strict=False) + assert index_list == [0, 1, 2] + assert names_list == ["a", "b", "c"] + assert values_list == [1, 2, 1] + + # test failure case: multiple matches for a string (should still raise ValueError even with strict=False) + data = {"a|c": 1, "a": 2, "b": 3} + with pytest.raises(ValueError, match="Multiple matches for 'a':"): + _ = string_utils.resolve_matching_names_values(data, target_names, strict=False) + + # test failure case: invalid input type (should still raise TypeError even with strict=False) + with pytest.raises(TypeError, match="Input argument `data` should be a dictionary"): + _ = string_utils.resolve_matching_names_values("not_a_dict", target_names, strict=False) + + +def test_resolve_matching_names_values_with_basic_strings_and_preserved_order(): + """Test resolving matching names with a basic expression.""" + # list of strings + target_names = ["a", "b", "c", "d", "e"] + # test matching names + data = {"a|c": 1, "b": 2} + index_list, names_list, values_list = string_utils.resolve_matching_names_values( + data, target_names, preserve_order=True + ) + assert index_list == [0, 2, 1] + assert names_list == ["a", "c", "b"] + assert values_list == [1, 1, 2] + # test matching names with regex + data = {"a|d|e": 1, "b|c": 2} + index_list, names_list, values_list = string_utils.resolve_matching_names_values( + data, target_names, preserve_order=True + ) + assert index_list == [0, 3, 4, 1, 2] + assert names_list == ["a", "d", "e", "b", "c"] + assert values_list == [1, 1, 1, 2, 2] + # test matching names with regex + data = {"a|d|e|b": 1, "b|c": 2} + with pytest.raises(ValueError): + _ = string_utils.resolve_matching_names_values(data, target_names, preserve_order=True) + # test no regex match + query_names = {"a|c": 1, "b": 0, "f": 2} + with pytest.raises(ValueError): + _ = string_utils.resolve_matching_names_values(query_names, target_names, preserve_order=True) diff --git a/source/isaaclab/test/utils/test_timer.py b/source/isaaclab/test/utils/test_timer.py index 79fb221eb8b7..8d99db3b2d80 100644 --- a/source/isaaclab/test/utils/test_timer.py +++ b/source/isaaclab/test/utils/test_timer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,44 +7,35 @@ # because warp is only available in the context of a running simulation """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch omniverse app -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app +simulation_app = AppLauncher(headless=True).app """Rest everything follows.""" import time -import unittest from isaaclab.utils.timer import Timer +# number of decimal places to check +PRECISION_PLACES = 2 -class TestTimer(unittest.TestCase): - """Test fixture for the Timer class.""" - def setUp(self): - # number of decimal places to check - self.precision_places = 2 +def test_timer_as_object(): + """Test using a `Timer` as a regular object.""" + timer = Timer() + timer.start() + assert abs(0 - timer.time_elapsed) < 10 ** (-PRECISION_PLACES) + time.sleep(1) + assert abs(1 - timer.time_elapsed) < 10 ** (-PRECISION_PLACES) + timer.stop() + assert abs(1 - timer.total_run_time) < 10 ** (-PRECISION_PLACES) - def test_timer_as_object(self): - """Test using a `Timer` as a regular object.""" - timer = Timer() - timer.start() - self.assertAlmostEqual(0, timer.time_elapsed, self.precision_places) - time.sleep(1) - self.assertAlmostEqual(1, timer.time_elapsed, self.precision_places) - timer.stop() - self.assertAlmostEqual(1, timer.total_run_time, self.precision_places) - - def test_timer_as_context_manager(self): - """Test using a `Timer` as a context manager.""" - with Timer() as timer: - self.assertAlmostEqual(0, timer.time_elapsed, self.precision_places) - time.sleep(1) - self.assertAlmostEqual(1, timer.time_elapsed, self.precision_places) - -if __name__ == "__main__": - run_tests() +def test_timer_as_context_manager(): + """Test using a `Timer` as a context manager.""" + with Timer() as timer: + assert abs(0 - timer.time_elapsed) < 10 ** (-PRECISION_PLACES) + time.sleep(1) + assert abs(1 - timer.time_elapsed) < 10 ** (-PRECISION_PLACES) diff --git a/source/isaaclab/test/utils/test_version.py b/source/isaaclab/test/utils/test_version.py new file mode 100644 index 000000000000..ba737b53643e --- /dev/null +++ b/source/isaaclab/test/utils/test_version.py @@ -0,0 +1,152 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for version comparison utilities.""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest +from packaging.version import Version + +from isaaclab.utils.version import compare_versions, get_isaac_sim_version + + +def test_get_isaac_sim_version(): + """Test that get_isaac_sim_version returns cached Version object.""" + # Call twice to ensure caching works + version1 = get_isaac_sim_version() + version2 = get_isaac_sim_version() + + # Should return the same object (cached) + assert version1 is version2 + + # Should return a packaging.version.Version object + assert isinstance(version1, Version) + + # Major version should be reasonable + assert version1.major >= 4 + + # Minor and micro should be non-negative + assert version1.minor >= 0 + assert version1.micro >= 0 + + +def test_get_isaac_sim_version_format(): + """Test that get_isaac_sim_version returns correct format.""" + isaac_version = get_isaac_sim_version() + + # Should be able to convert to string + version_str = str(isaac_version) + assert isinstance(version_str, str) + + # Should have proper format (e.g., "5.0.0") + parts = version_str.split(".") + assert len(parts) >= 3 + + # Can access components + assert hasattr(isaac_version, "major") + assert hasattr(isaac_version, "minor") + assert hasattr(isaac_version, "micro") + + +def test_version_caching_performance(): + """Test that caching improves performance for version checks.""" + # First call (will cache) + version1 = get_isaac_sim_version() + + # Subsequent calls should be instant (from cache) + for _ in range(100): + version = get_isaac_sim_version() + assert version == version1 + assert version is version1 # Should be the exact same object + + +def test_version_comparison_operators(): + """Test that Version objects support natural comparisons.""" + isaac_version = get_isaac_sim_version() + + # Should support comparison operators + assert isaac_version >= Version("4.0.0") + assert isaac_version == isaac_version + + # Test less than + if isaac_version.major >= 5: + assert isaac_version > Version("4.5.0") + assert isaac_version >= Version("5.0.0") + + # Test not equal + assert isaac_version != Version("0.0.1") + + +@pytest.mark.parametrize( + "v1,v2,expected", + [ + # Equal versions + ("1.0.0", "1.0.0", 0), + ("2.5.3", "2.5.3", 0), + # Equal with different lengths (implicit zeros) + ("1.0", "1.0.0", 0), + ("1", "1.0.0.0", 0), + ("2.5", "2.5.0.0", 0), + # Major version differences + ("2.0.0", "1.0.0", 1), + ("1.0.0", "2.0.0", -1), + ("2.0.0", "1.99.99", 1), + # Minor version differences + ("1.5.0", "1.4.0", 1), + ("1.4.0", "1.5.0", -1), + ("1.10.0", "1.9.99", 1), + # Patch version differences + ("1.0.5", "1.0.4", 1), + ("1.0.4", "1.0.5", -1), + ("2.5.10", "2.5.9", 1), + # Single/double digit versions + ("2", "1", 1), + ("1", "2", -1), + ("1.5", "1.4", 1), + # Extended versions + ("1.0.0.1", "1.0.0.0", 1), + ("1.2.3.4.5", "1.2.3.4", 1), + # Zero versions + ("0.0.1", "0.0.0", 1), + ("0.1.0", "0.0.9", 1), + ("0", "0.0.0", 0), + # Large numbers + ("100.200.300", "100.200.299", 1), + ("999.999.999", "1000.0.0", -1), + ], +) +def test_version_comparisons(v1, v2, expected): + """Test version comparisons with various scenarios.""" + assert compare_versions(v1, v2) == expected + + +def test_symmetry(): + """Test anti-symmetric property: if v1 < v2, then v2 > v1.""" + test_pairs = [("1.0.0", "2.0.0"), ("1.5.3", "1.4.9"), ("1.0.0", "1.0.0")] + + for v1, v2 in test_pairs: + result1 = compare_versions(v1, v2) + result2 = compare_versions(v2, v1) + + if result1 == 0: + assert result2 == 0 + else: + assert result1 == -result2 + + +def test_transitivity(): + """Test transitive property: if v1 < v2 < v3, then v1 < v3.""" + v1, v2, v3 = "1.0.0", "2.0.0", "3.0.0" + assert compare_versions(v1, v2) == -1 + assert compare_versions(v2, v3) == -1 + assert compare_versions(v1, v3) == -1 diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py new file mode 100644 index 000000000000..3cc88b3b9028 --- /dev/null +++ b/source/isaaclab/test/utils/test_wrench_composer.py @@ -0,0 +1,712 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import numpy as np +import pytest +import torch +import warp as wp + +from isaaclab.assets import RigidObject +from isaaclab.utils.wrench_composer import WrenchComposer + + +class MockAssetData: + """Mock data class that provides body link positions and quaternions.""" + + def __init__( + self, + num_envs: int, + num_bodies: int, + device: str, + link_pos: torch.Tensor | None = None, + link_quat: torch.Tensor | None = None, + ): + """Initialize mock asset data. + + Args: + num_envs: Number of environments. + num_bodies: Number of bodies. + device: Device to use. + link_pos: Optional link positions (num_envs, num_bodies, 3). Defaults to zeros. + link_quat: Optional link quaternions in (w, x, y, z) format (num_envs, num_bodies, 4). + Defaults to identity quaternion. + """ + if link_pos is not None: + self.body_link_pos_w = link_pos.to(device=device, dtype=torch.float32) + else: + self.body_link_pos_w = torch.zeros((num_envs, num_bodies, 3), dtype=torch.float32, device=device) + + if link_quat is not None: + self.body_link_quat_w = link_quat.to(device=device, dtype=torch.float32) + else: + # Identity quaternion (w, x, y, z) = (1, 0, 0, 0) + self.body_link_quat_w = torch.zeros((num_envs, num_bodies, 4), dtype=torch.float32, device=device) + self.body_link_quat_w[..., 0] = 1.0 + + +class MockRigidObject: + """Mock RigidObject that provides the minimal interface required by WrenchComposer. + + This mock enables testing WrenchComposer in isolation without requiring a full simulation setup. + It passes isinstance checks by registering as a virtual subclass of RigidObject. + """ + + def __init__( + self, + num_envs: int, + num_bodies: int, + device: str, + link_pos: torch.Tensor | None = None, + link_quat: torch.Tensor | None = None, + ): + """Initialize mock rigid object. + + Args: + num_envs: Number of environments. + num_bodies: Number of bodies. + device: Device to use. + link_pos: Optional link positions (num_envs, num_bodies, 3). + link_quat: Optional link quaternions in (w, x, y, z) format (num_envs, num_bodies, 4). + """ + self.num_instances = num_envs + self.num_bodies = num_bodies + self.device = device + self.data = MockAssetData(num_envs, num_bodies, device, link_pos, link_quat) + + +# --- Helper functions for quaternion math --- + + +def quat_rotate_inv_np(quat_wxyz: np.ndarray, vec: np.ndarray) -> np.ndarray: + """Rotate a vector by the inverse of a quaternion (numpy). + + Args: + quat_wxyz: Quaternion in (w, x, y, z) format. Shape: (..., 4) + vec: Vector to rotate. Shape: (..., 3) + + Returns: + Rotated vector. Shape: (..., 3) + """ + # Extract components + w = quat_wxyz[..., 0:1] + xyz = quat_wxyz[..., 1:4] + + # For inverse rotation, we conjugate the quaternion (negate xyz) + # q^-1 * v * q = q_conj * v * q_conj^-1 for unit quaternion + # Using the formula: v' = v + 2*w*(xyz x v) + 2*(xyz x (xyz x v)) + # But for inverse: use -xyz + + # Cross product: xyz x vec + t = 2.0 * np.cross(-xyz, vec, axis=-1) + # Result: vec + w*t + xyz x t + return vec + w * t + np.cross(-xyz, t, axis=-1) + + +def random_unit_quaternion_np(rng: np.random.Generator, shape: tuple) -> np.ndarray: + """Generate random unit quaternions in (w, x, y, z) format. + + Args: + rng: Random number generator. + shape: Output shape, e.g. (num_envs, num_bodies). + + Returns: + Random unit quaternions. Shape: (*shape, 4) + """ + # Generate random quaternion components + q = rng.standard_normal(shape + (4,)).astype(np.float32) + # Normalize to unit quaternion + q = q / np.linalg.norm(q, axis=-1, keepdims=True) + return q + + +# Register MockRigidObject as a virtual subclass of RigidObject +# This allows isinstance(mock, RigidObject) to return True +RigidObject.register(MockRigidObject) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) +def test_wrench_composer_add_force(device: str, num_envs: int, num_bodies: int): + # Initialize random number generator + rng = np.random.default_rng(seed=0) + + for _ in range(10): + mock_asset = MockRigidObject(num_envs, num_bodies, device) + wrench_composer = WrenchComposer(mock_asset) + # Initialize hand-calculated composed force + hand_calculated_composed_force_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + for _ in range(10): + # Get random number of envs and bodies and their indices + num_envs_np = rng.integers(1, num_envs, endpoint=True) + num_bodies_np = rng.integers(1, num_bodies, endpoint=True) + env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) + body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) + # Convert to warp arrays + env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) + body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) + # Get random forces + forces_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) + # Add forces to wrench composer + wrench_composer.add_forces_and_torques(forces=forces, body_ids=body_ids, env_ids=env_ids) + # Add forces to hand-calculated composed force + hand_calculated_composed_force_np[env_ids_np[:, None], body_ids_np[None, :], :] += forces_np + # Get composed force from wrench composer + composed_force_np = wrench_composer.composed_force.numpy() + assert np.allclose(composed_force_np, hand_calculated_composed_force_np, atol=1, rtol=1e-7) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) +def test_wrench_composer_add_torque(device: str, num_envs: int, num_bodies: int): + # Initialize random number generator + rng = np.random.default_rng(seed=1) + + for _ in range(10): + mock_asset = MockRigidObject(num_envs, num_bodies, device) + wrench_composer = WrenchComposer(mock_asset) + # Initialize hand-calculated composed torque + hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + for _ in range(10): + # Get random number of envs and bodies and their indices + num_envs_np = rng.integers(1, num_envs, endpoint=True) + num_bodies_np = rng.integers(1, num_bodies, endpoint=True) + env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) + body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) + # Convert to warp arrays + env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) + body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) + # Get random torques + torques_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) + # Add torques to wrench composer + wrench_composer.add_forces_and_torques(torques=torques, body_ids=body_ids, env_ids=env_ids) + # Add torques to hand-calculated composed torque + hand_calculated_composed_torque_np[env_ids_np[:, None], body_ids_np[None, :], :] += torques_np + # Get composed torque from wrench composer + composed_torque_np = wrench_composer.composed_torque.numpy() + assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) +def test_add_forces_at_positons(device: str, num_envs: int, num_bodies: int): + """Test adding forces at local positions (offset from link frame).""" + rng = np.random.default_rng(seed=2) + + for _ in range(10): + # Initialize wrench composer + mock_asset = MockRigidObject(num_envs, num_bodies, device) + wrench_composer = WrenchComposer(mock_asset) + # Initialize hand-calculated composed force + hand_calculated_composed_force_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + # Initialize hand-calculated composed torque + hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + for _ in range(10): + # Get random number of envs and bodies and their indices + num_envs_np = rng.integers(1, num_envs, endpoint=True) + num_bodies_np = rng.integers(1, num_bodies, endpoint=True) + env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) + body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) + # Convert to warp arrays + env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) + body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) + # Get random forces + forces_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + positions_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) + positions = wp.from_numpy(positions_np, dtype=wp.vec3f, device=device) + # Add forces at positions to wrench composer + wrench_composer.add_forces_and_torques( + forces=forces, positions=positions, body_ids=body_ids, env_ids=env_ids + ) + # Add forces to hand-calculated composed force + hand_calculated_composed_force_np[env_ids_np[:, None], body_ids_np[None, :], :] += forces_np + # Add torques to hand-calculated composed torque: torque = cross(position, force) + torques_from_forces = np.cross(positions_np, forces_np) + for i in range(num_envs_np): + for j in range(num_bodies_np): + hand_calculated_composed_torque_np[env_ids_np[i], body_ids_np[j], :] += torques_from_forces[i, j, :] + + # Get composed force from wrench composer + composed_force_np = wrench_composer.composed_force.numpy() + assert np.allclose(composed_force_np, hand_calculated_composed_force_np, atol=1, rtol=1e-7) + # Get composed torque from wrench composer + composed_torque_np = wrench_composer.composed_torque.numpy() + assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) +def test_add_torques_at_position(device: str, num_envs: int, num_bodies: int): + rng = np.random.default_rng(seed=3) + + for _ in range(10): + mock_asset = MockRigidObject(num_envs, num_bodies, device) + wrench_composer = WrenchComposer(mock_asset) + # Initialize hand-calculated composed torque + hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + for _ in range(10): + # Get random number of envs and bodies and their indices + num_envs_np = rng.integers(1, num_envs, endpoint=True) + num_bodies_np = rng.integers(1, num_bodies, endpoint=True) + env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) + body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) + # Convert to warp arrays + env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) + body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) + # Get random torques + torques_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + positions_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) + positions = wp.from_numpy(positions_np, dtype=wp.vec3f, device=device) + # Add torques at positions to wrench composer + wrench_composer.add_forces_and_torques( + torques=torques, positions=positions, body_ids=body_ids, env_ids=env_ids + ) + # Add torques to hand-calculated composed torque + hand_calculated_composed_torque_np[env_ids_np[:, None], body_ids_np[None, :], :] += torques_np + # Get composed torque from wrench composer + composed_torque_np = wrench_composer.composed_torque.numpy() + assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) +def test_add_forces_and_torques_at_position(device: str, num_envs: int, num_bodies: int): + """Test adding forces and torques at local positions.""" + rng = np.random.default_rng(seed=4) + + for _ in range(10): + mock_asset = MockRigidObject(num_envs, num_bodies, device) + wrench_composer = WrenchComposer(mock_asset) + # Initialize hand-calculated composed force and torque + hand_calculated_composed_force_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + for _ in range(10): + # Get random number of envs and bodies and their indices + num_envs_np = rng.integers(1, num_envs, endpoint=True) + num_bodies_np = rng.integers(1, num_bodies, endpoint=True) + env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) + body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) + # Convert to warp arrays + env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) + body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) + # Get random forces and torques + forces_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + torques_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + positions_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) + torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) + positions = wp.from_numpy(positions_np, dtype=wp.vec3f, device=device) + # Add forces and torques at positions to wrench composer + wrench_composer.add_forces_and_torques( + forces=forces, torques=torques, positions=positions, body_ids=body_ids, env_ids=env_ids + ) + # Add forces to hand-calculated composed force + hand_calculated_composed_force_np[env_ids_np[:, None], body_ids_np[None, :], :] += forces_np + # Add torques to hand-calculated composed torque: torque = cross(position, force) + torque + torques_from_forces = np.cross(positions_np, forces_np) + for i in range(num_envs_np): + for j in range(num_bodies_np): + hand_calculated_composed_torque_np[env_ids_np[i], body_ids_np[j], :] += torques_from_forces[i, j, :] + hand_calculated_composed_torque_np[env_ids_np[:, None], body_ids_np[None, :], :] += torques_np + # Get composed force from wrench composer + composed_force_np = wrench_composer.composed_force.numpy() + assert np.allclose(composed_force_np, hand_calculated_composed_force_np, atol=1, rtol=1e-7) + # Get composed torque from wrench composer + composed_torque_np = wrench_composer.composed_torque.numpy() + assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) +def test_wrench_composer_reset(device: str, num_envs: int, num_bodies: int): + rng = np.random.default_rng(seed=5) + for _ in range(10): + mock_asset = MockRigidObject(num_envs, num_bodies, device) + wrench_composer = WrenchComposer(mock_asset) + # Get random number of envs and bodies and their indices + num_envs_np = rng.integers(1, num_envs, endpoint=True) + num_bodies_np = rng.integers(1, num_bodies, endpoint=True) + env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) + body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) + # Convert to warp arrays + env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) + body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) + # Get random forces and torques + forces_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + torques_np = ( + np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) + .reshape(num_envs_np, num_bodies_np, 3) + .astype(np.float32) + ) + forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) + torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) + # Add forces and torques to wrench composer + wrench_composer.add_forces_and_torques(forces=forces, torques=torques, body_ids=body_ids, env_ids=env_ids) + # Reset wrench composer + wrench_composer.reset() + # Get composed force and torque from wrench composer + composed_force_np = wrench_composer.composed_force.numpy() + composed_torque_np = wrench_composer.composed_torque.numpy() + assert np.allclose(composed_force_np, np.zeros((num_envs, num_bodies, 3)), atol=1, rtol=1e-7) + assert np.allclose(composed_torque_np, np.zeros((num_envs, num_bodies, 3)), atol=1, rtol=1e-7) + + +# ============================================================================ +# Global Frame Tests +# ============================================================================ + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5]) +def test_global_forces_with_rotation(device: str, num_envs: int, num_bodies: int): + """Test that global forces are correctly rotated to the local frame.""" + rng = np.random.default_rng(seed=10) + + for _ in range(5): + # Create random link quaternions + link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) + link_quat_torch = torch.from_numpy(link_quat_np) + + # Create mock asset with custom quaternions + mock_asset = MockRigidObject(num_envs, num_bodies, device, link_quat=link_quat_torch) + wrench_composer = WrenchComposer(mock_asset) + + # Generate random global forces for all envs and bodies + forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) + + # Apply global forces + wrench_composer.add_forces_and_torques(forces=forces_global, is_global=True) + + # Compute expected local forces by rotating global forces by inverse quaternion + expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np) + + # Verify + composed_force_np = wrench_composer.composed_force.numpy() + assert np.allclose(composed_force_np, expected_forces_local, atol=1e-4, rtol=1e-5), ( + f"Global force rotation failed.\nExpected:\n{expected_forces_local}\nGot:\n{composed_force_np}" + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 100]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5]) +def test_global_torques_with_rotation(device: str, num_envs: int, num_bodies: int): + """Test that global torques are correctly rotated to the local frame.""" + rng = np.random.default_rng(seed=11) + + for _ in range(5): + # Create random link quaternions + link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) + link_quat_torch = torch.from_numpy(link_quat_np) + + # Create mock asset with custom quaternions + mock_asset = MockRigidObject(num_envs, num_bodies, device, link_quat=link_quat_torch) + wrench_composer = WrenchComposer(mock_asset) + + # Generate random global torques + torques_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + torques_global = wp.from_numpy(torques_global_np, dtype=wp.vec3f, device=device) + + # Apply global torques + wrench_composer.add_forces_and_torques(torques=torques_global, is_global=True) + + # Compute expected local torques + expected_torques_local = quat_rotate_inv_np(link_quat_np, torques_global_np) + + # Verify + composed_torque_np = wrench_composer.composed_torque.numpy() + assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-4, rtol=1e-5), ( + f"Global torque rotation failed.\nExpected:\n{expected_torques_local}\nGot:\n{composed_torque_np}" + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 50]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5]) +def test_global_forces_at_global_position(device: str, num_envs: int, num_bodies: int): + """Test global forces at global positions with full coordinate transformation.""" + rng = np.random.default_rng(seed=12) + + for _ in range(5): + # Create random link poses + link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) + link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) + link_pos_torch = torch.from_numpy(link_pos_np) + link_quat_torch = torch.from_numpy(link_quat_np) + + # Create mock asset + mock_asset = MockRigidObject(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) + wrench_composer = WrenchComposer(mock_asset) + + # Generate random global forces and positions + forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + positions_global_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) + forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) + positions_global = wp.from_numpy(positions_global_np, dtype=wp.vec3f, device=device) + + # Apply global forces at global positions + wrench_composer.add_forces_and_torques(forces=forces_global, positions=positions_global, is_global=True) + + # Compute expected results: + # 1. Force in local frame = quat_rotate_inv(link_quat, global_force) + expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np) + + # 2. Position offset in local frame = global_position - link_position (then used for torque) + position_offset_global = positions_global_np - link_pos_np + + # 3. Torque = skew(position_offset_global) @ force_global, then rotate to local + expected_torques_local = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + for i in range(num_envs): + for j in range(num_bodies): + pos_offset = position_offset_global[i, j] # global frame offset + force_local = expected_forces_local[i, j] # local frame force + # skew(pos_offset) @ force_local + expected_torques_local[i, j] = np.cross(pos_offset, force_local) + + # Verify forces + composed_force_np = wrench_composer.composed_force.numpy() + assert np.allclose(composed_force_np, expected_forces_local, atol=1e-3, rtol=1e-4), ( + f"Global force at position failed.\nExpected forces:\n{expected_forces_local}\nGot:\n{composed_force_np}" + ) + + # Verify torques + composed_torque_np = wrench_composer.composed_torque.numpy() + assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-3, rtol=1e-4), ( + f"Global force at position failed.\nExpected torques:\n{expected_torques_local}\nGot:\n{composed_torque_np}" + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_local_vs_global_identity_quaternion(device: str): + """Test that local and global give same result with identity quaternion and zero position.""" + rng = np.random.default_rng(seed=13) + num_envs, num_bodies = 10, 5 + + # Create mock with identity pose (default) + mock_asset_local = MockRigidObject(num_envs, num_bodies, device) + mock_asset_global = MockRigidObject(num_envs, num_bodies, device) + + wrench_composer_local = WrenchComposer(mock_asset_local) + wrench_composer_global = WrenchComposer(mock_asset_global) + + # Generate random forces and torques + forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + torques_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) + torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) + + # Apply as local + wrench_composer_local.add_forces_and_torques(forces=forces, torques=torques, is_global=False) + + # Apply as global (should be same with identity quaternion) + wrench_composer_global.add_forces_and_torques(forces=forces, torques=torques, is_global=True) + + # Results should be identical + assert np.allclose( + wrench_composer_local.composed_force.numpy(), + wrench_composer_global.composed_force.numpy(), + atol=1e-6, + ) + assert np.allclose( + wrench_composer_local.composed_torque.numpy(), + wrench_composer_global.composed_torque.numpy(), + atol=1e-6, + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_90_degree_rotation_global_force(device: str): + """Test global force with a known 90-degree rotation for easy verification.""" + num_envs, num_bodies = 1, 1 + + # 90-degree rotation around Z-axis: (w, x, y, z) = (cos(45ยฐ), 0, 0, sin(45ยฐ)) + # This rotates X -> Y, Y -> -X + angle = np.pi / 2 + link_quat_np = np.array([[[[np.cos(angle / 2), 0, 0, np.sin(angle / 2)]]]], dtype=np.float32).reshape(1, 1, 4) + link_quat_torch = torch.from_numpy(link_quat_np) + + mock_asset = MockRigidObject(num_envs, num_bodies, device, link_quat=link_quat_torch) + wrench_composer = WrenchComposer(mock_asset) + + # Apply force in global +X direction + force_global = np.array([[[1.0, 0.0, 0.0]]], dtype=np.float32) + force_wp = wp.from_numpy(force_global, dtype=wp.vec3f, device=device) + + wrench_composer.add_forces_and_torques(forces=force_wp, is_global=True) + + # Expected: After inverse rotation (rotate by -90ยฐ around Z), X becomes -Y + # Actually, inverse rotation of +90ยฐ around Z applied to (1,0,0) gives (0,-1,0) + expected_force_local = np.array([[[0.0, -1.0, 0.0]]], dtype=np.float32) + + composed_force_np = wrench_composer.composed_force.numpy() + assert np.allclose(composed_force_np, expected_force_local, atol=1e-5), ( + f"90-degree rotation test failed.\nExpected:\n{expected_force_local}\nGot:\n{composed_force_np}" + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_composition_mixed_local_and_global(device: str): + """Test that local and global forces can be composed together correctly.""" + rng = np.random.default_rng(seed=14) + num_envs, num_bodies = 5, 3 + + # Create random link quaternions + link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) + link_quat_torch = torch.from_numpy(link_quat_np) + + mock_asset = MockRigidObject(num_envs, num_bodies, device, link_quat=link_quat_torch) + wrench_composer = WrenchComposer(mock_asset) + + # Generate random local and global forces + forces_local_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + + forces_local = wp.from_numpy(forces_local_np, dtype=wp.vec3f, device=device) + forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) + + # Add local forces first + wrench_composer.add_forces_and_torques(forces=forces_local, is_global=False) + + # Add global forces + wrench_composer.add_forces_and_torques(forces=forces_global, is_global=True) + + # Expected: local forces stay as-is, global forces get rotated, then sum + global_forces_in_local = quat_rotate_inv_np(link_quat_np, forces_global_np) + expected_total = forces_local_np + global_forces_in_local + + composed_force_np = wrench_composer.composed_force.numpy() + assert np.allclose(composed_force_np, expected_total, atol=1e-4, rtol=1e-5), ( + f"Mixed local/global composition failed.\nExpected:\n{expected_total}\nGot:\n{composed_force_np}" + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("num_envs", [1, 10, 50]) +@pytest.mark.parametrize("num_bodies", [1, 3, 5]) +def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies: int): + """Test local forces at local positions (offset from link frame).""" + rng = np.random.default_rng(seed=15) + + for _ in range(5): + # Create random link poses (shouldn't affect local frame calculations) + link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) + link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) + link_pos_torch = torch.from_numpy(link_pos_np) + link_quat_torch = torch.from_numpy(link_quat_np) + + mock_asset = MockRigidObject(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) + wrench_composer = WrenchComposer(mock_asset) + + # Generate random local forces and local positions (offsets) + forces_local_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + positions_local_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) + forces_local = wp.from_numpy(forces_local_np, dtype=wp.vec3f, device=device) + positions_local = wp.from_numpy(positions_local_np, dtype=wp.vec3f, device=device) + + # Apply local forces at local positions + wrench_composer.add_forces_and_torques(forces=forces_local, positions=positions_local, is_global=False) + + # Expected: forces stay as-is, torque = cross(position, force) + expected_forces = forces_local_np + expected_torques = np.cross(positions_local_np, forces_local_np) + + # Verify + composed_force_np = wrench_composer.composed_force.numpy() + composed_torque_np = wrench_composer.composed_torque.numpy() + + assert np.allclose(composed_force_np, expected_forces, atol=1e-4, rtol=1e-5) + assert np.allclose(composed_torque_np, expected_torques, atol=1e-4, rtol=1e-5) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_global_force_at_link_origin_no_torque(device: str): + """Test that a global force applied at the link origin produces no torque.""" + rng = np.random.default_rng(seed=16) + num_envs, num_bodies = 5, 3 + + # Create random link poses + link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) + link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) + link_pos_torch = torch.from_numpy(link_pos_np) + link_quat_torch = torch.from_numpy(link_quat_np) + + mock_asset = MockRigidObject(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) + wrench_composer = WrenchComposer(mock_asset) + + # Generate random global forces + forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) + + # Position = link position (so offset is zero) + positions_at_link = wp.from_numpy(link_pos_np, dtype=wp.vec3f, device=device) + + # Apply global forces at link origin + wrench_composer.add_forces_and_torques(forces=forces_global, positions=positions_at_link, is_global=True) + + # Expected: force rotated to local, torque = 0 (since position offset is zero) + expected_forces = quat_rotate_inv_np(link_quat_np, forces_global_np) + expected_torques = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) + + composed_force_np = wrench_composer.composed_force.numpy() + composed_torque_np = wrench_composer.composed_torque.numpy() + + assert np.allclose(composed_force_np, expected_forces, atol=1e-4, rtol=1e-5) + assert np.allclose(composed_torque_np, expected_torques, atol=1e-4, rtol=1e-5) diff --git a/source/isaaclab/test/visualization/check_scene_xr_visualization.py b/source/isaaclab/test/visualization/check_scene_xr_visualization.py new file mode 100644 index 000000000000..b03fa9e88bd2 --- /dev/null +++ b/source/isaaclab/test/visualization/check_scene_xr_visualization.py @@ -0,0 +1,262 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script checks if the XR visualization widgets are visible from the camera. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p source/isaaclab/test/visualization/check_scene_visualization.py + +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Check XR visualization widgets in Isaac Lab.") +parser.add_argument("--num_envs", type=int, default=2, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +# launch omniverse app with XR support +args_cli.xr = True +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import time +from typing import Any + +from pxr import Gf + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.ui.xr_widgets import DataCollector, TriggerType, VisualizationManager, XRVisualization, update_instruction +from isaaclab.utils import configclass + +## +# Pre-defined configs +## + + +@configclass +class SimpleSceneCfg(InteractiveSceneCfg): + """Design the scene with sensors on the robot.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + +def get_camera_position(): + """Get the current camera position from the USD stage. + + Returns: + tuple: (x, y, z) camera position or None if not available + """ + try: + from pxr import UsdGeom + + stage = sim_utils.get_current_stage() + if stage is not None: + # Get the viewport camera prim + camera_prim_path = "/OmniverseKit_Persp" + camera_prim = stage.GetPrimAtPath(camera_prim_path) + + if camera_prim and camera_prim.IsValid(): + # Get the camera's world transform + camera_xform = UsdGeom.Xformable(camera_prim) + world_transform = camera_xform.ComputeLocalToWorldTransform(0) # 0 = current time + + # Extract position from the transform matrix + camera_pos = world_transform.ExtractTranslation() + return (camera_pos[0], camera_pos[1], camera_pos[2]) + return None + except Exception as e: + print(f"[ERROR]: Failed to get camera position: {e}") + return None + + +def _sample_handle_ik_error(mgr: VisualizationManager, data_collector: DataCollector, params: Any = None) -> None: + error_text_color = getattr(mgr, "_error_text_color", 0xFF0000FF) + mgr.display_widget( + "IK Error Detected", + "/ik_error", + VisualizationManager.message_widget_preset() + | { + "text_color": error_text_color, + "prim_path_source": "/World/defaultGroundPlane/GroundPlane", + "translation": Gf.Vec3f(0, 0, 1), + }, + ) + + +def _sample_update_error_text_color(mgr: VisualizationManager, data_collector: DataCollector) -> None: + current_color = getattr(mgr, "_error_text_color", 0xFF0000FF) + new_color = current_color + 0x100 + if new_color >= 0xFFFFFFFF: + new_color = 0xFF0000FF + mgr.set_attr("_error_text_color", new_color) + + +def _sample_update_left_panel(mgr: VisualizationManager, data_collector: DataCollector) -> None: + left_panel_id = getattr(mgr, "left_panel_id", None) + + if left_panel_id is None: + return + + left_panel_created = getattr(mgr, "_left_panel_created", False) + if left_panel_created is False: + # create a new left panel + mgr.display_widget( + "Left Panel", + left_panel_id, + VisualizationManager.panel_widget_preset() + | { + "text_color": 0xFFFFFFFF, + "prim_path_source": "/World/defaultGroundPlane/GroundPlane", + "translation": Gf.Vec3f(0, -3, 1), + }, + ) + mgr.set_attr("_left_panel_created", True) + + updated_times = getattr(mgr, "_left_panel_updated_times", 0) + # Create a simple panel content since make_panel_content doesn't exist + content = f"Left Panel\nUpdated #{updated_times} times" + update_instruction(left_panel_id, content) + mgr.set_attr("_left_panel_updated_times", updated_times + 1) + + +def _sample_update_right_panel(mgr: VisualizationManager, data_collector: DataCollector) -> None: + right_panel_id = getattr(mgr, "right_panel_id", None) + + if right_panel_id is None: + return + + updated_times = getattr(mgr, "_right_panel_updated_times", 0) + # Create a simple panel content since make_panel_content doesn't exist + right_panel_data = data_collector.get_data("right_panel_data") + if right_panel_data is not None: + assert isinstance(right_panel_data, (tuple, list)), "Right panel data must be a tuple or list" + # Format each element to 3 decimal places + formatted_data = tuple(f"{x:.3f}" for x in right_panel_data) + content = f"Right Panel\nUpdated #{updated_times} times\nData: {formatted_data}" + else: + content = f"Right Panel\nUpdated #{updated_times} times\nData: None" + + right_panel_created = getattr(mgr, "_right_panel_created", False) + if right_panel_created is False: + # create a new left panel + mgr.display_widget( + content, + right_panel_id, + VisualizationManager.panel_widget_preset() + | { + "text_color": 0xFFFFFFFF, + "prim_path_source": "/World/defaultGroundPlane/GroundPlane", + "translation": Gf.Vec3f(0, 3, 1), + }, + ) + mgr.set_attr("_right_panel_created", True) + + update_instruction(right_panel_id, content) + mgr.set_attr("_right_panel_updated_times", updated_times + 1) + + +def apply_sample_visualization(): + # Error Message + XRVisualization.register_callback(TriggerType.TRIGGER_ON_EVENT, {"event_name": "ik_error"}, _sample_handle_ik_error) + + # Display a panel on the left to display DataCollector data + # Refresh periodically + XRVisualization.set_attrs( + { + "left_panel_id": "/left_panel", + "left_panel_translation": Gf.Vec3f(-2, 2.6, 2), + "left_panel_updated_times": 0, + "right_panel_updated_times": 0, + } + ) + XRVisualization.register_callback(TriggerType.TRIGGER_ON_PERIOD, {"period": 1.0}, _sample_update_left_panel) + + # Display a panel on the right to display DataCollector data + # Refresh when camera position changes + XRVisualization.set_attrs( + { + "right_panel_id": "/right_panel", + "right_panel_translation": Gf.Vec3f(1.5, 2, 2), + } + ) + XRVisualization.register_callback( + TriggerType.TRIGGER_ON_CHANGE, {"variable_name": "right_panel_data"}, _sample_update_right_panel + ) + + # Change error text color every second + XRVisualization.set_attrs( + { + "error_text_color": 0xFF0000FF, + } + ) + XRVisualization.register_callback(TriggerType.TRIGGER_ON_UPDATE, {}, _sample_update_error_text_color) + + +def run_simulator( + sim: sim_utils.SimulationContext, + scene: InteractiveScene, +): + """Run the simulator.""" + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + + apply_sample_visualization() + + # Simulate + while simulation_app.is_running(): + if int(time.time()) % 10 < 1: + XRVisualization.push_event("ik_error") + + XRVisualization.push_data({"right_panel_data": get_camera_position()}) + + sim.step() + scene.update(sim_dt) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.005) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view(eye=(8, 0, 4), target=(0.0, 0.0, 0.0)) + # design scene + scene = InteractiveScene(SimpleSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/source/isaaclab_assets/config/extension.toml b/source/isaaclab_assets/config/extension.toml index bafe43859eaa..d45724d97347 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.1" +version = "0.2.4" # Description title = "Isaac Lab Assets" @@ -12,6 +12,7 @@ keywords = ["kit", "robotics", "assets", "isaaclab"] [dependencies] "isaaclab" = {} +"isaaclab_contrib" = {} [core] reloadable = false diff --git a/source/isaaclab_assets/docs/CHANGELOG.rst b/source/isaaclab_assets/docs/CHANGELOG.rst index 27bdb7fbecef..3456213b3e8e 100644 --- a/source/isaaclab_assets/docs/CHANGELOG.rst +++ b/source/isaaclab_assets/docs/CHANGELOG.rst @@ -1,6 +1,30 @@ Changelog --------- +0.2.4 (2025-11-26) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Configuration for OpenArm robots used for manipulation tasks. + +0.2.3 (2025-08-11) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Configuration for G1 robot used for locomanipulation tasks. + +0.2.2 (2025-03-10) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added configuration for the Fourier GR1T2 robot. + 0.2.1 (2025-01-14) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_assets/isaaclab_assets/__init__.py b/source/isaaclab_assets/isaaclab_assets/__init__.py index 5d60703ff5bb..d83e15466fc3 100644 --- a/source/isaaclab_assets/isaaclab_assets/__init__.py +++ b/source/isaaclab_assets/isaaclab_assets/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py b/source/isaaclab_assets/isaaclab_assets/robots/__init__.py index 9c61cb14736a..77bcf04d0a3e 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,15 +7,22 @@ # Configuration for different assets. ## +from .agibot import * +from .agility import * from .allegro import * from .ant import * from .anymal import * from .cart_double_pendulum import * from .cartpole import * +from .cassie import * +from .fourier import * from .franka import * +from .galbot import * 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 * from .sawyer 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 000000000000..c5483721d2e0 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/agibot.py @@ -0,0 +1,162 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the 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/agility.py b/source/isaaclab_assets/isaaclab_assets/robots/agility.py new file mode 100644 index 000000000000..2c85a42ec681 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/agility.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +LEG_JOINT_NAMES = [ + ".*_hip_roll", + ".*_hip_yaw", + ".*_hip_pitch", + ".*_knee", + ".*_toe_a", + ".*_toe_b", +] + +ARM_JOINT_NAMES = [".*_arm_.*"] + + +DIGIT_V4_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Agility/Digit/digit_v4.usd", + activate_contact_sensors=True, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.05), + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "all": ImplicitActuatorCfg( + joint_names_expr=".*", + stiffness=None, + damping=None, + ), + }, +) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/allegro.py b/source/isaaclab_assets/isaaclab_assets/robots/allegro.py index 85d3b265400c..0e18ef77c13c 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/allegro.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/allegro.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -15,11 +15,10 @@ """ - import math import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -29,7 +28,7 @@ ALLEGRO_HAND_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/AllegroHand/allegro_hand_instanceable.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/WonikRobotics/AllegroHand/allegro_hand_instanceable.usd", activate_contact_sensors=False, rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=True, @@ -58,8 +57,7 @@ actuators={ "fingers": ImplicitActuatorCfg( joint_names_expr=[".*"], - effort_limit=0.5, - velocity_limit=100.0, + effort_limit_sim=0.5, stiffness=3.0, damping=0.1, friction=0.01, diff --git a/source/isaaclab_assets/isaaclab_assets/robots/ant.py b/source/isaaclab_assets/isaaclab_assets/robots/ant.py index 65f0cf0760a9..49798ad638dd 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/ant.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/ant.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -19,7 +19,7 @@ ANT_CFG = ArticulationCfg( prim_path="{ENV_REGEX_NS}/Robot", spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Ant/ant_instanceable.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/Ant/ant_instanceable.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=10.0, diff --git a/source/isaaclab_assets/isaaclab_assets/robots/anymal.py b/source/isaaclab_assets/isaaclab_assets/robots/anymal.py index 35ace93184df..ac0e565513f4 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/anymal.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/anymal.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -19,14 +19,14 @@ """ -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 +from isaaclab_assets.sensors.velodyne import VELODYNE_VLP_16_RAYCASTER_CFG + ## # Configuration - Actuators. ## diff --git a/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py b/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py new file mode 100644 index 000000000000..6ac4b9fc55e8 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py @@ -0,0 +1,75 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the ARL robots. + +The following configuration parameters are available: + +* :obj:`ARL_ROBOT_1_CFG`: The ARL_Robot_1 with (TODO add motor propeller combination) +""" + +import isaaclab.sim as sim_utils +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from isaaclab_contrib.actuators import ThrusterCfg +from isaaclab_contrib.assets import MultirotorCfg + +## +# Configuration - Actuators. +## + +ARL_ROBOT_1_THRUSTER = ThrusterCfg( + thrust_range=(0.1, 10.0), + thrust_const_range=(9.26312e-06, 1.826312e-05), + tau_inc_range=(0.05, 0.08), + tau_dec_range=(0.005, 0.005), + torque_to_thrust_ratio=0.07, + thruster_names_expr=["back_left_prop", "back_right_prop", "front_left_prop", "front_right_prop"], +) + +## +# Configuration - Articulation. +## + +ARL_ROBOT_1_CFG = MultirotorCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/NTNU/ARL-Robot-1/arl_robot_1.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=MultirotorCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.0), + lin_vel=(0.0, 0.0, 0.0), + ang_vel=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + rps={ + "back_left_prop": 200.0, + "back_right_prop": 200.0, + "front_left_prop": 200.0, + "front_right_prop": 200.0, + }, + ), + actuators={"thrusters": ARL_ROBOT_1_THRUSTER}, + rotor_directions=[1, -1, 1, -1], + allocation_matrix=[ + [0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0], + [1.0, 1.0, 1.0, 1.0], + [-0.13, -0.13, 0.13, 0.13], + [-0.13, 0.13, 0.13, -0.13], + [-0.07, 0.07, -0.07, 0.07], + ], +) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/cart_double_pendulum.py b/source/isaaclab_assets/isaaclab_assets/robots/cart_double_pendulum.py index ab77bd7d74f7..22028f39baf2 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/cart_double_pendulum.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/cart_double_pendulum.py @@ -1,11 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Configuration for 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 @@ -39,16 +38,15 @@ actuators={ "cart_actuator": ImplicitActuatorCfg( joint_names_expr=["slider_to_cart"], - effort_limit=400.0, - velocity_limit=100.0, + effort_limit_sim=400.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 + joint_names_expr=["cart_to_pole"], effort_limit_sim=400.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 + joint_names_expr=["pole_to_pendulum"], effort_limit_sim=400.0, stiffness=0.0, damping=0.0 ), }, ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/cartpole.py b/source/isaaclab_assets/isaaclab_assets/robots/cartpole.py index 3a28abce21ca..1e236eda6b93 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/cartpole.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/cartpole.py @@ -1,11 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Configuration for a simple Cartpole robot.""" - import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg @@ -39,13 +38,12 @@ actuators={ "cart_actuator": ImplicitActuatorCfg( joint_names_expr=["slider_to_cart"], - effort_limit=400.0, - velocity_limit=100.0, + effort_limit_sim=400.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 + joint_names_expr=["cart_to_pole"], effort_limit_sim=400.0, stiffness=0.0, damping=0.0 ), }, ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/cassie.py b/source/isaaclab_assets/isaaclab_assets/robots/cassie.py index dbd9a8b97843..09e75e241fed 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/cassie.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/cassie.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -60,8 +60,7 @@ actuators={ "legs": ImplicitActuatorCfg( joint_names_expr=["hip_.*", "thigh_.*", "ankle_.*"], - effort_limit=200.0, - velocity_limit=10.0, + effort_limit_sim=200.0, stiffness={ "hip_abduction.*": 100.0, "hip_rotation.*": 100.0, @@ -79,8 +78,7 @@ ), "toes": ImplicitActuatorCfg( joint_names_expr=["toe_.*"], - effort_limit=20.0, - velocity_limit=10.0, + effort_limit_sim=20.0, stiffness={ "toe_joint.*": 20.0, }, diff --git a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py new file mode 100644 index 000000000000..58e143d11885 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py @@ -0,0 +1,164 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Fourier Robots. + +The following configuration parameters are available: + +* :obj:`GR1T2_CFG`: The GR1T2 humanoid. +* :obj:`GR1T2_HIGH_PD_CFG`: The GR1T2 humanoid configured with high PD gains on upper + body joints for pick-place manipulation tasks. + +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.""" + + +GR1T2_HIGH_PD_CFG = GR1T2_CFG.replace( + actuators={ + "trunk": ImplicitActuatorCfg( + joint_names_expr=["waist_.*"], + effort_limit=None, + velocity_limit=None, + stiffness=4400, + damping=40.0, + armature=0.01, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=["right_shoulder_.*", "right_elbow_.*", "right_wrist_.*"], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=["left_shoulder_.*", "left_elbow_.*", "left_wrist_.*"], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=["R_.*"], + stiffness=None, + damping=None, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=["L_.*"], + stiffness=None, + damping=None, + ), + }, +) +"""Configuration for the GR1T2 Humanoid robot configured for with high PD gains for pick-place manipulation tasks.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/franka.py b/source/isaaclab_assets/isaaclab_assets/robots/franka.py index 368be2b25509..caacf214c58f 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/franka.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/franka.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,6 +9,7 @@ * :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 +* :obj:`FRANKA_ROBOTIQ_GRIPPER_CFG`: Franka robot with Robotiq_2f_85 gripper Reference: https://github.com/frankaemika/franka_ros """ @@ -16,7 +17,7 @@ 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 +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR ## # Configuration @@ -50,22 +51,19 @@ actuators={ "panda_shoulder": ImplicitActuatorCfg( joint_names_expr=["panda_joint[1-4]"], - effort_limit=87.0, - velocity_limit=2.175, + effort_limit_sim=87.0, stiffness=80.0, damping=4.0, ), "panda_forearm": ImplicitActuatorCfg( joint_names_expr=["panda_joint[5-7]"], - effort_limit=12.0, - velocity_limit=2.61, + effort_limit_sim=12.0, stiffness=80.0, damping=4.0, ), "panda_hand": ImplicitActuatorCfg( joint_names_expr=["panda_finger_joint.*"], - effort_limit=200.0, - velocity_limit=0.2, + effort_limit_sim=200.0, stiffness=2e3, damping=1e2, ), @@ -85,3 +83,65 @@ This configuration is useful for task-space control using differential IK. """ + + +FRANKA_ROBOTIQ_GRIPPER_CFG = FRANKA_PANDA_CFG.copy() +FRANKA_ROBOTIQ_GRIPPER_CFG.spawn.usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" +FRANKA_ROBOTIQ_GRIPPER_CFG.spawn.variants = {"Gripper": "Robotiq_2F_85"} +FRANKA_ROBOTIQ_GRIPPER_CFG.spawn.rigid_props.disable_gravity = True +FRANKA_ROBOTIQ_GRIPPER_CFG.init_state.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, + "finger_joint": 0.0, + ".*_inner_finger_joint": 0.0, + ".*_inner_finger_knuckle_joint": 0.0, + ".*_outer_.*_joint": 0.0, +} +FRANKA_ROBOTIQ_GRIPPER_CFG.init_state.pos = (-0.85, 0, 0.76) +FRANKA_ROBOTIQ_GRIPPER_CFG.actuators = { + "panda_shoulder": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + effort_limit_sim=5200.0, + velocity_limit_sim=2.175, + stiffness=1100.0, + damping=80.0, + ), + "panda_forearm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + effort_limit_sim=720.0, + velocity_limit_sim=2.61, + stiffness=1000.0, + damping=80.0, + ), + "gripper_drive": ImplicitActuatorCfg( + joint_names_expr=["finger_joint"], # "right_outer_knuckle_joint" is its mimic joint + effort_limit_sim=1650, + velocity_limit_sim=10.0, + stiffness=17, + damping=0.02, + ), + # enable the gripper to grasp in a parallel manner + "gripper_finger": ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_joint"], + effort_limit_sim=50, + velocity_limit_sim=10.0, + stiffness=0.2, + damping=0.001, + ), + # set PD to zero for passive joints in close-loop gripper + "gripper_passive": ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_knuckle_joint", "right_outer_knuckle_joint"], + effort_limit_sim=1.0, + velocity_limit_sim=10.0, + stiffness=0.0, + damping=0.0, + ), +} + + +"""Configuration of Franka Emika Panda robot with Robotiq_2f_85 gripper.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/galbot.py b/source/isaaclab_assets/isaaclab_assets/robots/galbot.py new file mode 100644 index 000000000000..9827c7c8d31e --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/galbot.py @@ -0,0 +1,103 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Configuration for the Galbot humanoid robot. + +The following configuration parameters are available: + +* :obj:`GALBOT_ONE_CHARLIE_CFG`: The galbot_one_charlie humanoid 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 +## + + +GALBOT_ONE_CHARLIE_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Galbot/galbot_one_charlie/galbot_one_charlie.usd", + variants={"Physics": "PhysX"}, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + 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( + joint_pos={ + "leg_joint1": 0.8, + "leg_joint2": 2.3, + "leg_joint3": 1.55, + "leg_joint4": 0.0, + "head_joint1": 0.0, + "head_joint2": 0.36, + "left_arm_joint1": -0.5480, + "left_arm_joint2": -0.6551, + "left_arm_joint3": 2.407, + "left_arm_joint4": 1.3641, + "left_arm_joint5": -0.4416, + "left_arm_joint6": 0.1168, + "left_arm_joint7": 1.2308, + "left_gripper_left_joint": 0.035, + "left_gripper_right_joint": 0.035, + "right_arm_joint1": 0.1535, + "right_arm_joint2": 1.0087, + "right_arm_joint3": 0.0895, + "right_arm_joint4": 1.5743, + "right_arm_joint5": -0.2422, + "right_arm_joint6": -0.0009, + "right_arm_joint7": -0.9143, + "right_suction_cup_joint1": 0.0, + }, + pos=(-0.6, 0.0, -0.8), + ), + # PD parameters are read from USD file with Gain Tuner + actuators={ + "head": ImplicitActuatorCfg( + joint_names_expr=["head_joint.*"], + velocity_limit_sim=None, + effort_limit_sim=None, + stiffness=None, + damping=None, + ), + "leg": ImplicitActuatorCfg( + joint_names_expr=["leg_joint.*"], + velocity_limit_sim=None, + effort_limit_sim=None, + stiffness=None, + damping=None, + ), + "left_arm": ImplicitActuatorCfg( + joint_names_expr=["left_arm_joint.*"], + velocity_limit_sim=None, + effort_limit_sim=None, + stiffness=None, + damping=None, + ), + "right_arm": ImplicitActuatorCfg( + joint_names_expr=["right_arm_joint.*", "right_suction_cup_joint1"], + velocity_limit_sim=None, + effort_limit_sim=None, + stiffness=None, + damping=None, + ), + "left_gripper": ImplicitActuatorCfg( + joint_names_expr=["left_gripper_.*_joint"], + velocity_limit_sim=1.0, + effort_limit_sim=None, + stiffness=None, + damping=None, + ), + }, +) +"""Configuration of Galbot_one_charlie humanoid using implicit actuator models.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/humanoid.py b/source/isaaclab_assets/isaaclab_assets/robots/humanoid.py index 3e25d0693e61..42940b4fa1f4 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/humanoid.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/humanoid.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -19,7 +19,7 @@ HUMANOID_CFG = ArticulationCfg( prim_path="{ENV_REGEX_NS}/Robot", spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Humanoid/humanoid_instanceable.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/Humanoid/humanoid_instanceable.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=None, max_depenetration_velocity=10.0, @@ -63,6 +63,7 @@ ".*_shin": 0.1, ".*_foot.*": 1.0, }, + velocity_limit_sim={".*": 100.0}, ), }, ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/humanoid_28.py b/source/isaaclab_assets/isaaclab_assets/robots/humanoid_28.py index 304b4f3e89d0..84f44339a537 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/humanoid_28.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/humanoid_28.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -43,6 +43,7 @@ joint_names_expr=[".*"], stiffness=None, damping=None, + velocity_limit_sim={".*": 100.0}, ), }, ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/kinova.py b/source/isaaclab_assets/isaaclab_assets/robots/kinova.py index b0d2d5f5ad16..3bef3896232e 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/kinova.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/kinova.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -51,8 +51,7 @@ actuators={ "arm": ImplicitActuatorCfg( joint_names_expr=[".*_joint_[1-7]"], - velocity_limit=100.0, - effort_limit={ + effort_limit_sim={ ".*_joint_[1-2]": 80.0, ".*_joint_[3-4]": 40.0, ".*_joint_[5-7]": 20.0, @@ -68,8 +67,7 @@ ), "gripper": ImplicitActuatorCfg( joint_names_expr=[".*_finger_[1-3]", ".*_finger_tip_[1-3]"], - velocity_limit=100.0, - effort_limit=2.0, + effort_limit_sim=2.0, stiffness=1.2, damping=0.01, ), @@ -105,8 +103,7 @@ actuators={ "arm": ImplicitActuatorCfg( joint_names_expr=[".*_joint_[1-6]"], - velocity_limit=100.0, - effort_limit={ + effort_limit_sim={ ".*_joint_[1-2]": 80.0, ".*_joint_3": 40.0, ".*_joint_[4-6]": 20.0, @@ -122,8 +119,7 @@ ), "gripper": ImplicitActuatorCfg( joint_names_expr=[".*_finger_[1-3]", ".*_finger_tip_[1-3]"], - velocity_limit=100.0, - effort_limit=2.0, + effort_limit_sim=2.0, stiffness=1.2, damping=0.01, ), @@ -158,7 +154,6 @@ 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, 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 000000000000..35b7e0b179a0 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py @@ -0,0 +1,114 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the 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 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/openarm.py b/source/isaaclab_assets/isaaclab_assets/robots/openarm.py new file mode 100644 index 000000000000..02743c5da915 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/openarm.py @@ -0,0 +1,173 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration of OpenArm robots. + +The following configurations are available: + +* :obj:`OPENARM_BI_CFG`: OpenArm robot with two arms. +* :obj:`OPENARM_BI_HIGH_PD_CFG`: OpenArm robot with two arms and stiffer PD control. +* :obj:`OPENARM_UNI_CFG`: OpenArm robot with one arm. +* :obj:`OPENARM_UNI_HIGH_PD_CFG`: OpenArm robot with one arm and stiffer PD control. + +References: +OpenArm repositories: +* https://github.com/enactic/openarm +* https://github.com/enactic/openarm_isaac_lab + +Motor spec sheets: +* Joint 1โ€“2 (DM-J8009P-2EC): + https://cdn.shopify.com/s/files/1/0673/6848/5000/files/DM-J8009P-2EC_User_Manual.pdf?v=1755481750 +* Joint 3โ€“4 (DM-J4340P-2EC / DM-J4340-2EC): + https://cdn.shopify.com/s/files/1/0673/6848/5000/files/DM-J4340-2EC_User_Manual.pdf?v=1756883905 +* Joint 5โ€“8 (DM-J4310-2EC V1.1): + https://files.seeedstudio.com/products/Damiao/DM-J4310-en.pdf +""" + +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 + +OPENARM_BI_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/OpenArm/openarm_bimanual/openarm_bimanual.usd", + 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={ + "openarm_left_joint.*": 0.0, + "openarm_right_joint.*": 0.0, + "openarm_left_finger_joint.*": 0.0, + "openarm_right_finger_joint.*": 0.0, + }, + ), + # spec sheet for reference + # DM-J8009P-2EC (Joint 1, 2): + # https://cdn.shopify.com/s/files/1/0673/6848/5000/files/DM-J8009P-2EC_User_Manual.pdf?v=1755481750 + # DM-J4340P-2EC, DM-J4340-2EC (Joint 3, 4): + # https://cdn.shopify.com/s/files/1/0673/6848/5000/files/DM-J4340-2EC_User_Manual.pdf?v=1756883905 + # DM-J4310-2EC V1.1 (Joint 5, 6, 7, 8): + # https://files.seeedstudio.com/products/Damiao/DM-J4310-en.pdf + actuators={ + "openarm_arm": ImplicitActuatorCfg( + joint_names_expr=[ + "openarm_left_joint[1-7]", + "openarm_right_joint[1-7]", + ], + velocity_limit_sim={ + "openarm_left_joint[1-2]": 2.175, + "openarm_right_joint[1-2]": 2.175, + "openarm_left_joint[3-4]": 2.175, + "openarm_right_joint[3-4]": 2.175, + "openarm_left_joint[5-7]": 2.61, + "openarm_right_joint[5-7]": 2.61, + }, + effort_limit_sim={ + "openarm_left_joint[1-2]": 40.0, + "openarm_right_joint[1-2]": 40.0, + "openarm_left_joint[3-4]": 27.0, + "openarm_right_joint[3-4]": 27.0, + "openarm_left_joint[5-7]": 7.0, + "openarm_right_joint[5-7]": 7.0, + }, + stiffness=80.0, + damping=4.0, + ), + "openarm_gripper": ImplicitActuatorCfg( + joint_names_expr=[ + "openarm_left_finger_joint.*", + "openarm_right_finger_joint.*", + ], + velocity_limit_sim=0.2, + effort_limit_sim=333.33, + stiffness=2e3, + damping=1e2, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of OpenArm Bimanual robot.""" + +OPENARM_UNI_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/OpenArm/openarm_unimanual/openarm_unimanual.usd", + 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={ + "openarm_joint1": 1.57, + "openarm_joint2": 0.0, + "openarm_joint3": -1.57, + "openarm_joint4": 1.57, + "openarm_joint5": 0.0, + "openarm_joint6": 0.0, + "openarm_joint7": 0.0, + "openarm_finger_joint.*": 0.044, + }, + ), + actuators={ + "openarm_arm": ImplicitActuatorCfg( + joint_names_expr=["openarm_joint[1-7]"], + velocity_limit_sim={ + "openarm_joint[1-2]": 2.175, + "openarm_joint[3-4]": 2.175, + "openarm_joint[5-7]": 2.61, + }, + effort_limit_sim={ + "openarm_joint[1-2]": 40.0, + "openarm_joint[3-4]": 27.0, + "openarm_joint[5-7]": 7.0, + }, + stiffness=80.0, + damping=4.0, + ), + "openarm_gripper": ImplicitActuatorCfg( + joint_names_expr=["openarm_finger_joint.*"], + velocity_limit_sim=0.2, + effort_limit_sim=333.33, + stiffness=2e3, + damping=1e2, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of OpenArm Unimanual robot.""" + +OPENARM_BI_HIGH_PD_CFG = OPENARM_BI_CFG.copy() +OPENARM_BI_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True +OPENARM_BI_HIGH_PD_CFG.actuators["openarm_arm"].stiffness = 400.0 +OPENARM_BI_HIGH_PD_CFG.actuators["openarm_arm"].damping = 80.0 +OPENARM_BI_HIGH_PD_CFG.actuators["openarm_gripper"].stiffness = 2e3 +OPENARM_BI_HIGH_PD_CFG.actuators["openarm_gripper"].damping = 1e2 +"""Configuration of OpenArm Bimanual robot with stiffer PD control. + +This configuration is useful for task-space control using differential IK. +""" + +OPENARM_UNI_HIGH_PD_CFG = OPENARM_UNI_CFG.copy() +OPENARM_UNI_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True +OPENARM_UNI_HIGH_PD_CFG.actuators["openarm_arm"].stiffness = 400.0 +OPENARM_UNI_HIGH_PD_CFG.actuators["openarm_arm"].damping = 80.0 +"""Configuration of OpenArm Unimanual robot with stiffer PD control. + +This configuration is useful for task-space control using differential IK. +""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/pick_and_place.py b/source/isaaclab_assets/isaaclab_assets/robots/pick_and_place.py new file mode 100644 index 000000000000..988e042fcf65 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/pick_and_place.py @@ -0,0 +1,69 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for a simple pick and place robot with a suction cup.""" + +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 +## + +PICK_AND_PLACE_CFG = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/PickAndPlace/pick_and_place_robot.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.0), + joint_pos={ + "x_axis": 0.0, + "y_axis": 0.0, + "z_axis": 0.0, + }, + ), + actuators={ + "x_gantry": ImplicitActuatorCfg( + joint_names_expr=["x_axis"], + effort_limit=400.0, + velocity_limit=10.0, + stiffness=0.0, + damping=10.0, + ), + "y_gantry": ImplicitActuatorCfg( + joint_names_expr=["y_axis"], + effort_limit=400.0, + velocity_limit=10.0, + stiffness=0.0, + damping=10.0, + ), + "z_gantry": ImplicitActuatorCfg( + joint_names_expr=["z_axis"], + effort_limit=400.0, + velocity_limit=10.0, + stiffness=0.0, + damping=10.0, + ), + }, +) +"""Configuration for a simple pick and place robot with a suction cup.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/quadcopter.py b/source/isaaclab_assets/isaaclab_assets/robots/quadcopter.py index c27b052ad358..f404a90e3f14 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/quadcopter.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/quadcopter.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -19,7 +19,7 @@ CRAZYFLIE_CFG = ArticulationCfg( prim_path="{ENV_REGEX_NS}/Robot", spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Crazyflie/cf2x.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Bitcraze/Crazyflie/cf2x.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=10.0, diff --git a/source/isaaclab_assets/isaaclab_assets/robots/ridgeback_franka.py b/source/isaaclab_assets/isaaclab_assets/robots/ridgeback_franka.py index 0e1a4a8415bc..312236d23373 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/ridgeback_franka.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/ridgeback_franka.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -49,29 +49,25 @@ actuators={ "base": ImplicitActuatorCfg( joint_names_expr=["dummy_base_.*"], - velocity_limit=100.0, - effort_limit=1000.0, + effort_limit_sim=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, + effort_limit_sim=87.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, + effort_limit_sim=12.0, stiffness=800.0, damping=40.0, ), "panda_hand": ImplicitActuatorCfg( joint_names_expr=["panda_finger_joint.*"], - effort_limit=200.0, - velocity_limit=0.2, + effort_limit_sim=200.0, stiffness=1e5, damping=1e3, ), diff --git a/source/isaaclab_assets/isaaclab_assets/robots/sawyer.py b/source/isaaclab_assets/isaaclab_assets/robots/sawyer.py index 64cac2c37e68..179df09e7d81 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/sawyer.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/sawyer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -23,7 +23,7 @@ SAWYER_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/RethinkRobotics/sawyer_instanceable.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/RethinkRobotics/Sawyer/sawyer_instanceable.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=5.0, @@ -48,15 +48,13 @@ actuators={ "head": ImplicitActuatorCfg( joint_names_expr=["head_pan"], - velocity_limit=100.0, - effort_limit=8.0, + effort_limit_sim=8.0, stiffness=800.0, damping=40.0, ), "arm": ImplicitActuatorCfg( joint_names_expr=["right_j[0-6]"], - velocity_limit=100.0, - effort_limit={ + effort_limit_sim={ "right_j[0-1]": 80.0, "right_j[2-3]": 40.0, "right_j[4-6]": 9.0, diff --git a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py index 4521c5a0d324..d13e90e3b1c0 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -15,9 +15,8 @@ """ - import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -27,7 +26,7 @@ SHADOW_HAND_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ShadowHand/shadow_hand_instanceable.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ShadowRobot/ShadowHand/shadow_hand_instanceable.usd", activate_contact_sensors=False, rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=True, @@ -53,7 +52,7 @@ 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={ + effort_limit_sim={ "robot0_WRJ1": 4.785, "robot0_WRJ0": 2.175, "robot0_(FF|MF|RF|LF)J1": 0.7245, diff --git a/source/isaaclab_assets/isaaclab_assets/robots/spot.py b/source/isaaclab_assets/isaaclab_assets/robots/spot.py index dae0ce8285c0..3bc98b8b2da3 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/spot.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/spot.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py index 22253bf0d08f..ff7685a3c607 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -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. @@ -216,8 +218,7 @@ actuators={ "legs": ImplicitActuatorCfg( joint_names_expr=[".*_hip_yaw", ".*_hip_roll", ".*_hip_pitch", ".*_knee", "torso"], - effort_limit=300, - velocity_limit=100.0, + effort_limit_sim=300, stiffness={ ".*_hip_yaw": 150.0, ".*_hip_roll": 150.0, @@ -235,15 +236,13 @@ ), "feet": ImplicitActuatorCfg( joint_names_expr=[".*_ankle"], - effort_limit=100, - velocity_limit=100.0, + effort_limit_sim=100, 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, + effort_limit_sim=300, stiffness={ ".*_shoulder_pitch": 40.0, ".*_shoulder_roll": 40.0, @@ -315,8 +314,7 @@ ".*_knee_joint", "torso_joint", ], - effort_limit=300, - velocity_limit=100.0, + effort_limit_sim=300, stiffness={ ".*_hip_yaw_joint": 150.0, ".*_hip_roll_joint": 150.0, @@ -338,7 +336,7 @@ }, ), "feet": ImplicitActuatorCfg( - effort_limit=20, + effort_limit_sim=20, joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], stiffness=20.0, damping=2.0, @@ -359,8 +357,7 @@ ".*_one_joint", ".*_two_joint", ], - effort_limit=300, - velocity_limit=100.0, + effort_limit_sim=300, stiffness=40.0, damping=10.0, armature={ @@ -386,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/isaaclab_assets/robots/universal_robots.py b/source/isaaclab_assets/isaaclab_assets/robots/universal_robots.py index 9fc6620f1b38..1026e00a9713 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/universal_robots.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/universal_robots.py @@ -1,13 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause + """Configuration for the Universal Robots. The following configuration parameters are available: * :obj:`UR10_CFG`: The UR10 arm without a gripper. +* :obj:`UR10E_ROBOTIQ_GRIPPER_CFG`: The UR10E arm with Robotiq_2f_140 gripper. +* :obj:`UR10e_ROBOTIQ_2F_85_CFG`: The UR10E arm with Robotiq 2F-85 gripper. Reference: https://github.com/ros-industrial/universal_robot """ @@ -15,13 +18,12 @@ 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 +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR ## # Configuration ## - UR10_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/UniversalRobots/UR10/ur10_instanceable.usd", @@ -44,11 +46,162 @@ actuators={ "arm": ImplicitActuatorCfg( joint_names_expr=[".*"], - velocity_limit=100.0, - effort_limit=87.0, + effort_limit_sim=87.0, stiffness=800.0, damping=40.0, ), }, ) + +UR10e_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/UniversalRobots/ur10e/ur10e.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=16, solver_velocity_iteration_count=1 + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "shoulder_pan_joint": 3.141592653589793, + "shoulder_lift_joint": -1.5707963267948966, + "elbow_joint": 1.5707963267948966, + "wrist_1_joint": -1.5707963267948966, + "wrist_2_joint": -1.5707963267948966, + "wrist_3_joint": 0.0, + }, + pos=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + ), + actuators={ + # 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' + "shoulder": ImplicitActuatorCfg( + joint_names_expr=["shoulder_.*"], + stiffness=1320.0, + damping=72.6636085, + friction=0.0, + armature=0.0, + ), + "elbow": ImplicitActuatorCfg( + joint_names_expr=["elbow_joint"], + stiffness=600.0, + damping=34.64101615, + friction=0.0, + armature=0.0, + ), + "wrist": ImplicitActuatorCfg( + joint_names_expr=["wrist_.*"], + stiffness=216.0, + damping=29.39387691, + friction=0.0, + armature=0.0, + ), + }, +) + """Configuration of UR-10 arm using implicit actuator models.""" + +UR10_LONG_SUCTION_CFG = UR10_CFG.copy() +UR10_LONG_SUCTION_CFG.spawn.usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/UniversalRobots/ur10/ur10.usd" +UR10_LONG_SUCTION_CFG.spawn.variants = {"Gripper": "Long_Suction"} +UR10_LONG_SUCTION_CFG.spawn.rigid_props.disable_gravity = True +UR10_LONG_SUCTION_CFG.init_state.joint_pos = { + "shoulder_pan_joint": 0.0, + "shoulder_lift_joint": -1.5707, + "elbow_joint": 1.5707, + "wrist_1_joint": -1.5707, + "wrist_2_joint": 1.5707, + "wrist_3_joint": 0.0, +} + +"""Configuration of UR10 arm with long suction gripper.""" + +UR10_SHORT_SUCTION_CFG = UR10_LONG_SUCTION_CFG.copy() +UR10_SHORT_SUCTION_CFG.spawn.variants = {"Gripper": "Short_Suction"} + +"""Configuration of UR10 arm with short suction gripper.""" + +UR10e_ROBOTIQ_GRIPPER_CFG = UR10e_CFG.copy() +"""Configuration of UR10e arm with Robotiq_2f_140 gripper.""" +UR10e_ROBOTIQ_GRIPPER_CFG.spawn.variants = {"Gripper": "Robotiq_2f_140"} +UR10e_ROBOTIQ_GRIPPER_CFG.spawn.rigid_props.disable_gravity = True +UR10e_ROBOTIQ_GRIPPER_CFG.init_state.joint_pos["finger_joint"] = 0.0 +UR10e_ROBOTIQ_GRIPPER_CFG.init_state.joint_pos[".*_inner_finger_joint"] = 0.0 +UR10e_ROBOTIQ_GRIPPER_CFG.init_state.joint_pos[".*_inner_finger_pad_joint"] = 0.0 +UR10e_ROBOTIQ_GRIPPER_CFG.init_state.joint_pos[".*_outer_.*_joint"] = 0.0 +# the major actuator joint for gripper +UR10e_ROBOTIQ_GRIPPER_CFG.actuators["gripper_drive"] = ImplicitActuatorCfg( + joint_names_expr=["finger_joint"], + effort_limit_sim=10.0, + velocity_limit_sim=1.0, + stiffness=11.25, + damping=0.1, + friction=0.0, + armature=0.0, +) +# the auxiliary actuator joint for gripper +UR10e_ROBOTIQ_GRIPPER_CFG.actuators["gripper_finger"] = ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_joint"], + effort_limit_sim=1.0, + velocity_limit_sim=1.0, + stiffness=0.2, + damping=0.001, + friction=0.0, + armature=0.0, +) +# the passive joints for gripper +UR10e_ROBOTIQ_GRIPPER_CFG.actuators["gripper_passive"] = ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_pad_joint", ".*_outer_finger_joint", "right_outer_knuckle_joint"], + effort_limit_sim=1.0, + velocity_limit_sim=1.0, + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, +) + + +UR10e_ROBOTIQ_2F_85_CFG = UR10e_CFG.copy() +"""Configuration of UR-10E arm with Robotiq_2f_140 gripper.""" +UR10e_ROBOTIQ_2F_85_CFG.spawn.variants = {"Gripper": "Robotiq_2f_85"} +UR10e_ROBOTIQ_2F_85_CFG.spawn.rigid_props.disable_gravity = True +UR10e_ROBOTIQ_2F_85_CFG.init_state.joint_pos["finger_joint"] = 0.0 +UR10e_ROBOTIQ_2F_85_CFG.init_state.joint_pos[".*_inner_finger_joint"] = 0.0 +UR10e_ROBOTIQ_2F_85_CFG.init_state.joint_pos[".*_inner_finger_knuckle_joint"] = 0.0 +UR10e_ROBOTIQ_2F_85_CFG.init_state.joint_pos[".*_outer_.*_joint"] = 0.0 +# the major actuator joint for gripper +UR10e_ROBOTIQ_2F_85_CFG.actuators["gripper_drive"] = ImplicitActuatorCfg( + joint_names_expr=["finger_joint"], # "right_outer_knuckle_joint" is its mimic joint + effort_limit_sim=10.0, + velocity_limit_sim=1.0, + stiffness=11.25, + damping=0.1, + friction=0.0, + armature=0.0, +) +# enable the gripper to grasp in a parallel manner +UR10e_ROBOTIQ_2F_85_CFG.actuators["gripper_finger"] = ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_joint"], + effort_limit_sim=1.0, + velocity_limit_sim=1.0, + stiffness=0.2, + damping=0.001, + friction=0.0, + armature=0.0, +) +# set PD to zero for passive joints in close-loop gripper +UR10e_ROBOTIQ_2F_85_CFG.actuators["gripper_passive"] = ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_knuckle_joint", "right_outer_knuckle_joint"], + effort_limit_sim=1.0, + velocity_limit_sim=1.0, + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, +) + +"""Configuration of UR-10E arm with Robotiq 2F-85 gripper.""" diff --git a/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py b/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py index b4078d9f06e6..f5f6c6ac116e 100644 --- a/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py +++ b/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,4 +7,5 @@ # Configuration for different assets. ## +from .gelsight import * from .velodyne import * diff --git a/source/isaaclab_assets/isaaclab_assets/sensors/gelsight.py b/source/isaaclab_assets/isaaclab_assets/sensors/gelsight.py new file mode 100644 index 000000000000..13fe00e9d3c6 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/sensors/gelsight.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Predefined configurations for GelSight tactile sensors.""" + +from isaaclab_contrib.sensors.tacsl_sensor.visuotactile_sensor_cfg import GelSightRenderCfg + +## +# Predefined Configurations +## + +GELSIGHT_R15_CFG = GelSightRenderCfg( + sensor_data_dir_name="gelsight_r15_data", + background_path="bg.jpg", + calib_path="polycalib.npz", + real_background="real_bg.npy", + image_height=320, + image_width=240, + num_bins=120, + mm_per_pixel=0.0877, +) +"""Configuration for GelSight R1.5 sensor rendering parameters. + +The GelSight R1.5 is a high-resolution tactile sensor with a 320x240 pixel tactile image. +It uses a pixel-to-millimeter ratio of 0.0877 mm/pixel. + +Reference: https://www.gelsight.com/gelsightinc-products/ +""" + +GELSIGHT_MINI_CFG = GelSightRenderCfg( + sensor_data_dir_name="gs_mini_data", + background_path="bg.jpg", + calib_path="polycalib.npz", + real_background="real_bg.npy", + image_height=240, + image_width=320, + num_bins=120, + mm_per_pixel=0.065, +) +"""Configuration for GelSight Mini sensor rendering parameters. + +The GelSight Mini is a compact tactile sensor with a 240x320 pixel tactile image. +It uses a pixel-to-millimeter ratio of 0.065 mm/pixel, providing higher spatial resolution +than the R1.5 model. + +Reference: https://www.gelsight.com/gelsightinc-products/ +""" diff --git a/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py b/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py index d2324e118e7d..6cd075f4fa25 100644 --- a/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py +++ b/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py @@ -1,11 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Configuration for Velodyne LiDAR sensors.""" - from isaaclab.sensors import RayCasterCfg, patterns ## @@ -13,7 +12,7 @@ ## VELODYNE_VLP_16_RAYCASTER_CFG = RayCasterCfg( - attach_yaw_only=False, + ray_alignment="base", pattern_cfg=patterns.LidarPatternCfg( channels=16, vertical_fov_range=(-15.0, 15.0), horizontal_fov_range=(-180.0, 180.0), horizontal_res=0.2 ), diff --git a/source/isaaclab_assets/setup.py b/source/isaaclab_assets/setup.py index 37915a4ae188..ebd9331bd5f8 100644 --- a/source/isaaclab_assets/setup.py +++ b/source/isaaclab_assets/setup.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,8 +6,8 @@ """Installation script for the 'isaaclab_assets' python package.""" import os -import toml +import toml from setuptools import setup # Obtain the extension data from the extension.toml file @@ -30,7 +30,10 @@ classifiers=[ "Natural Language :: English", "Programming Language :: Python :: 3.10", + "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_assets/test/test_valid_configs.py b/source/isaaclab_assets/test/test_valid_configs.py index 85c337346c2d..acd68b260e54 100644 --- a/source/isaaclab_assets/test/test_valid_configs.py +++ b/source/isaaclab_assets/test/test_valid_configs.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,7 +8,7 @@ """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True) @@ -17,55 +17,48 @@ """Rest everything follows.""" -import unittest - -import isaaclab_assets as lab_assets # noqa: F401 +# Define a fixture to replace setUpClass +import pytest from isaaclab.assets import AssetBase, AssetBaseCfg from isaaclab.sim import build_simulation_context - -class TestValidEntitiesConfigs(unittest.TestCase): - """Test cases for all registered entities configurations.""" - - @classmethod - def setUpClass(cls): - # load all registered entities configurations from the module - cls.registered_entities: dict[str, AssetBaseCfg] = {} - # inspect all classes from the module - for obj_name in dir(lab_assets): - obj = getattr(lab_assets, obj_name) - # store all registered entities configurations - if isinstance(obj, AssetBaseCfg): - cls.registered_entities[obj_name] = obj - # print all existing entities names - print(">>> All registered entities:", list(cls.registered_entities.keys())) - - """ - Test fixtures. - """ - - def test_asset_configs(self): - """Check all registered asset configurations.""" - # iterate over all registered assets - for asset_name, entity_cfg in self.registered_entities.items(): - for device in ("cuda:0", "cpu"): - with self.subTest(asset_name=asset_name, device=device): - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # print the asset name - print(f">>> Testing entity {asset_name} on device {device}") - # name the prim path - entity_cfg.prim_path = "/World/asset" - # create the asset / sensors - entity: AssetBase = entity_cfg.class_type(entity_cfg) # type: ignore - - # play the sim - sim.reset() - - # check asset is initialized successfully - self.assertTrue(entity.is_initialized) +import isaaclab_assets as lab_assets # noqa: F401 -if __name__ == "__main__": - run_tests() +@pytest.fixture(scope="module") +def registered_entities(): + # load all registered entities configurations from the module + registered_entities: dict[str, AssetBaseCfg] = {} + # inspect all classes from the module + for obj_name in dir(lab_assets): + obj = getattr(lab_assets, obj_name) + # store all registered entities configurations + if isinstance(obj, AssetBaseCfg): + registered_entities[obj_name] = obj + # print all existing entities names + print(">>> All registered entities:", list(registered_entities.keys())) + return registered_entities + + +# Add parameterization for the device +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_asset_configs(registered_entities, device): + """Check all registered asset configurations.""" + # iterate over all registered assets + for asset_name, entity_cfg in registered_entities.items(): + # Use pytest's subtests + with build_simulation_context(device=device, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + # print the asset name + print(f">>> Testing entity {asset_name} on device {device}") + # name the prim path + entity_cfg.prim_path = "/World/asset" + # create the asset / sensors + entity: AssetBase = entity_cfg.class_type(entity_cfg) # type: ignore + + # play the sim + sim.reset() + + # check asset is initialized successfully + assert entity.is_initialized diff --git a/source/isaaclab_contrib/config/extension.toml b/source/isaaclab_contrib/config/extension.toml new file mode 100644 index 000000000000..326c9faf7ee8 --- /dev/null +++ b/source/isaaclab_contrib/config/extension.toml @@ -0,0 +1,21 @@ +[package] +# Semantic Versioning is used: https://semver.org/ +version = "0.0.2" + +# Description +title = "Isaac Lab External Contributions" +description="An extension used to stage and integrate externally contributed features and implementations." +readme = "docs/README.md" +repository = "https://github.com/isaac-sim/IsaacLab" +category = "robotics" +keywords = ["kit", "robotics", "assets", "isaaclab"] + +[dependencies] +"isaaclab" = {} + +[core] +reloadable = false + +# Main python module this extension provides. +[[python.module]] +name = "isaaclab_contrib" diff --git a/source/isaaclab_contrib/docs/CHANGELOG.rst b/source/isaaclab_contrib/docs/CHANGELOG.rst new file mode 100644 index 000000000000..a5e0bf4c2ef4 --- /dev/null +++ b/source/isaaclab_contrib/docs/CHANGELOG.rst @@ -0,0 +1,20 @@ +Changelog +--------- + +0.0.2 (2026-01-28) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :mod:`isaaclab_contrib.sensors.tacsl_sensor` module with the TacSL tactile sensor implementation + from :cite:t:`si2022taxim`. + + +0.0.1 (2025-12-17) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added initial implementation for multi rotor systems. diff --git a/source/isaaclab_contrib/docs/README.md b/source/isaaclab_contrib/docs/README.md new file mode 100644 index 000000000000..1861b593bd50 --- /dev/null +++ b/source/isaaclab_contrib/docs/README.md @@ -0,0 +1,506 @@ +# Isaac Lab: Community Contributions + +This extension (`isaaclab_contrib`) provides a collection of community-contributed components for Isaac Lab. These contributions extend the core framework with specialized robot types, actuator models, sensors, and other features that are not yet part of the main Isaac Lab package but are actively maintained and supported by the community. + +## Overview + +The `isaaclab_contrib` package serves as an incubator for experimental and specialized features that: + +- Extend Isaac Lab's capabilities for specific robot types or use cases +- Provide domain-specific actuator models and control interfaces +- Offer specialized MDP components for reinforcement learning tasks +- May eventually be integrated into the core Isaac Lab framework + +## Current Contributions + +### Multirotor Systems + +Comprehensive support for multirotor vehicles (drones, quadcopters, hexacopters, octocopters, etc.) including: + +- **Assets**: `Multirotor` articulation class with thruster-based control +- **Actuators**: `Thruster` model with realistic motor dynamics +- **MDP Components**: `ThrustAction` terms for RL control + +See the [Multirotor Systems](#multirotor-systems-detailed) section below for detailed documentation. + +### TacSL Tactile Sensor + +Support for tactile sensor from [Akinola et al., 2025](https://arxiv.org/abs/2408.06506). +It uses the Taxim model from [Si et al., 2022](https://arxiv.org/abs/2109.04027) to render the tactile images. + +See the [TacSL Tactile Sensor](#tacsl-tactile-sensor-detailed) section below for detailed documentation. + +## Extension Structure + +The extension follows Isaac Lab's standard package structure: + +```tree +isaaclab_contrib/ +โ”œโ”€โ”€ actuators/ # Contributed actuator models +โ”‚ โ”œโ”€โ”€ thruster.py # Thruster actuator for multirotors +โ”‚ โ””โ”€โ”€ thruster_cfg.py +โ”œโ”€โ”€ assets/ # Contributed asset classes +โ”‚ โ””โ”€โ”€ multirotor/ # Multirotor asset implementation +โ”œโ”€โ”€ mdp/ # MDP components for RL +โ”‚ โ””โ”€โ”€ actions/ # Action terms +โ”œโ”€โ”€ sensors/ # Contributed sensor classes +โ”‚ โ””โ”€โ”€ tacsl_sensor/ # TacSL tactile sensor implementation +โ””โ”€โ”€ utils/ # Utility functions and types +``` + +## Installation and Usage + +The `isaaclab_contrib` package is included with Isaac Lab. To use contributed components: + +```python +# Import multirotor components +from isaaclab_contrib.assets import Multirotor, MultirotorCfg +from isaaclab_contrib.actuators import Thruster, ThrusterCfg +from isaaclab_contrib.mdp.actions import ThrustActionCfg +from isaaclab_contrib.sensors import VisuoTactileSensor, VisuoTactileSensorCfg +``` + +--- + +## Multirotor Systems (Detailed) + +This section provides detailed documentation for the multirotor contribution, which enables simulation and control of multirotor aerial vehicles in Isaac Lab. + +
+ +### Features + +The multirotor system includes: + +#### Assets + +- **`Multirotor`**: A specialized articulation class that extends the base `Articulation` class to support multirotor systems with thruster actuators + - Manages multiple thruster actuators as a group + - Applies thrust forces at specific body locations + - Uses allocation matrices for control allocation + - Provides thruster-specific state information through `MultirotorData` + +#### Actuators + +- **`Thruster`**: A low-level motor/thruster dynamics model with realistic response characteristics: + - **Asymmetric rise and fall time constants**: Models different spin-up/spin-down rates + - **Thrust limits**: Configurable minimum and maximum thrust constraints + - **Integration schemes**: Euler or RK4 integration methods + - **Dynamic response**: Simulates motor transient behavior + +#### MDP Components + +- **`ThrustAction`**: Action terms specifically designed for multirotor control: + - Direct thrust commands to individual thrusters or groups + - Flexible preprocessing (scaling, offsetting, clipping) + - Automatic hover thrust offset computation + - Integrates with Isaac Lab's MDP framework for RL tasks + +### Quick Start + +#### Creating a Multirotor Asset + +```python +import isaaclab.sim as sim_utils +from isaaclab_contrib.assets import MultirotorCfg +from isaaclab_contrib.actuators import ThrusterCfg + +# Define thruster actuator configuration +thruster_cfg = ThrusterCfg( + thruster_names_expr=["rotor_[0-3]"], # Match rotors 0-3 + thrust_range=(0.0, 10.0), # Min and max thrust in Newtons + rise_time_constant=0.12, # Time constant for thrust increase (120ms) + fall_time_constant=0.25, # Time constant for thrust decrease (250ms) +) + +# Create multirotor configuration +multirotor_cfg = MultirotorCfg( + prim_path="/World/envs/env_.*/Quadcopter", + spawn=sim_utils.UsdFileCfg( + usd_path="path/to/quadcopter.usd", + ), + init_state=MultirotorCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.0), # Start 1m above ground + rps={".*": 110.0}, # All thrusters at 110 RPS (hover) + ), + actuators={ + "thrusters": thruster_cfg, + }, + allocation_matrix=[ # 6x4 matrix for quadcopter + [1.0, 1.0, 1.0, 1.0], # Total vertical thrust + [0.0, 0.0, 0.0, 0.0], # Lateral force X + [0.0, 0.0, 0.0, 0.0], # Lateral force Y + [0.0, 0.13, 0.0, -0.13], # Roll torque + [-0.13, 0.0, 0.13, 0.0], # Pitch torque + [0.01, -0.01, 0.01, -0.01], # Yaw torque + ], + rotor_directions=[1, -1, 1, -1], # Alternating CW/CCW +) +``` + +#### Using Thrust Actions in RL Environments + +```python +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.utils import configclass +from isaaclab_contrib.mdp.actions import ThrustActionCfg + +@configclass +class QuadcopterEnvCfg(ManagerBasedRLEnvCfg): + # ... scene, observations, rewards, etc. ... + + @configclass + class ActionsCfg: + # Normalized thrust control around hover + thrust = ThrustActionCfg( + asset_name="robot", + scale=2.0, # Actions in [-1,1] become [-2,2] N deviation + use_default_offset=True, # Add hover thrust from config + clip={".*": (0.0, 10.0)}, # Constrain final thrust to [0, 10] N + ) + + actions = ActionsCfg() +``` + +### Key Concepts + +#### Allocation Matrix + +The allocation matrix maps individual thruster forces to a 6D wrench (force + torque) applied to the multirotor's base link: + +``` +wrench = allocation_matrix @ thrust_vector +``` + +Where: +- `wrench`: [Fx, Fy, Fz, Tx, Ty, Tz]แต€ (6D body wrench) +- `allocation_matrix`: 6 ร— N matrix (6 DOF, N thrusters) +- `thrust_vector`: [Tโ‚, Tโ‚‚, ..., Tโ‚™]แต€ (N thruster forces) + +The matrix encodes the geometric configuration of thrusters including positions, orientations, and moment arms. + +#### Thruster Dynamics + +The `Thruster` actuator model simulates realistic motor response with asymmetric first-order dynamics: + +``` +dT/dt = (T_target - T_current) / ฯ„ +``` + +Where ฯ„ is the time constant (different for rise vs. fall): +- **Rise Time (ฯ„_rise)**: How quickly thrust increases when commanded (typically slower) +- **Fall Time (ฯ„_fall)**: How quickly thrust decreases when commanded (typically faster) +- **Thrust Limits**: Physical constraints [T_min, T_max] enforced after integration + +This asymmetry reflects real-world motor behavior primarily caused by ESC (Electronic Speed Controller) response and propeller aerodynamics, which result in slower spin-up (thrust increase) than spin-down. While rotor inertia affects both acceleration and deceleration equally, it is not the main cause of the asymmetric response. + +#### Thruster Control Modes + +The multirotor system supports different control approaches: + +1. **Direct Thrust Control**: Directly command thrust forces/RPS +2. **Normalized Control**: Commands as deviations from hover thrust +3. **Differential Control**: Small adjustments around equilibrium + +The `ThrustAction` term provides flexible preprocessing to support all modes through scaling and offsetting. + +
+ +### Demo Script + +A complete demonstration of quadcopter simulation is available: + +```bash +# Run quadcopter demo +./isaaclab.sh -p scripts/demos/quadcopter.py +``` + +## TacSL Tactile Sensor (Detailed) + +This section provides detailed documentation for the TacSL tactile sensor contribution, which enables GPU-based simulation of vision-based tactile sensors in Isaac Lab. The implementation is based on the TacSL framework from [Akinola et al., 2025](https://arxiv.org/abs/2408.06506) and uses the Taxim model from [Si et al., 2022](https://arxiv.org/abs/2109.04027) for rendering tactile images. + +
+ +### Features + +The TacSL tactile sensor system includes: + +#### Sensor Capabilities + +- **`VisuoTactileSensor`**: A specialized sensor class that simulates vision-based tactile sensors with elastomer deformation + - **Camera-based RGB sensing**: Renders realistic tactile images showing surface deformation and contact patterns + - **Force field sensing**: Computes per-taxel normal and shear forces for contact-rich manipulation + - **GPU-accelerated rendering**: Leverages GPU for efficient tactile image generation + - **SDF-based contact detection**: Uses signed distance fields for accurate geometry-elastomer interaction + +#### Configuration Options + +- **Elastomer Properties**: + - Configurable tactile array size (rows ร— columns of taxels) + - Adjustable tactile margin for sensor boundaries + - Compliant contact parameters (stiffness, damping) + +- **Physics Parameters**: + - Normal contact stiffness: Controls elastomer compression response + - Tangential stiffness: Models lateral resistance to sliding + - Friction coefficient: Defines surface friction properties + +- **Visualization & Debug**: + - Trimesh visualization of tactile contact points + - SDF closest point visualization + - Debug rendering of sensor point cloud + +### Quick Start + +#### Creating a Tactile Sensor + +```python +import isaaclab.sim as sim_utils +from isaaclab.sensors import TiledCameraCfg + +from isaaclab_contrib.sensors.tacsl_sensor import VisuoTactileSensorCfg + +from isaaclab_assets.sensors import GELSIGHT_R15_CFG + +# Define tactile sensor configuration +tactile_sensor_cfg = VisuoTactileSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer/tactile_sensor", + history_length=0, + debug_vis=False, + + # Sensor rendering configuration + render_cfg=GELSIGHT_R15_CFG, # Use GelSight R15 sensor parameters + + # Enable RGB and/or force field sensing + enable_camera_tactile=True, # RGB tactile images + enable_force_field=True, # Force field data + + # Elastomer configuration + tactile_array_size=(20, 25), # 20ร—25 taxel array + tactile_margin=0.003, # 3mm sensor margin + + # Contact object configuration + contact_object_prim_path_expr="{ENV_REGEX_NS}/contact_object", + + # Force field physics parameters + normal_contact_stiffness=1.0, # Normal stiffness (N/mm) + friction_coefficient=2.0, # Surface friction + tangential_stiffness=0.1, # Tangential stiffness + + # Camera configuration (must match render_cfg dimensions) + camera_cfg=TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer_tip/cam", + height=GELSIGHT_R15_CFG.image_height, + width=GELSIGHT_R15_CFG.image_width, + data_types=["distance_to_image_plane"], + spawn=None, # Camera already exists in USD + ), +) +``` + +#### Setting Up the Robot Asset with Compliant Contact + +```python +from isaaclab.assets import ArticulationCfg + +robot_cfg = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileWithCompliantContactCfg( + usd_path="path/to/gelsight_finger.usd", + + # Compliant contact parameters for elastomer + compliant_contact_stiffness=100.0, # Elastomer stiffness + compliant_contact_damping=10.0, # Elastomer damping + physics_material_prim_path="elastomer", # Prim with compliant contact + + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=12, + solver_velocity_iteration_count=1, + ), + collision_props=sim_utils.CollisionPropertiesCfg( + contact_offset=0.001, + rest_offset=-0.0005, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + ), + actuators={}, +) +``` + +#### Accessing Tactile Data + +```python +# In your simulation loop +scene.update(sim_dt) + +# Access tactile sensor data +tactile_data = scene["tactile_sensor"].data + +# RGB tactile image (if enabled) +if tactile_data.tactile_rgb_image is not None: + rgb_images = tactile_data.tactile_rgb_image # Shape: (num_envs, height, width, 3) + +# Force field data (if enabled) +if tactile_data.tactile_normal_force is not None: + normal_forces = tactile_data.tactile_normal_force # Shape: (num_envs * rows * cols,) + shear_forces = tactile_data.tactile_shear_force # Shape: (num_envs * rows * cols, 2) + + # Reshape to tactile array dimensions + num_envs = scene.num_envs + rows, cols = scene["tactile_sensor"].cfg.tactile_array_size + normal_forces = normal_forces.view(num_envs, rows, cols) + shear_forces = shear_forces.view(num_envs, rows, cols, 2) +``` + +### Key Concepts + +#### Sensor Modalities + +The TacSL sensor supports two complementary sensing modalities: + +1. **Camera-Based RGB Sensing** (`enable_camera_tactile=True`): + - Uses depth information from a camera inside the elastomer + - Renders realistic tactile images showing contact patterns and deformation + - Employs the Taxim rendering model for physically-based appearance + - Outputs RGB images that mimic real GelSight/DIGIT sensors + +2. **Force Field Sensing** (`enable_force_field=True`): + - Computes forces at each taxel (tactile element) in the array + - Provides normal forces (compression) and shear forces (tangential) + - Uses SDF-based contact detection with contact objects + - Enables direct force-based manipulation strategies + +#### Compliant Contact Model + +The sensor uses PhysX compliant contact for realistic elastomer deformation: + +- **Compliant Contact Stiffness**: Controls how much the elastomer compresses under load (higher = stiffer) +- **Compliant Contact Damping**: Controls energy dissipation during contact (affects bounce/settling) +- **Physics Material**: Specified prim (e.g., "elastomer") that has compliant contact enabled + +This allows the elastomer surface to deform realistically when contacting objects, which is essential for accurate tactile sensing. + +#### Tactile Array Configuration + +The sensor discretizes the elastomer surface into a grid of taxels: + +``` +tactile_array_size = (rows, cols) # e.g., (20, 25) = 500 taxels +``` + +- Each taxel corresponds to a point on the elastomer surface +- Forces are computed per-taxel for force field sensing +- The tactile_margin parameter defines the boundary region to exclude from sensing +- Higher resolution (more taxels) provides finer spatial detail but increases computation + +#### SDF-Based Contact Detection + +For force field sensing, the sensor uses Signed Distance Fields (SDFs): + +- Contact objects must have SDF collision meshes +- SDF provides distance and gradient information for force computation +- **Note**: Simple shape primitives (cubes, spheres spawned via `CuboidCfg`) cannot generate SDFs +- Use USD mesh assets for contact objects when force field sensing is required + +#### Sensor Rendering Pipeline + +The RGB tactile rendering follows this pipeline: + +1. **Initial Render**: Captures the reference state (no contact) +2. **Depth Capture**: Camera measures depth to elastomer surface during contact +3. **Deformation Computation**: Compares current depth to reference depth +4. **Taxim Rendering**: Generates RGB image based on deformation field +5. **Output**: Realistic tactile image showing contact geometry and patterns + +#### Physics Simulation Parameters + +For accurate tactile sensing, configure PhysX parameters: + +```python +sim_cfg = sim_utils.SimulationCfg( + dt=0.005, # 5ms timestep for stable contact simulation + physx=sim_utils.PhysxCfg( + gpu_collision_stack_size=2**30, # Increase for contact-rich scenarios + ), +) +``` + +Also ensure high solver iteration counts for the robot: + +```python +solver_position_iteration_count=12 # Higher = more accurate contact resolution +solver_velocity_iteration_count=1 +``` + +### Performance Considerations + +- **GPU Acceleration**: Tactile rendering is GPU-accelerated for efficiency +- **Multiple Sensors**: Can simulate multiple tactile sensors across parallel environments +- **Timing Analysis**: Use `sensor.get_timing_summary()` to profile rendering performance +- **SDF Computation**: Initial SDF generation may take time for complex meshes + +
+ +### Demo Script + +A complete demonstration of TacSL tactile sensor is available: + +```bash +# Run TacSL tactile sensor demo with RGB and force field sensing +./isaaclab.sh -p scripts/demos/sensors/tacsl_sensor.py \ + --use_tactile_rgb \ + --use_tactile_ff \ + --num_envs 16 \ + --contact_object_type nut + +# Save visualization data +./isaaclab.sh -p scripts/demos/sensors/tacsl_sensor.py \ + --use_tactile_rgb \ + --use_tactile_ff \ + --save_viz \ + --save_viz_dir tactile_output +``` + +--- + +## Testing + +The extension includes comprehensive unit tests for all contributed components: + +```bash +# Test multirotor components +python -m pytest source/isaaclab_contrib/test/assets/test_multirotor.py +python -m pytest source/isaaclab_contrib/test/actuators/test_thruster.py + +# Run all contrib tests +python -m pytest source/isaaclab_contrib/test/ +``` + +## Contributing + +We welcome community contributions to `isaaclab_contrib`! If you have developed: + +- Specialized robot asset classes +- Novel actuator models +- Custom MDP components +- Domain-specific utilities + +Please follow the Isaac Lab contribution guidelines and open a pull request. Contributions should: + +1. Follow the existing package structure +2. Include comprehensive documentation (docstrings, examples) +3. Provide unit tests +4. Be well-tested with Isaac Lab's simulation framework + +For more information, see the [Isaac Lab Contributing Guide](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html). + +## License + +This extension follows the same BSD-3-Clause license as Isaac Lab. See the LICENSE file for details. diff --git a/source/isaaclab_contrib/isaaclab_contrib/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/__init__.py new file mode 100644 index 000000000000..aef3b737ed8c --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/__init__.py @@ -0,0 +1,24 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Package for externally contributed components for Isaac Lab. + +This package provides externally contributed components for Isaac Lab, such as multirotors. +These components are not part of the core Isaac Lab framework yet, but are planned to be added +in the future. They are contributed by the community to extend the capabilities of Isaac Lab. +""" + +import os +import toml + +# Conveniences to other module directories via relative paths +ISAACLAB_CONTRIB_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +"""Path to the extension source directory.""" + +ISAACLAB_CONTRIB_METADATA = toml.load(os.path.join(ISAACLAB_CONTRIB_EXT_DIR, "config", "extension.toml")) +"""Extension metadata dictionary parsed from the extension.toml file.""" + +# Configure the module-level variables +__version__ = ISAACLAB_CONTRIB_METADATA["package"]["version"] diff --git a/source/isaaclab_contrib/isaaclab_contrib/actuators/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/actuators/__init__.py new file mode 100644 index 000000000000..7d0cbbc80882 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/actuators/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for thruster actuator models. + +This package provides actuator models specifically designed for multirotor thrusters. +The thruster actuator simulates realistic motor/propeller dynamics including asymmetric +rise and fall time constants, thrust limits, and dynamic response characteristics. +""" + +from .thruster import Thruster +from .thruster_cfg import ThrusterCfg diff --git a/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster.py b/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster.py new file mode 100644 index 000000000000..036a817fbfbd --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster.py @@ -0,0 +1,229 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils + +from isaaclab_contrib.utils.types import MultiRotorActions + +if TYPE_CHECKING: + from .thruster_cfg import ThrusterCfg + + +class Thruster: + """Low-level motor/thruster dynamics with separate rise/fall time constants. + + Integration scheme is Euler or RK4. All internal buffers are shaped (num_envs, num_motors). + Units: thrust [N], rates [N/s], time [s]. + """ + + computed_thrust: torch.Tensor + """The computed thrust for the actuator group. Shape is (num_envs, num_thrusters).""" + + applied_thrust: torch.Tensor + """The applied thrust for the actuator group. Shape is (num_envs, num_thrusters). + + This is the thrust obtained after clipping the :attr:`computed_thrust` based on the + actuator characteristics. + """ + + cfg: ThrusterCfg + + def __init__( + self, + cfg: ThrusterCfg, + thruster_names: list[str], + thruster_ids: slice | torch.Tensor, + num_envs: int, + device: str, + init_thruster_rps: torch.Tensor, + ): + """Construct buffers and sample per-motor parameters. + + Args: + cfg: Thruster configuration. + thruster_names: List of thruster names belonging to this group. + thruster_ids: Slice or tensor of indices into the articulation thruster array. + num_envs: Number of parallel/vectorized environments. + device: PyTorch device string or device identifier. + init_thruster_rps: Initial per-thruster rotations-per-second tensor used when + the configuration uses RPM-based thrust modelling. + """ + self.cfg = cfg + self._num_envs = num_envs + self._device = device + self._thruster_names = thruster_names + self._thruster_indices = thruster_ids + self._init_thruster_rps = init_thruster_rps + + # Range tensors, shaped (num_envs, 2, num_motors); [:,0,:]=min, [:,1,:]=max + self.num_motors = len(thruster_names) + self.thrust_r = torch.tensor(cfg.thrust_range).to(self._device) + self.tau_inc_r = torch.tensor(cfg.tau_inc_range).to(self._device) + self.tau_dec_r = torch.tensor(cfg.tau_dec_range).to(self._device) + + self.max_rate = torch.tensor(cfg.max_thrust_rate).expand(self._num_envs, self.num_motors).to(self._device) + + self.max_thrust = self.cfg.thrust_range[1] + self.min_thrust = self.cfg.thrust_range[0] + + # State & randomized per-motor parameters + self.tau_inc_s = math_utils.sample_uniform(*self.tau_inc_r, (self._num_envs, self.num_motors), self._device) + self.tau_dec_s = math_utils.sample_uniform(*self.tau_dec_r, (self._num_envs, self.num_motors), self._device) + self.thrust_const_r = torch.tensor(cfg.thrust_const_range, device=self._device, dtype=torch.float32) + self.thrust_const = math_utils.sample_uniform( + *self.thrust_const_r, (self._num_envs, self.num_motors), self._device + ).clamp(min=1e-6) + + self.curr_thrust = self.thrust_const * (self._init_thruster_rps.to(self._device).float() ** 2) + + # Mixing factor (discrete vs continuous form) + if self.cfg.use_discrete_approximation: + self.mixing_factor_function = self.discrete_mixing_factor + else: + self.mixing_factor_function = self.continuous_mixing_factor + + # Choose stepping kernel once (avoids per-step branching) + if self.cfg.integration_scheme == "euler": + self._step_thrust = self.compute_thrust_with_rpm_time_constant + elif self.cfg.integration_scheme == "rk4": + self._step_thrust = self.compute_thrust_with_rpm_time_constant_rk4 + else: + raise ValueError("integration scheme unknown") + + @property + def num_thrusters(self) -> int: + """Number of actuators in the group.""" + return len(self._thruster_names) + + @property + def thruster_names(self) -> list[str]: + """Articulation's thruster names that are part of the group.""" + return self._thruster_names + + @property + def thruster_indices(self) -> slice | torch.Tensor: + """Articulation's thruster indices that are part of the group. + + Note: + If :obj:`slice(None)` is returned, then the group contains all the thrusters in the articulation. + We do this to avoid unnecessary indexing of the thrusters for performance reasons. + """ + return self._thruster_indices + + def compute(self, control_action: MultiRotorActions) -> MultiRotorActions: + """Advance the thruster state one step. + + Applies saturation, chooses rise/fall tau per motor, computes mixing factor, + and integrates with the selected kernel. + + Args: + control_action: (num_envs, num_thrusters) commanded per-thruster thrust [N]. + + Returns: + (num_envs, num_thrusters) updated thrust state [N]. + + """ + des_thrust = control_action.thrusts + des_thrust = torch.clamp(des_thrust, *self.thrust_r) + + thrust_decrease_mask = torch.sign(self.curr_thrust) * torch.sign(des_thrust - self.curr_thrust) + motor_tau = torch.where(thrust_decrease_mask < 0, self.tau_dec_s, self.tau_inc_s) + mixing = self.mixing_factor_function(motor_tau) + + self.curr_thrust[:] = self._step_thrust(des_thrust, self.curr_thrust, mixing) + + self.computed_thrust = self.curr_thrust + self.applied_thrust = torch.clamp(self.computed_thrust, self.min_thrust, self.max_thrust) + + control_action.thrusts = self.applied_thrust + + return control_action + + def reset_idx(self, env_ids=None) -> None: + """Re-sample parameters and reinitialize state. + + Args: + env_ids: Env indices to reset. If ``None``, resets all envs. + """ + if env_ids is None: + env_ids = slice(None) + + if isinstance(env_ids, slice): + num_resets = self._num_envs + else: + num_resets = len(env_ids) + + self.tau_inc_s[env_ids] = math_utils.sample_uniform( + *self.tau_inc_r, + (num_resets, self.num_motors), + self._device, + ) + self.tau_dec_s[env_ids] = math_utils.sample_uniform( + *self.tau_dec_r, + (num_resets, self.num_motors), + self._device, + ) + self.thrust_const[env_ids] = math_utils.sample_uniform( + *self.thrust_const_r, + (num_resets, self.num_motors), + self._device, + ) + self.curr_thrust[env_ids] = self.thrust_const[env_ids] * self._init_thruster_rps[env_ids] ** 2 + + def reset(self, env_ids: Sequence[int]) -> None: + """Reset all envs.""" + self.reset_idx(env_ids) + + def motor_model_rate(self, error: torch.Tensor, mixing_factor: torch.Tensor): + return torch.clamp(mixing_factor * (error), -self.max_rate, self.max_rate) + + def rk4_integration(self, error: torch.Tensor, mixing_factor: torch.Tensor): + k1 = self.motor_model_rate(error, mixing_factor) + k2 = self.motor_model_rate(error + 0.5 * self.cfg.dt * k1, mixing_factor) + k3 = self.motor_model_rate(error + 0.5 * self.cfg.dt * k2, mixing_factor) + k4 = self.motor_model_rate(error + self.cfg.dt * k3, mixing_factor) + return (self.cfg.dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4) + + def discrete_mixing_factor(self, time_constant: torch.Tensor): + return 1.0 / (self.cfg.dt + time_constant) + + def continuous_mixing_factor(self, time_constant: torch.Tensor): + return 1.0 / time_constant + + def compute_thrust_with_rpm_time_constant( + self, + des_thrust: torch.Tensor, + curr_thrust: torch.Tensor, + mixing_factor: torch.Tensor, + ): + # Avoid negative or NaN values inside sqrt by clamping the ratio to >= 0. + current_ratio = torch.clamp(curr_thrust / self.thrust_const, min=0.0) + desired_ratio = torch.clamp(des_thrust / self.thrust_const, min=0.0) + current_rpm = torch.sqrt(current_ratio) + desired_rpm = torch.sqrt(desired_ratio) + rpm_error = desired_rpm - current_rpm + current_rpm += self.motor_model_rate(rpm_error, mixing_factor) * self.cfg.dt + return self.thrust_const * current_rpm**2 + + def compute_thrust_with_rpm_time_constant_rk4( + self, + des_thrust: torch.Tensor, + curr_thrust: torch.Tensor, + mixing_factor: torch.Tensor, + ) -> torch.Tensor: + current_ratio = torch.clamp(curr_thrust / self.thrust_const, min=0.0) + desired_ratio = torch.clamp(des_thrust / self.thrust_const, min=0.0) + current_rpm = torch.sqrt(current_ratio) + desired_rpm = torch.sqrt(desired_ratio) + rpm_error = desired_rpm - current_rpm + current_rpm += self.rk4_integration(rpm_error, mixing_factor) + return self.thrust_const * current_rpm**2 diff --git a/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster_cfg.py new file mode 100644 index 000000000000..29072f421abb --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster_cfg.py @@ -0,0 +1,62 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from .thruster import Thruster + + +@configclass +class ThrusterCfg: + """Configuration for thruster actuator groups. + + This config defines per-actuator-group parameters used by the low-level + thruster/motor models (time-constants, thrust ranges, integration scheme, + and initial state specifications). Fields left as ``MISSING`` are required + and must be provided by the user configuration. + """ + + class_type: type[Thruster] = Thruster + """Concrete Python class that consumes this config.""" + + dt: float = MISSING + """Simulation/integration timestep used by the thruster update [s].""" + + thrust_range: tuple[float, float] = MISSING + """Per-motor thrust clamp range [N]: values are clipped to this interval.""" + + max_thrust_rate: float = 100000.0 + """Per-motor thrust slew-rate limit applied inside the first-order model [N/s].""" + + thrust_const_range: tuple[float, float] = MISSING + """Range for thrust coefficient :math:`k_f` [N/(rpsยฒ)].""" + + tau_inc_range: tuple[float, float] = MISSING + """Range of time constants when commanded output is **increasing** (rise dynamics) [s].""" + + tau_dec_range: tuple[float, float] = MISSING + """Range of time constants when commanded output is **decreasing** (fall dynamics) [s].""" + + torque_to_thrust_ratio: float = MISSING + """Yaw-moment coefficient converting thrust to motor torque about +Z [Nยทm per N]. + Used as ``tau_z = torque_to_thrust_ratio * thrust_z * direction``. + """ + + use_discrete_approximation: bool = True + """ + Determines how the actuator/motor mixing factor is computed. Defaults to True. + + If True, uses the discrete-time factor ``1 / (dt + tau)``, accounting for the control loop timestep. + If False, uses the continuous-time factor ``1 / tau``. + """ + + integration_scheme: Literal["rk4", "euler"] = "rk4" + """Numerical integrator for the first-order model. Defaults to 'rk4'.""" + + thruster_names_expr: list[str] = MISSING + """Articulation's joint names that are part of the group.""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/assets/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/assets/__init__.py new file mode 100644 index 000000000000..8c40124e72ac --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/assets/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for externally contributed assets. + +This package provides specialized asset classes for simulating externally contributed +robots in Isaac Lab, such as multirotors. These assets are not part of the core +Isaac Lab framework yet, but are planned to be added in the future. They are +contributed by the community to extend the capabilities of Isaac Lab. +""" + +from .multirotor import Multirotor, MultirotorCfg, MultirotorData diff --git a/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/__init__.py new file mode 100644 index 000000000000..3ef1b482d05b --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/__init__.py @@ -0,0 +1,46 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for multirotor assets. + +This module provides specialized classes for simulating multirotor vehicles (drones, +quadcopters, hexacopters, etc.) in Isaac Lab. It extends the base articulation +framework to support thrust-based control through individual rotor/propeller actuators. + +Key Components: + - :class:`Multirotor`: Asset class for multirotor vehicles with thruster control + - :class:`MultirotorCfg`: Configuration class for multirotors + - :class:`MultirotorData`: Data container for multirotor state information + +Example: + .. code-block:: python + + from isaaclab_contrib.assets import Multirotor, MultirotorCfg + from isaaclab_contrib.actuators import ThrusterCfg + import isaaclab.sim as sim_utils + + # Configure multirotor + cfg = MultirotorCfg( + prim_path="/World/Robot", + spawn=sim_utils.UsdFileCfg(usd_path="path/to/quadcopter.usd"), + actuators={ + "thrusters": ThrusterCfg( + thruster_names_expr=["rotor_[0-3]"], + thrust_range=(0.0, 10.0), + ) + }, + ) + + # Create multirotor instance + multirotor = Multirotor(cfg) + +.. seealso:: + - :mod:`isaaclab_contrib.actuators`: Thruster actuator models + - :mod:`isaaclab_contrib.mdp.actions`: Thrust action terms for RL +""" + +from .multirotor import Multirotor +from .multirotor_cfg import MultirotorCfg +from .multirotor_data import MultirotorData diff --git a/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor.py b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor.py new file mode 100644 index 000000000000..6f8800c32215 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor.py @@ -0,0 +1,565 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Flag for pyright to ignore type errors in this file. +# pyright: reportPrivateUsage=false + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Articulation + +from isaaclab_contrib.actuators import Thruster +from isaaclab_contrib.utils.types import MultiRotorActions + +from .multirotor_data import MultirotorData + +if TYPE_CHECKING: + from .multirotor_cfg import MultirotorCfg + +# import logger +logger = logging.getLogger(__name__) + + +class Multirotor(Articulation): + """A multirotor articulation asset class. + + This class extends the base :class:`~isaaclab.assets.Articulation` class to support multirotor vehicles + (such as quadcopters, hexacopters, and octocopters) with thruster actuators that apply forces + at specific body locations. It is based on the implementation from :cite:t:`kulkarni2025aerialgym`. + + Unlike standard articulations that use joint-based control, multirotors are controlled through + thrust forces generated by individual rotors/propellers. This class provides specialized functionality + for managing multiple thruster actuators, computing combined wrenches from individual thrusts, + and applying them to the multirotor's base link. + + Key Features: + - **Thruster-based control**: Uses :class:`~isaaclab_contrib.actuators.Thruster` actuators + instead of joint actuators for realistic rotor dynamics simulation. + - **Force allocation**: Supports allocation matrices to convert individual thruster forces + into combined body wrenches (forces and torques). + - **Asymmetric dynamics**: Thruster actuators can model asymmetric rise/fall dynamics + that reflect real motor behavior. + - **Flexible configuration**: Supports arbitrary numbers and arrangements of thrusters + through regex-based thruster naming patterns. + + Usage Example: + .. code-block:: python + + import isaaclab.sim as sim_utils + from isaaclab_contrib.assets import MultirotorCfg + from isaaclab_contrib.actuators import ThrusterCfg + + # Define thruster actuator configuration + thruster_cfg = ThrusterCfg( + thruster_names_expr=["rotor_[0-3]"], # Match rotors 0-3 + thrust_range=(0.0, 10.0), # Min and max thrust in Newtons + rise_time_constant=0.1, # Time constant for thrust increase + fall_time_constant=0.2, # Time constant for thrust decrease + ) + + # Create multirotor configuration + multirotor_cfg = MultirotorCfg( + prim_path="/World/envs/env_.*/Robot", + spawn=sim_utils.UsdFileCfg(usd_path="path/to/quadcopter.usd"), + actuators={"thrusters": thruster_cfg}, + allocation_matrix=[ # 6x4 matrix for quadcopter (6 DOF, 4 thrusters) + [1.0, 1.0, 1.0, 1.0], # Total vertical force + [0.0, 0.0, 0.0, 0.0], # Lateral force (x) + [0.0, 0.0, 0.0, 0.0], # Lateral force (y) + [0.0, 0.1, 0.0, -0.1], # Roll torque + [-0.1, 0.0, 0.1, 0.0], # Pitch torque + [0.01, -0.01, 0.01, -0.01], # Yaw torque + ], + ) + + # Create the multirotor instance + multirotor = multirotor_cfg.class_type(multirotor_cfg) + + .. note:: + The allocation matrix maps individual thruster forces to a 6D wrench (3 forces + 3 torques) + applied to the base link. The matrix dimensions should be (6, num_thrusters). + + .. seealso:: + - :class:`~isaaclab.assets.Articulation`: Base articulation class + - :class:`MultirotorCfg`: Configuration class for multirotors + - :class:`MultirotorData`: Data container for multirotor state + - :class:`~isaaclab_contrib.actuators.Thruster`: Thruster actuator model + """ + + cfg: MultirotorCfg + """Configuration instance for the multirotor.""" + + actuators: dict[str, Thruster] + """Dictionary of thruster actuator instances for the multirotor. + + 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:`MultirotorCfg.actuators` + attribute. They are used to compute the thruster commands during the :meth:`write_data_to_sim` function. + """ + + def __init__(self, cfg: MultirotorCfg): + """Initialize the multirotor articulation. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + """ + Properties + """ + + @property + def thruster_names(self) -> list[str]: + """Ordered names of thrusters in the multirotor. + + This property aggregates thruster names from all thruster actuator groups configured + for the multirotor. The names are ordered according to their array indices, which is + important for setting thrust targets and interpreting thruster data. + + Returns: + A list of thruster names in order. Returns an empty list if actuators are not yet initialized. + + Raises: + ValueError: If a non-thruster actuator is found in the multirotor actuators. + """ + if not hasattr(self, "actuators") or not self.actuators: + return [] + + thruster_names = [] + for actuator in self.actuators.values(): + if hasattr(actuator, "thruster_names"): + thruster_names.extend(actuator.thruster_names) + else: + raise ValueError("Non thruster actuator found in multirotor actuators. Not supported at the moment.") + + return thruster_names + + @property + def num_thrusters(self) -> int: + """Number of thrusters in the multirotor. + + Returns: + Total number of thrusters across all actuator groups. + """ + return len(self.thruster_names) + + @property + def allocation_matrix(self) -> torch.Tensor: + """Allocation matrix for control allocation. + + The allocation matrix maps individual thruster forces to a 6D wrench vector + (3 forces + 3 torques) applied to the base link. This allows converting + per-thruster commands into the resulting body-frame forces and moments. + + The matrix has shape (6, num_thrusters), where: + - Rows 0-2: Force contributions in body frame (Fx, Fy, Fz) + - Rows 3-5: Torque contributions in body frame (Tx, Ty, Tz) + + Returns: + Allocation matrix as a torch tensor on the device. + """ + return torch.tensor(self.cfg.allocation_matrix, device=self.device, dtype=torch.float32) + + """ + Operations + """ + + def set_thrust_target( + self, + target: torch.Tensor, + thruster_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set target thrust values for thrusters. + + This method sets the desired thrust values for specific thrusters in specific environments. + The thrust targets are stored and later processed by the thruster actuator models during + the :meth:`write_data_to_sim` call. The actuator models may apply dynamics (rise/fall times) + and constraints (thrust limits) to these targets. + + Args: + target: Target thrust values. Shape is (num_envs, num_thrusters) or (num_envs,). + The values are typically in the same units as configured in the thruster actuator + (e.g., Newtons for force, or revolutions per second for RPS). + thruster_ids: Indices of thrusters to set. Defaults to None (all thrusters). + Can be a sequence of integers, a slice, or None. + env_ids: Environment indices to set. Defaults to None (all environments). + Can be a sequence of integers or None. + + Example: + .. code-block:: python + + # Set thrust for all thrusters in all environments + multirotor.set_thrust_target(torch.ones(num_envs, 4) * 5.0) + + # Set thrust for specific thrusters + multirotor.set_thrust_target( + torch.tensor([[5.0, 6.0]]), # Different thrust for 2 thrusters + thruster_ids=[0, 2], # Apply to thrusters 0 and 2 + env_ids=[0], # Only in environment 0 + ) + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if thruster_ids is None: + thruster_ids = slice(None) + + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and thruster_ids != slice(None): + env_ids = env_ids[:, None] + + # set targets + self._data.thrust_target[env_ids, thruster_ids] = target + + def reset(self, env_ids: Sequence[int] | None = None): + """Reset the multirotor to default state. + + This method resets both the base articulation state (pose, velocities) and + multirotor-specific state (thruster targets) to their default values as specified + in the configuration. + + Args: + env_ids: Environment indices to reset. Defaults to None (all environments). + Can be a sequence of integers or None. + + Note: + The default thruster state is set via the :attr:`MultirotorCfg.init_state.rps` + configuration parameter. + """ + # call parent reset + super().reset(env_ids) + + # reset multirotor-specific data + 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) + + # reset thruster targets to default values + if self._data.thrust_target is not None and self._data.default_thruster_rps is not None: + self._data.thrust_target[env_ids] = self._data.default_thruster_rps[env_ids] + + def write_data_to_sim(self): + """Write thrust and torque commands to the simulation. + + This method performs the following operations in sequence: + + 1. **Apply actuator models**: Process thrust targets through thruster actuator models + to compute actual thrust values considering dynamics (rise/fall times) and + constraints (thrust limits). + + 2. **Combine thrusts into wrench**: Use the allocation matrix to convert individual + thruster forces into a combined 6D wrench (force + torque) vector. + + 3. **Apply to simulation**: Apply the combined wrench to the base link of the multirotor + in the PhysX simulation. + + This method should be called after setting thrust targets with :meth:`set_thrust_target` + and before stepping the simulation. + + Note: + This method overrides the base class implementation because multirotors use thrust-based + control rather than joint-based control. + """ + self._apply_actuator_model() + # apply thruster forces at individual locations + self._apply_combined_wrench() + + """ + Internal methods + """ + + def _initialize_impl(self): + """Initialize the multirotor implementation.""" + # call parent initialization + super()._initialize_impl() + + # Replace data container with MultirotorData + self._data = MultirotorData(self.root_physx_view, self.device) + + # Create thruster buffers with correct size (SINGLE PHASE) + self._create_thruster_buffers() + # Process thruster configuration + self._process_thruster_cfg() + # Process configuration + self._process_cfg() + # Update the robot data + self.update(0.0) + + # Log multirotor information + self._log_multirotor_info() + + def _create_thruster_buffers(self): + """Create thruster buffers with correct size.""" + num_instances = self.num_instances + num_thrusters = self._count_thrusters_from_config() + + # Create thruster data tensors with correct size + self._data.default_thruster_rps = torch.zeros(num_instances, num_thrusters, device=self.device) + # thrust after controller and allocation is applied + self._data.thrust_target = torch.zeros(num_instances, num_thrusters, device=self.device) + self._data.computed_thrust = torch.zeros(num_instances, num_thrusters, device=self.device) + self._data.applied_thrust = torch.zeros(num_instances, num_thrusters, device=self.device) + + # Combined wrench buffers + self._thrust_target_sim = torch.zeros_like(self._data.thrust_target) # thrust after actuator model is applied + # wrench target for combined mode + self._internal_wrench_target_sim = torch.zeros(num_instances, 6, device=self.device) + # internal force/torque targets per body for combined mode + self._internal_force_target_sim = torch.zeros(num_instances, self.num_bodies, 3, device=self.device) + self._internal_torque_target_sim = torch.zeros(num_instances, self.num_bodies, 3, device=self.device) + + # Placeholder thruster names (will be filled during actuator creation) + self._data.thruster_names = [f"thruster_{i}" for i in range(num_thrusters)] + + def _count_thrusters_from_config(self) -> int: + """Count total number of thrusters from actuator configuration. + + This method parses all actuator configurations to determine the total number + of thrusters before they are initialized. It uses the thruster name expressions + to find matching bodies in the USD prim. + + Returns: + Total number of thrusters across all actuator groups. + + Raises: + ValueError: If no thrusters are found in the configuration. + """ + total_thrusters = 0 + + for actuator_name, actuator_cfg in self.cfg.actuators.items(): + if not hasattr(actuator_cfg, "thruster_names_expr"): + continue + + # Use find_bodies to count thrusters for this actuator + body_indices, thruster_names = self.find_bodies(actuator_cfg.thruster_names_expr) + total_thrusters += len(body_indices) + + if total_thrusters == 0: + raise ValueError( + "No thrusters found in actuator configuration. " + "Please check 'thruster_names_expr' in the provided 'MultirotorCfg.actuators' configuration." + ) + + return total_thrusters + + def _process_actuators_cfg(self): + """Override parent method to do nothing - we handle thrusters separately.""" + # Do nothing - we handle thruster processing in _process_thruster_cfg() otherwise this + # gives issues with joint name expressions + pass + + def _process_cfg(self): + """Post processing of multirotor configuration parameters.""" + # Handle root state (like parent does) + 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) + + # Handle thruster-specific initial state + if hasattr(self._data, "default_thruster_rps") and hasattr(self.cfg.init_state, "rps"): + # Match against thruster names + indices_list, _, values_list = string_utils.resolve_matching_names_values( + self.cfg.init_state.rps, self.thruster_names + ) + if indices_list: + rps_values = torch.tensor(values_list, device=self.device) + self._data.default_thruster_rps[:, indices_list] = rps_values + self._data.thrust_target[:, indices_list] = rps_values + + def _process_thruster_cfg(self): + """Process and apply multirotor thruster properties.""" + # create actuators + self.actuators = dict() + self._has_implicit_actuators = False + + # Check for mixed configurations (same as before) + has_thrusters = False + has_joints = False + + for actuator_name, actuator_cfg in self.cfg.actuators.items(): + if hasattr(actuator_cfg, "thruster_names_expr"): + has_thrusters = True + elif hasattr(actuator_cfg, "joint_names_expr"): + has_joints = True + + if has_thrusters and has_joints: + raise ValueError("Mixed configurations with both thrusters and regular joints are not supported.") + + if has_joints: + raise ValueError("Regular joint actuators are not supported in Multirotor class.") + + # Store the body-to-thruster mapping + self._thruster_body_mapping = {} + + # Track thruster names as we create actuators + all_thruster_names = [] + + for actuator_name, actuator_cfg in self.cfg.actuators.items(): + body_indices, thruster_names = self.find_bodies(actuator_cfg.thruster_names_expr) + + # Create 0-based thruster array indices starting from current count + start_idx = len(all_thruster_names) + thruster_array_indices = list(range(start_idx, start_idx + len(body_indices))) + + # Track all thruster names + all_thruster_names.extend(thruster_names) + + # Store the mapping + self._thruster_body_mapping[actuator_name] = { + "body_indices": body_indices, + "array_indices": thruster_array_indices, + "thruster_names": thruster_names, + } + + # Create thruster actuator + actuator: Thruster = actuator_cfg.class_type( + cfg=actuator_cfg, + thruster_names=thruster_names, + thruster_ids=thruster_array_indices, + num_envs=self.num_instances, + device=self.device, + init_thruster_rps=self._data.default_thruster_rps[:, thruster_array_indices], + ) + + # Store actuator + self.actuators[actuator_name] = actuator + + # Log information + logger.info( + f"Thruster actuator: {actuator_name} with model '{actuator_cfg.class_type.__name__}'" + f" (thruster names: {thruster_names} [{body_indices}])." + ) + + # Update thruster names in data container + self._data.thruster_names = all_thruster_names + + # Log summary + logger.info(f"Initialized {len(self.actuators)} thruster actuator(s) for multirotor.") + + def _apply_actuator_model(self): + """Processes thruster commands for the multirotor by forwarding them to the actuators. + + This internal method iterates through all thruster actuator groups and applies their + respective actuator models to the thrust targets. The actuator models simulate realistic + motor dynamics including: + + - Rise/fall time constants for asymmetric response + - Thrust saturation and clipping to physical limits + - Integration of motor dynamics over time + + The computed thrust values are stored in internal buffers for subsequent wrench computation. + + Note: + This method updates: + - :attr:`_thrust_target_sim`: Processed thrust values after actuator model + - :attr:`_data.computed_thrust`: Thrust before saturation + - :attr:`_data.applied_thrust`: Final thrust after saturation + """ + + # process thruster actions per group + for actuator in self.actuators.values(): + if not isinstance(actuator, Thruster): + continue + + # prepare input for actuator model based on cached data + control_action = MultiRotorActions( + thrusts=self._data.thrust_target[:, actuator.thruster_indices], + thruster_indices=actuator.thruster_indices, + ) + + # compute thruster command from the actuator model + control_action = actuator.compute(control_action) + + # update targets (these are set into the simulation) + if control_action.thrusts is not None: + self._thrust_target_sim[:, actuator.thruster_indices] = control_action.thrusts + + # update state of the actuator model + self._data.computed_thrust[:, actuator.thruster_indices] = actuator.computed_thrust + self._data.applied_thrust[:, actuator.thruster_indices] = actuator.applied_thrust + + def _apply_combined_wrench(self): + """Apply combined wrench to the base link. + + This internal method applies the 6D wrench (computed by :meth:`_combine_thrusts`) + to the base link of the multirotor. The wrench is applied at the center of mass + of the base link in the local body frame. + + The forces and torques are applied through PhysX's force/torque API, which integrates + them during the physics step to produce accelerations and velocities. + """ + # Combine individual thrusts into a wrench vector + self._combine_thrusts() + + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._internal_force_target_sim.view(-1, 3), # Shape: (num_envs * num_bodies, 3) + torque_data=self._internal_torque_target_sim.view(-1, 3), # Shape: (num_envs * num_bodies, 3) + position_data=None, # Apply at center of mass + indices=self._ALL_INDICES, + is_global=False, # Forces are in local frame + ) + + def _combine_thrusts(self): + """Combine individual thrusts into a wrench vector. + + This internal method uses the allocation matrix to convert individual thruster + forces into a 6D wrench vector (3D force + 3D torque) in the body frame. The + wrench is then assigned to the base link (body index 0) for application to + the simulation. + + The allocation matrix encodes the geometric configuration of the thrusters, + including their positions and orientations relative to the center of mass. + + Mathematical operation: + wrench = allocation_matrix @ thrusts + where wrench = [Fx, Fy, Fz, Tx, Ty, Tz]^T + """ + thrusts = self._thrust_target_sim + self._internal_wrench_target_sim = (self.allocation_matrix @ thrusts.T).T + # Apply forces to base link (body index 0) only + self._internal_force_target_sim[:, 0, :] = self._internal_wrench_target_sim[:, :3] + self._internal_torque_target_sim[:, 0, :] = self._internal_wrench_target_sim[:, 3:] + + def _validate_cfg(self): + """Validate the multirotor 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. + """ + # Only validate if actuators have been created + if hasattr(self, "actuators") and self.actuators: + # Validate thruster-specific configuration + for actuator_name in self.actuators: + if isinstance(self.actuators[actuator_name], Thruster): + initial_thrust = self.actuators[actuator_name].curr_thrust + # check that the initial thrust is within the limits + thrust_limits = self.actuators[actuator_name].cfg.thrust_range + if torch.any(initial_thrust < thrust_limits[0]) or torch.any(initial_thrust > thrust_limits[1]): + raise ValueError( + f"Initial thrust for actuator '{actuator_name}' is out of bounds: " + f"{initial_thrust} not in {thrust_limits}" + ) + + def _log_multirotor_info(self): + """Log multirotor-specific information.""" + logger.info(f"Multirotor initialized with {self.num_thrusters} thrusters") + logger.info(f"Thruster names: {self.thruster_names}") + logger.info(f"Thruster force direction: {self.cfg.thruster_force_direction}") diff --git a/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor_cfg.py new file mode 100644 index 000000000000..9638fcf2aa66 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor_cfg.py @@ -0,0 +1,286 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Sequence +from dataclasses import MISSING + +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils import configclass + +from isaaclab_contrib.actuators import ThrusterCfg + +from .multirotor import Multirotor + + +@configclass +class MultirotorCfg(ArticulationCfg): + """Configuration parameters for a multirotor articulation. + + This configuration class extends :class:`~isaaclab.assets.ArticulationCfg` to add + multirotor-specific parameters including thruster actuators, allocation matrices, + and thruster-specific initial states. + + Unlike standard articulations that use joint actuators, multirotors are configured + with :class:`~isaaclab_contrib.actuators.ThrusterCfg` actuators that model individual + rotor/propeller dynamics. + + Key Configuration Parameters: + - **actuators**: Dictionary mapping actuator names to :class:`~isaaclab_contrib.actuators.ThrusterCfg` + configurations. Each configuration defines a group of thrusters with shared properties. + - **allocation_matrix**: Maps individual thruster forces to 6D body wrenches. This matrix + encodes the geometric configuration and should have shape (6, num_thrusters). + - **thruster_force_direction**: Direction vector in body frame that thrusters push along. + Typically (0, 0, 1) for upward-facing thrusters. + - **rotor_directions**: Spin direction of each rotor (1 for CCW, -1 for CW). Used for + computing reaction torques. + + Example: + .. code-block:: python + + from isaaclab_contrib.assets import MultirotorCfg + from isaaclab_contrib.actuators import ThrusterCfg + import isaaclab.sim as sim_utils + + # Quadcopter configuration + quadcopter_cfg = MultirotorCfg( + prim_path="/World/envs/env_.*/Quadcopter", + spawn=sim_utils.UsdFileCfg( + usd_path="path/to/quadcopter.usd", + ), + init_state=MultirotorCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.0), # Start 1m above ground + rps={".*": 110.0}, # All thrusters at 110 RPS (hover) + ), + actuators={ + "thrusters": ThrusterCfg( + thruster_names_expr=["rotor_[0-3]"], + thrust_range=(0.0, 12.0), # 0-12N per thruster + rise_time_constant=0.12, + fall_time_constant=0.25, + ), + }, + allocation_matrix=[ + [1.0, 1.0, 1.0, 1.0], # Vertical thrust + [0.0, 0.0, 0.0, 0.0], # Lateral force X + [0.0, 0.0, 0.0, 0.0], # Lateral force Y + [0.0, 0.13, 0.0, -0.13], # Roll torque + [-0.13, 0.0, 0.13, 0.0], # Pitch torque + [0.01, -0.01, 0.01, -0.01], # Yaw torque + ], + rotor_directions=[1, -1, 1, -1], # Alternating CW/CCW + ) + + .. seealso:: + - :class:`~isaaclab.assets.ArticulationCfg`: Base articulation configuration + - :class:`~isaaclab_contrib.actuators.ThrusterCfg`: Thruster actuator configuration + - :class:`Multirotor`: Multirotor asset class + """ + + class_type: type = Multirotor + + @configclass + class InitialStateCfg(ArticulationCfg.InitialStateCfg): + """Initial state of the multirotor articulation. + + This extends the base articulation initial state to include thruster-specific + initial conditions. The thruster initial state is particularly important for + multirotor stability, as it determines the starting thrust levels. + + For hovering multirotors, the initial RPS should be set to values that produce + enough thrust to counteract gravity. + """ + + # multirotor-specific initial state + rps: dict[str, float] = {".*": 100.0} + """Revolutions per second (RPS) of the thrusters. Default is 100 RPS. + + This can be specified as: + + - A dictionary mapping regex patterns to RPS values + - A single wildcard pattern like ``{".*": 100.0}`` for uniform RPS + - Explicit per-thruster values like ``{"rotor_0": 95.0, "rotor_1": 105.0}`` + + The RPS values are used to initialize the thruster states and determine the + default thrust targets when the multirotor is reset. + + Example: + .. code-block:: python + + # Uniform RPS for all thrusters + rps = {".*": 110.0} + + # Different RPS for different thruster groups + rps = {"rotor_[0-1]": 105.0, "rotor_[2-3]": 115.0} + + Note: + The actual thrust produced depends on the thruster model's thrust curve + and other parameters in :class:`~isaaclab_contrib.actuators.ThrusterCfg`. + """ + + # multirotor-specific configuration + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the multirotor object. + + This includes both the base articulation state (position, orientation, velocities) + and multirotor-specific state (thruster RPS). See :class:`InitialStateCfg` for details. + """ + + actuators: dict[str, ThrusterCfg] = MISSING + """Thruster actuators for the multirotor with corresponding thruster names. + + This dictionary maps actuator group names to their configurations. Each + :class:`~isaaclab_contrib.actuators.ThrusterCfg` defines a group of thrusters + with shared dynamic properties (rise/fall times, thrust limits, etc.). + + Example: + .. code-block:: python + + actuators = { + "thrusters": ThrusterCfg( + thruster_names_expr=["rotor_.*"], # Regex to match thruster bodies + thrust_range=(0.0, 10.0), + rise_time_constant=0.1, + fall_time_constant=0.2, + ) + } + + Note: + Unlike standard articulations, multirotors should only contain thruster actuators. + Mixing joint-based and thrust-based actuators is not currently supported. + """ + + # multirotor force application settings + thruster_force_direction: tuple[float, float, float] = (0.0, 0.0, 1.0) + """Default force direction in body-local frame for thrusters. Default is ``(0.0, 0.0, 1.0)``, + which is upward along the Z-axis. + + This 3D unit vector specifies the direction in which thrusters generate force + in the multirotor's body frame. For standard configurations: + + - ``(0.0, 0.0, 1.0)``: Thrusters push upward (Z-axis, typical for quadcopters) + - ``(0.0, 0.0, -1.0)``: Thrusters push downward + - ``(1.0, 0.0, 0.0)``: Thrusters push forward (X-axis) + + This is used in conjunction with the allocation matrix to compute the wrench + produced by each thruster. + + Default: ``(0.0, 0.0, 1.0)`` (upward along Z-axis) + """ + + allocation_matrix: Sequence[Sequence[float]] | None = None + """Allocation matrix for control allocation. Default is ``None``, which means that the thrusters + are not used for control allocation. + + This matrix maps individual thruster forces to the 6D wrench (force + torque) + applied to the multirotor's base link. It has shape ``(6, num_thrusters)``: + + - **Rows 0-2**: Force contributions in body frame (Fx, Fy, Fz) + - **Rows 3-5**: Torque contributions in body frame (Tx, Ty, Tz) + + The allocation matrix encodes the geometric configuration of the multirotor, + including thruster positions, orientations, and moment arms. + + Example for a quadcopter (4 thrusters in + configuration): + .. code-block:: python + + allocation_matrix = [ + [1.0, 1.0, 1.0, 1.0], # Total vertical thrust + [0.0, 0.0, 0.0, 0.0], # No lateral force + [0.0, 0.0, 0.0, 0.0], # No lateral force + [0.0, 0.13, 0.0, -0.13], # Roll moment (left/right) + [-0.13, 0.0, 0.13, 0.0], # Pitch moment (forward/back) + [0.01,-0.01, 0.01,-0.01], # Yaw moment (rotation) + ] + + Note: + If ``None``, forces must be applied through other means. For typical + multirotor control, this should always be specified. + """ + + rotor_directions: Sequence[int] | None = None + """Sequence of rotor directions for each thruster. Default is ``None``, which means that the rotor directions + are not specified. + + This specifies the spin direction of each rotor, which affects the reaction + torques generated. Values should be: + + - ``1``: Counter-clockwise (CCW) rotation + - ``-1``: Clockwise (CW) rotation + + For a quadcopter, a typical configuration is alternating directions to + cancel reaction torques during hover: ``[1, -1, 1, -1]``. + + Example: + .. code-block:: python + + # Quadcopter with alternating rotor directions + rotor_directions = [1, -1, 1, -1] + + # Hexacopter + rotor_directions = [1, -1, 1, -1, 1, -1] + + Note: + The length must match the total number of thrusters defined in the + actuators configuration, otherwise a ``ValueError`` will be raised + during initialization. + """ + + def __post_init__(self): + """Post initialization validation.""" + # Skip validation if actuators is MISSING + if self.actuators is MISSING: + return + + # Count the total number of thrusters from all actuator configs + num_thrusters = 0 + for thruster_cfg in self.actuators.values(): + if hasattr(thruster_cfg, "thruster_names_expr") and thruster_cfg.thruster_names_expr is not None: + num_thrusters += len(thruster_cfg.thruster_names_expr) + + # Validate rotor_directions matches number of thrusters + if self.rotor_directions is not None: + num_rotor_directions = len(self.rotor_directions) + if num_thrusters != num_rotor_directions: + raise ValueError( + f"Mismatch between number of thrusters ({num_thrusters}) and " + f"rotor_directions ({num_rotor_directions}). " + "They must have the same number of elements." + ) + + # Validate rps explicit entries match number of thrusters + # Only validate if rps has explicit entries (not just a wildcard pattern) + if hasattr(self.init_state, "rps") and self.init_state.rps is not None: + rps_keys = list(self.init_state.rps.keys()) + # Check if rps uses a wildcard pattern (single key that's a regex) + is_wildcard = len(rps_keys) == 1 and (rps_keys[0] == ".*" or rps_keys[0] == ".*:.*") + + if not is_wildcard and len(rps_keys) != num_thrusters: + raise ValueError( + f"Mismatch between number of thrusters ({num_thrusters}) and " + f"rps entries ({len(rps_keys)}). " + "They must have the same number of elements when using explicit rps keys." + ) + + # Validate allocation_matrix second dimension matches number of thrusters + if self.allocation_matrix is not None: + if len(self.allocation_matrix) == 0: + raise ValueError("Allocation matrix cannot be empty.") + + # Check that all rows have the same length + num_cols = len(self.allocation_matrix[0]) + for i, row in enumerate(self.allocation_matrix): + if len(row) != num_cols: + raise ValueError( + f"Allocation matrix row {i} has length {len(row)}, " + f"but expected {num_cols} (all rows must have the same length)." + ) + + # Validate that the second dimension (columns) matches number of thrusters + if num_cols != num_thrusters: + raise ValueError( + f"Mismatch between number of thrusters ({num_thrusters}) and " + f"allocation matrix columns ({num_cols}). " + "The second dimension of the allocation matrix must match the number of thrusters." + ) diff --git a/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor_data.py b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor_data.py new file mode 100644 index 000000000000..05ea56c4565a --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/assets/multirotor/multirotor_data.py @@ -0,0 +1,105 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch + +from isaaclab.assets.articulation.articulation_data import ArticulationData + + +class MultirotorData(ArticulationData): + """Data container for a multirotor articulation. + + This class extends the base :class:`~isaaclab.assets.ArticulationData` container to include + multirotor-specific data such as thruster states, thrust targets, and computed forces. + It provides access to all the state information needed for monitoring and controlling + multirotor vehicles. + + The data container is automatically created and managed by the :class:`~isaaclab_contrib.assets.Multirotor` + class. Users typically access this data through the :attr:`Multirotor.data` property. + + Note: + All tensor attributes have shape ``(num_instances, num_thrusters)`` where + ``num_instances`` is the number of environment instances and ``num_thrusters`` + is the total number of thrusters per multirotor. + + .. seealso:: + - :class:`~isaaclab.assets.ArticulationData`: Base articulation data container + - :class:`~isaaclab_contrib.assets.Multirotor`: Multirotor asset class + """ + + thruster_names: list[str] = None + """List of thruster names in the multirotor. + + This list contains the ordered names of all thrusters, matching the order used + for indexing in the thrust tensors. The names correspond to the USD body prim names + matched by the thruster name expressions in the actuator configuration. + + Example: + ``["rotor_0", "rotor_1", "rotor_2", "rotor_3"]`` for a quadcopter + """ + + default_thruster_rps: torch.Tensor = None + """Default thruster RPS (revolutions per second) state of all thrusters. Shape is (num_instances, num_thrusters). + + This quantity is configured through the :attr:`MultirotorCfg.init_state.rps` parameter + and represents the baseline/hover RPS for each thruster. It is used to initialize + thruster states during reset operations. + + For a hovering multirotor, these values should produce enough collective thrust + to counteract gravity. + + Example: + For a 1kg quadcopter with 4 thrusters, if each thruster produces 2.5N at 110 RPS, + the default might be ``[[110.0, 110.0, 110.0, 110.0]]`` for hover. + """ + + thrust_target: torch.Tensor = None + """Thrust targets commanded by the user or controller. Shape is ``(num_instances, num_thrusters)`` + + This quantity contains the target thrust values set through the + :meth:`~isaaclab_contrib.assets.Multirotor.set_thrust_target` method or by + action terms in RL environments. These targets are processed by the thruster + actuator models to compute actual applied thrusts. + + The units depend on the actuator model configuration (typically Newtons for + force or RPS for rotational speed). + """ + + ## + # Thruster commands + ## + + computed_thrust: torch.Tensor = None + """Computed thrust from the actuator model before clipping. Shape is (num_instances, num_thrusters). + + This quantity contains the thrust values computed by the thruster actuator models + before any clipping or saturation is applied. It represents the "desired" thrust + based on the actuator dynamics (rise/fall times) but may exceed physical limits. + + The difference between :attr:`computed_thrust` and :attr:`applied_thrust` indicates + when the actuator is saturating at its limits. + + Example Use: + Monitor actuator saturation by comparing computed vs applied thrust: + + .. code-block:: python + + saturation = multirotor.data.computed_thrust - multirotor.data.applied_thrust + is_saturated = saturation.abs() > 1e-6 + """ + + applied_thrust: torch.Tensor = None + """Applied thrust from the actuator model after clipping. Shape is (num_instances, num_thrusters). + + This quantity contains the final thrust values that are actually applied to the + simulation after all actuator model processing, including: + + - Dynamic response (rise/fall time constants) + - Clipping to thrust range limits + - Any other actuator model constraints + + This is the "ground truth" thrust that affects the multirotor's motion in the + physics simulation. + """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/mdp/__init__.py new file mode 100644 index 000000000000..bc099b36f648 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/__init__.py @@ -0,0 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for MDP (Markov Decision Process) components contributed by the community.""" + +from .actions import * # noqa: F401, F403 diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/__init__.py new file mode 100644 index 000000000000..695a4486066f --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Action terms for multirotor control. + +This module provides action terms specifically designed for controlling multirotor +vehicles through thrust commands. These action terms integrate with Isaac Lab's +MDP framework and :class:`~isaaclab_contrib.assets.Multirotor` assets. +""" + +from .thrust_actions import * # noqa: F401, F403 +from .thrust_actions_cfg import * # noqa: F401, F403 diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions.py b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions.py new file mode 100644 index 000000000000..7aa207849de6 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions.py @@ -0,0 +1,246 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.string as string_utils +from isaaclab.managers.action_manager import ActionTerm + +from isaaclab_contrib.assets import Multirotor + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor + + from . import thrust_actions_cfg + +# import logger +logger = logging.getLogger(__name__) + + +class ThrustAction(ActionTerm): + """Thrust action term that applies the processed actions as thrust commands. + + This action term is designed specifically for controlling multirotor vehicles by mapping + action inputs to thruster commands. It provides flexible preprocessing of actions through: + + - **Scaling**: Multiply actions by a scale factor to adjust command magnitudes + - **Offset**: Add an offset to center actions around a baseline (e.g., hover thrust) + - **Clipping**: Constrain actions to valid ranges to prevent unsafe commands + + The action term integrates with Isaac Lab's :class:`~isaaclab.managers.ActionManager` + framework and is specifically designed to work with :class:`~isaaclab_contrib.assets.Multirotor` + assets. + + Key Features: + - Supports per-thruster or uniform scaling and offsets + - Optional automatic offset computation based on hover thrust + - Action clipping for safety and constraint enforcement + - Regex-based thruster selection for flexible control schemes + + Example: + .. code-block:: python + + from isaaclab.envs import ManagerBasedRLEnvCfg + from isaaclab_contrib.mdp.actions import ThrustActionCfg + + + @configclass + class MyEnvCfg(ManagerBasedRLEnvCfg): + # ... other configuration ... + + @configclass + class ActionsCfg: + # Direct thrust control (normalized actions) + thrust = ThrustActionCfg( + asset_name="robot", + scale=5.0, # Convert [-1, 1] to [-5, 5] N + use_default_offset=True, # Add hover thrust as offset + clip={".*": (-2.0, 8.0)}, # Clip to safe thrust range + ) + + """ + + cfg: thrust_actions_cfg.ThrustActionCfg + """The configuration of the action term.""" + _asset: Multirotor + """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: thrust_actions_cfg.ThrustActionCfg, env: ManagerBasedEnv) -> None: + # initialize the action term + super().__init__(cfg, env) + + thruster_names_expr = self._asset.actuators["thrusters"].cfg.thruster_names_expr + + # resolve the thrusters over which the action term is applied + self._thruster_ids, self._thruster_names = self._asset.find_bodies( + thruster_names_expr, preserve_order=self.cfg.preserve_order + ) + self._num_thrusters = len(self._thruster_ids) + # log the resolved thruster names for debugging + logger.info( + f"Resolved thruster names for the action term {self.__class__.__name__}:" + f" {self._thruster_names} [{self._thruster_ids}]" + ) + + # Avoid indexing across all thrusters for efficiency + if self._num_thrusters == self._asset.num_thrusters and not self.cfg.preserve_order: + self._thruster_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._thruster_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._thruster_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 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._thruster_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.") + + # Handle use_default_offset + if cfg.use_default_offset: + # Use default thruster RPS as offset + self._offset = self._asset.data.default_thruster_rps[:, self._thruster_ids].clone() + + """ + Properties + """ + + @property + def action_dim(self) -> int: + return self._num_thrusters + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term.""" + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "ThrustAction" + self._IO_descriptor.thruster_names = self._thruster_names + self._IO_descriptor.scale = self._scale + if isinstance(self._offset, torch.Tensor): + self._IO_descriptor.offset = self._offset[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.offset = self._offset + if self.cfg.clip is not None: + if isinstance(self._clip, torch.Tensor): + self._IO_descriptor.clip = self._clip[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.clip = self._clip + else: + self._IO_descriptor.clip = None + return self._IO_descriptor + + """ + Methods + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Reset the action term. + + This method resets the raw actions to zero for the specified environments. + The processed actions will be recomputed during the next :meth:`process_actions` call. + + Args: + env_ids: Environment indices to reset. Defaults to None (all environments). + """ + self._raw_actions[env_ids] = 0.0 + + def process_actions(self, actions: torch.Tensor): + r"""Process actions by applying scaling, offset, and clipping. + + This method transforms raw policy actions into thrust commands through + an affine transformation followed by optional clipping. The transformation is: + + .. math:: + \text{processed} = \text{raw} \times \text{scale} + \text{offset} + + If clipping is configured, the processed actions are then clamped: + + .. math:: + \text{processed} = \text{clamp}(\text{processed}, \text{min}, \text{max}) + + Args: + actions: Raw action tensor from the policy. Shape is ``(num_envs, action_dim)``. + Typically in the range [-1, 1] for normalized policies. + + Note: + The processed actions are stored internally and applied during the next + :meth:`apply_actions` call. + """ + # 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 apply_actions(self): + """Apply the processed actions as thrust commands. + + This method sets the processed actions as thrust targets on the multirotor + asset. The thrust targets are then used by the thruster actuator models + to compute actual thrust forces during the simulation step. + + The method calls :meth:`~isaaclab_contrib.assets.Multirotor.set_thrust_target` + on the multirotor asset with the appropriate thruster IDs. + """ + # Set thrust targets using thruster IDs + self._asset.set_thrust_target(self.processed_actions, thruster_ids=self._thruster_ids) diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions_cfg.py new file mode 100644 index 000000000000..0f457fe4a5a0 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions_cfg.py @@ -0,0 +1,168 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + +from . import thrust_actions + + +@configclass +class ThrustActionCfg(ActionTermCfg): + """Configuration for the thrust action term. + + This configuration class specifies how policy actions are transformed into thruster + commands for multirotor control. It provides extensive customization of the action + processing pipeline including scaling, offsetting, and clipping. + + The action term is designed to work with :class:`~isaaclab_contrib.assets.Multirotor` + assets and uses their thruster configuration to determine which thrusters to control. + + Key Configuration Options: + - **scale**: Multiplies raw actions to adjust command magnitude + - **offset**: Adds a baseline value (e.g., hover thrust) to actions + - **clip**: Constrains actions to safe operational ranges + - **use_default_offset**: Automatically uses hover thrust as offset + + Example Configurations: + **Normalized thrust control around hover**: + + .. code-block:: python + + thrust_action = ThrustActionCfg( + asset_name="robot", + scale=2.0, # Actions in [-1,1] become [-2,2] N + use_default_offset=True, # Add hover thrust (e.g., 5N) + clip={".*": (0.0, 10.0)}, # Final thrust in [0, 10] N + ) + + **Direct thrust control with per-thruster scaling**: + + .. code-block:: python + + thrust_action = ThrustActionCfg( + asset_name="robot", + scale={ + "rotor_[0-1]": 8.0, # Front rotors: stronger + "rotor_[2-3]": 7.0, # Rear rotors: weaker + }, + offset=0.0, + use_default_offset=False, + ) + + **Differential thrust control**: + + .. code-block:: python + + thrust_action = ThrustActionCfg( + asset_name="robot", + scale=3.0, + use_default_offset=True, # Center around hover + clip={".*": (-2.0, 8.0)}, # Allow +/-2N deviation + ) + + .. seealso:: + - :class:`~isaaclab_contrib.mdp.actions.ThrustAction`: Implementation of this action term + - :class:`~isaaclab.managers.ActionTermCfg`: Base action term configuration + """ + + class_type: type[ActionTerm] = thrust_actions.ThrustAction + + asset_name: str = MISSING + """Name or regex expression of the asset that the action will be mapped to. + + This should match the name given to the multirotor asset in the scene configuration. + For example, if the robot is defined as ``robot = MultirotorCfg(...)``, then + ``asset_name`` should be ``"robot"``. + """ + + scale: float | dict[str, float] = 1.0 + """Scale factor for the action. Default is ``1.0``, which means no scaling. + + This multiplies the raw action values to adjust the command magnitude. It can be: + + - A float: uniform scaling for all thrusters (e.g., ``2.0``) + - A dict: per-thruster scaling using regex patterns (e.g., ``{"rotor_.*": 2.5}``) + + For normalized actions in [-1, 1], the scale determines the maximum deviation + from the offset value. + + Example: + .. code-block:: python + + # Uniform scaling + scale = 5.0 # Actions of ยฑ1 become ยฑ5N + + # Per-thruster scaling + scale = { + "rotor_[0-1]": 8.0, # Front rotors + "rotor_[2-3]": 6.0, # Rear rotors + } + """ + + offset: float | dict[str, float] = 0.0 + """Offset factor for the action. Default is ``0.0``, which means no offset. + + This value is added to the scaled actions to establish a baseline thrust. + It can be: + + - A float: uniform offset for all thrusters (e.g., ``5.0`` for 5N hover thrust) + - A dict: per-thruster offset using regex patterns + + If :attr:`use_default_offset` is ``True``, this value is overwritten by the + default thruster RPS from the multirotor configuration. + + Example: + .. code-block:: python + + # Uniform offset (5N baseline thrust) + offset = 5.0 + + # Per-thruster offset + offset = { + "rotor_0": 5.2, + "rotor_1": 4.8, + } + """ + + clip: dict[str, tuple[float, float]] | None = None + """Clipping ranges for processed actions. Default is ``None``, which means no clipping. + + This constrains the final thrust commands to safe operational ranges after + scaling and offset are applied. It must be specified as a dictionary mapping + regex patterns to (min, max) tuples. + + Example: + .. code-block:: python + + # Clip all thrusters to [0, 10] N + clip = {".*": (0.0, 10.0)} + + # Different limits for different thrusters + clip = { + "rotor_[0-1]": (0.0, 12.0), # Front rotors + "rotor_[2-3]": (0.0, 8.0), # Rear rotors + } + + """ + + preserve_order: bool = False + """Whether to preserve the order of the asset names in the action output. Default is ``False``. + + If ``True``, the thruster ordering matches the regex pattern order exactly. + If ``False``, ordering is determined by the USD scene traversal order. + """ + + use_default_offset: bool = True + """Whether to use default thrust configured in the multirotor asset as offset. Default is ``True``. + + If ``True``, the :attr:`offset` value is overwritten with the default thruster + RPS values from :attr:`MultirotorCfg.init_state.rps`. This is useful for + controlling thrust as deviations from the hover state. + + If ``False``, the manually specified :attr:`offset` value is used. + """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/sensors/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/sensors/__init__.py new file mode 100644 index 000000000000..a7ea884318a3 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/sensors/__init__.py @@ -0,0 +1,25 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for externally contributed sensors. + +This package provides specialized sensor classes for simulating externally contributed +sensors in Isaac Lab. These sensors are not part of the core Isaac Lab framework yet, +but are planned to be added in the future. They are contributed by the community to +extend the capabilities of Isaac Lab. + +Following the categorization in :mod:`isaaclab.sensors` sub-package, 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 | ++=====================+===========================+===============================================================+ +| Visuo-Tactile Sensor| /World/robot/base | Leaf exists and is a physics body (Rigid Body) | ++---------------------+---------------------------+---------------------------------------------------------------+ + +""" + +from .tacsl_sensor import * diff --git a/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/__init__.py new file mode 100644 index 000000000000..869b233d166d --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""TacSL Tactile Sensor implementation for IsaacLab.""" + +from .visuotactile_sensor import VisuoTactileSensor +from .visuotactile_sensor_cfg import GelSightRenderCfg, VisuoTactileSensorCfg +from .visuotactile_sensor_data import VisuoTactileSensorData diff --git a/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_render.py b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_render.py new file mode 100644 index 000000000000..27d21d03736c --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_render.py @@ -0,0 +1,293 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +import os +from typing import TYPE_CHECKING + +import cv2 +import numpy as np +import scipy +import torch + +from isaaclab.utils.assets import retrieve_file_path + +logger = logging.getLogger(__name__) + + +if TYPE_CHECKING: + from .visuotactile_sensor_cfg import GelSightRenderCfg + + +def compute_tactile_shear_image( + tactile_normal_force: np.ndarray, + tactile_shear_force: np.ndarray, + normal_force_threshold: float = 0.00008, + shear_force_threshold: float = 0.0005, + resolution: int = 30, +) -> np.ndarray: + """Visualize the tactile shear field. + + This function creates a visualization of tactile forces using arrows to represent shear forces + and color coding to represent normal forces. The thresholds are used to normalize forces for + visualization, chosen empirically to provide clear visual representation. + + Args: + tactile_normal_force: Array of tactile normal forces. Shape: (H, W). + tactile_shear_force: Array of tactile shear forces. Shape: (H, W, 2). + normal_force_threshold: Threshold for normal force visualization. Defaults to 0.00008. + shear_force_threshold: Threshold for shear force visualization. Defaults to 0.0005. + resolution: Resolution for the visualization. Defaults to 30. + + Returns: + Image visualizing the tactile shear forces. Shape: (H * resolution, W * resolution, 3). + """ + nrows = tactile_normal_force.shape[0] + ncols = tactile_normal_force.shape[1] + + imgs_tactile = np.zeros((nrows * resolution, ncols * resolution, 3), dtype=float) + + for row in range(nrows): + for col in range(ncols): + loc0_x = row * resolution + resolution // 2 + loc0_y = col * resolution + resolution // 2 + loc1_x = loc0_x + tactile_shear_force[row, col][0] / shear_force_threshold * resolution + loc1_y = loc0_y + tactile_shear_force[row, col][1] / shear_force_threshold * resolution + color = ( + 0.0, + max(0.0, 1.0 - tactile_normal_force[row][col] / normal_force_threshold), + min(1.0, tactile_normal_force[row][col] / normal_force_threshold), + ) + + cv2.arrowedLine( + imgs_tactile, (int(loc0_y), int(loc0_x)), (int(loc1_y), int(loc1_x)), color, 6, tipLength=0.4 + ) + + return imgs_tactile + + +def compute_penetration_depth( + penetration_depth_img: np.ndarray, resolution: int = 5, depth_multiplier: float = 300.0 +) -> np.ndarray: + """Visualize the penetration depth. + + Args: + penetration_depth_img: Image of penetration depth. Shape: (H, W). + resolution: Resolution for the upsampling; each pixel expands to a (res x res) block. Defaults to 5. + depth_multiplier: Multiplier for the depth values. Defaults to 300.0 (scales ~3.3mm to 1.0). + (e.g. typical Gelsight sensors have maximum penetration depths < 2.5mm, + see https://dspace.mit.edu/handle/1721.1/114627). + + Returns: + Upsampled image visualizing the penetration depth. Shape: (H * resolution, W * resolution). + """ + # penetration_depth_img_upsampled = penetration_depth.repeat(resolution, 0).repeat(resolution, 1) + penetration_depth_img_upsampled = np.kron(penetration_depth_img, np.ones((resolution, resolution))) + penetration_depth_img_upsampled = np.clip(penetration_depth_img_upsampled, 0.0, 1.0) * depth_multiplier + return penetration_depth_img_upsampled + + +class GelsightRender: + """Class to handle GelSight rendering using the Taxim example-based approach from :cite:t:`si2022taxim`. + + Reference: + Si, Z., & Yuan, W. (2022). Taxim: An example-based simulation model for GelSight + tactile sensors. IEEE Robotics and Automation Letters, 7(2), 2361-2368. + https://arxiv.org/abs/2109.04027 + """ + + def __init__(self, cfg: GelSightRenderCfg, device: str | torch.device): + """Initialize the GelSight renderer. + + Args: + cfg: Configuration object for the GelSight sensor. + device: Device to use ('cpu' or 'cuda'). + + Raises: + ValueError: If :attr:`GelSightRenderCfg.mm_per_pixel` is zero or negative. + FileNotFoundError: If render data files cannot be retrieved. + """ + self.cfg = cfg + self.device = device + + # Validate configuration parameters + eps = 1e-9 + if self.cfg.mm_per_pixel < eps: + raise ValueError(f"Input 'mm_per_pixel' must be positive (>= {eps}), got {self.cfg.mm_per_pixel}") + + # Retrieve render data files using the configured base path + bg_path = self._get_render_data(self.cfg.sensor_data_dir_name, self.cfg.background_path) + calib_path = self._get_render_data(self.cfg.sensor_data_dir_name, self.cfg.calib_path) + + if bg_path is None or calib_path is None: + raise FileNotFoundError( + "Failed to retrieve GelSight render data files. " + f"Base path: {self.cfg.base_data_path or 'default (Isaac Lab Nucleus)'}, " + f"Data dir: {self.cfg.sensor_data_dir_name}" + ) + + self.background = cv2.cvtColor(cv2.imread(bg_path), cv2.COLOR_BGR2RGB) + + # Load calibration data directly + calib_data = np.load(calib_path) + calib_grad_r = calib_data["grad_r"] + calib_grad_g = calib_data["grad_g"] + calib_grad_b = calib_data["grad_b"] + + image_height = self.cfg.image_height + image_width = self.cfg.image_width + num_bins = self.cfg.num_bins + [xx, yy] = np.meshgrid(range(image_width), range(image_height)) + xf = xx.flatten() + yf = yy.flatten() + self.A = np.array([xf * xf, yf * yf, xf * yf, xf, yf, np.ones(image_height * image_width)]).T + + binm = num_bins - 1 + self.x_binr = 0.5 * np.pi / binm # x [0,pi/2] + self.y_binr = 2 * np.pi / binm # y [-pi, pi] + + kernel = self._get_filtering_kernel(kernel_size=5) + self.kernel = torch.tensor(kernel, dtype=torch.float, device=self.device) + + self.calib_data_grad_r = torch.tensor(calib_grad_r, device=self.device) + self.calib_data_grad_g = torch.tensor(calib_grad_g, device=self.device) + self.calib_data_grad_b = torch.tensor(calib_grad_b, device=self.device) + + self.A_tensor = torch.tensor(self.A.reshape(image_height, image_width, 6), device=self.device).unsqueeze(0) + self.background_tensor = torch.tensor(self.background, device=self.device) + + # Pre-allocate buffer for RGB output (will be resized if needed) + self._sim_img_rgb_buffer = torch.empty((1, image_height, image_width, 3), device=self.device) + + logger.info("Gelsight renderer initialization done!") + + def render(self, height_map: torch.Tensor) -> torch.Tensor: + """Render the height map using the GelSight sensor. + + Args: + height_map: Input height map tensor. Shape is (N, H, W). + + Returns: + Rendered image tensor. Shape is (N, H, W, 3). + """ + height_map = height_map.clone() + height_map[torch.abs(height_map) < 1e-6] = 0 # remove minor artifact + height_map = height_map * -1000.0 + height_map /= self.cfg.mm_per_pixel + + height_map = self._gaussian_filtering(height_map.unsqueeze(-1), self.kernel).squeeze(-1) + + grad_mag, grad_dir = self._generate_normals(height_map) + + idx_x = torch.floor(grad_mag / self.x_binr).long() + idx_y = torch.floor((grad_dir + np.pi) / self.y_binr).long() + + # Clamp indices to valid range to prevent out-of-bounds errors + max_idx = self.cfg.num_bins - 1 + idx_x = torch.clamp(idx_x, 0, max_idx) + idx_y = torch.clamp(idx_y, 0, max_idx) + + params_r = self.calib_data_grad_r[idx_x, idx_y, :] + params_g = self.calib_data_grad_g[idx_x, idx_y, :] + params_b = self.calib_data_grad_b[idx_x, idx_y, :] + + # Reuse pre-allocated buffer, resize if batch size changed + target_shape = (*idx_x.shape, 3) + if self._sim_img_rgb_buffer.shape != target_shape: + self._sim_img_rgb_buffer = torch.empty(target_shape, device=self.device) + sim_img_rgb = self._sim_img_rgb_buffer + + sim_img_rgb[..., 0] = torch.sum(self.A_tensor * params_r, dim=-1) # R + sim_img_rgb[..., 1] = torch.sum(self.A_tensor * params_g, dim=-1) # G + sim_img_rgb[..., 2] = torch.sum(self.A_tensor * params_b, dim=-1) # B + + # write tactile image + sim_img = sim_img_rgb + self.background_tensor # /255.0 + sim_img = torch.clip(sim_img, 0, 255, out=sim_img).to(torch.uint8) + return sim_img + + """ + Internal Helpers. + """ + + def _get_render_data(self, data_dir: str, file_name: str) -> str: + """Gets the path for the GelSight render data file. + + Args: + data_dir: The data directory name containing the render data. + file_name: The specific file name to retrieve. + + Returns: + The local path to the file. + + Raises: + FileNotFoundError: If the file is not found locally or on Nucleus. + """ + # Construct path using the configured base path + file_path = os.path.join(self.cfg.base_data_path, data_dir, file_name) + + # Cache directory for downloads + cache_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), data_dir) + + # Use retrieve_file_path to handle local/Nucleus paths and caching + return retrieve_file_path(file_path, download_dir=cache_dir, force_download=False) + + def _generate_normals(self, img: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Generate the gradient magnitude and direction of the height map. + + Args: + img: Input height map tensor. Shape: (N, H, W). + + Returns: + Tuple containing gradient magnitude tensor and gradient direction tensor. Shape: (N, H, W). + """ + img_grad = torch.gradient(img, dim=(1, 2)) + dzdx, dzdy = img_grad + + grad_mag_orig = torch.sqrt(dzdx**2 + dzdy**2) + grad_mag = torch.arctan(grad_mag_orig) # seems that arctan is used as a squashing function + grad_dir = torch.arctan2(dzdx, dzdy) + grad_dir[grad_mag_orig == 0] = 0 + + # handle edges + grad_mag = torch.nn.functional.pad(grad_mag[:, 1:-1, 1:-1], pad=(1, 1, 1, 1)) + grad_dir = torch.nn.functional.pad(grad_dir[:, 1:-1, 1:-1], pad=(1, 1, 1, 1)) + + return grad_mag, grad_dir + + def _get_filtering_kernel(self, kernel_size: int = 5) -> np.ndarray: + """Create a Gaussian filtering kernel. + + For kernel derivation, see https://cecas.clemson.edu/~stb/ece847/internal/cvbook/ch03_filtering.pdf + + Args: + kernel_size: Size of the kernel. Defaults to 5. + + Returns: + Filtering kernel. Shape is (kernel_size, kernel_size). + """ + filter_1D = scipy.special.binom(kernel_size - 1, np.arange(kernel_size)) + filter_1D /= filter_1D.sum() + filter_1D = filter_1D[..., None] + + kernel = filter_1D @ filter_1D.T + return kernel + + def _gaussian_filtering(self, img: torch.Tensor, kernel: torch.Tensor) -> torch.Tensor: + """Apply Gaussian filtering to the input image tensor. + + Args: + img: Input image tensor. Shape is (N, H, W, 1). + kernel: Filtering kernel tensor. Shape is (K, K). + + Returns: + Filtered image tensor. Shape is (N, H, W, 1). + """ + img_output = torch.nn.functional.conv2d( + img.permute(0, 3, 1, 2), kernel.unsqueeze(0).unsqueeze(0), stride=1, padding="same" + ).permute(0, 2, 3, 1) + return img_output diff --git a/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor.py b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor.py new file mode 100644 index 000000000000..c08d5fe53381 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor.py @@ -0,0 +1,913 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import itertools +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import numpy as np +import torch + +import isaacsim.core.utils.torch as torch_utils +from isaacsim.core.simulation_manager import SimulationManager +from pxr import Usd, UsdGeom, UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.markers import VisualizationMarkers +from isaaclab.sensors.camera import Camera, TiledCamera +from isaaclab.sensors.sensor_base import SensorBase + +from .visuotactile_render import GelsightRender +from .visuotactile_sensor_data import VisuoTactileSensorData + +if TYPE_CHECKING: + from .visuotactile_sensor_cfg import VisuoTactileSensorCfg + +import trimesh + +logger = logging.getLogger(__name__) + + +class VisuoTactileSensor(SensorBase): + r"""A tactile sensor for both camera-based and force field tactile sensing. + + This sensor provides: + 1. Camera-based tactile sensing: depth images from tactile surface + 2. Force field tactile sensing: Penalty-based normal and shear forces using SDF queries + + The sensor can be configured to use either or both sensing modalities. + + **Computation Pipeline:** + Camera-based sensing computes depth differences from a nominal (no-contact) baseline and + processes them through the tac-sl GelSight renderer to produce realistic tactile images. + + Force field sensing queries Signed Distance Fields (SDF) to compute penetration depths, + then applies penalty-based spring-damper models + (:math:`F_n = k_n \cdot \text{depth}`, :math:`F_t = \min(k_t \cdot \|v_t\|, \mu \cdot F_n)`) + to compute normal and shear forces at discrete tactile points. + + **Example Usage:** + For a complete working example, see: ``scripts/demos/sensors/tacsl/tacsl_example.py`` + + **Current Limitations:** + - SDF collision meshes must be pre-computed and objects specified before simulation starts + - Force field computation requires specific rigid body and mesh configurations + - No support for dynamic addition/removal of interacting objects during runtime + + Configuration Requirements: + The following requirements must be satisfied for proper sensor operation: + + **Camera Tactile Imaging** + If ``enable_camera_tactile=True``, a valid ``camera_cfg`` (TiledCameraCfg) must be + provided with appropriate camera parameters. + + **Force Field Computation** + If ``enable_force_field=True``, the following parameters are required: + + * ``contact_object_prim_path_expr`` - Prim path expression to find the contact object prim + + **SDF Computation** + When force field computation is enabled, penalty-based normal and shear forces are + computed using Signed Distance Field (SDF) queries. To achieve GPU acceleration: + + * Interacting objects should have pre-computed SDF collision meshes + * An SDFView must be defined during initialization, therefore interacting objects + should be specified before simulation. + + """ + + cfg: VisuoTactileSensorCfg + """The configuration parameters.""" + + def __init__(self, cfg: VisuoTactileSensorCfg): + """Initializes the tactile sensor object. + + Args: + cfg: The configuration parameters. + """ + + # Create empty variables for storing output data + self._data: VisuoTactileSensorData = VisuoTactileSensorData() + + # Camera-based tactile sensing + self._camera_sensor: Camera | TiledCamera | None = None + self._nominal_tactile: dict | None = None + + # Force field tactile sensing + self._tactile_pos_local: torch.Tensor | None = None + self._tactile_quat_local: torch.Tensor | None = None + self._sdf_object: Any | None = None + + # COMs for velocity correction + self._elastomer_com_b: torch.Tensor | None = None + self._contact_object_com_b: torch.Tensor | None = None + + # Physics views + self._physics_sim_view = None + self._elastomer_body_view = None + self._elastomer_tip_view = None + self._contact_object_body_view = None + + # Visualization + self._tactile_visualizer: VisualizationMarkers | None = None + + # Tactile points count + self.num_tactile_points: int = 0 + + # Now call parent class constructor + super().__init__(cfg) + + def __del__(self): + """Unsubscribes from callbacks and detach from the replicator registry.""" + if self._camera_sensor is not None: + self._camera_sensor.__del__() + # unsubscribe from callbacks + super().__del__() + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Tactile sensor @ '{self.cfg.prim_path}': \n" + f"\trender config : {self.cfg.render_cfg.base_data_path}/{self.cfg.render_cfg.sensor_data_dir_name}\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tcamera enabled : {self.cfg.enable_camera_tactile}\n" + f"\tforce field enabled: {self.cfg.enable_force_field}\n" + f"\tnum instances : {self.num_instances}\n" + ) + + """ + Properties + """ + + @property + def num_instances(self) -> int: + return self._num_envs + + @property + def data(self) -> VisuoTactileSensorData: + # Update sensors if needed + self._update_outdated_buffers() + # Return the data + return self._data + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + """Resets the sensor internals.""" + # reset the timestamps + super().reset(env_ids) + + # Reset camera sensor if enabled + if self._camera_sensor: + self._camera_sensor.reset(env_ids) + + """ + Implementation + """ + + def _initialize_impl(self): + """Initializes the sensor-related handles and internal buffers.""" + super()._initialize_impl() + + # Obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + + # Initialize camera-based tactile sensing + if self.cfg.enable_camera_tactile: + self._initialize_camera_tactile() + + # Initialize force field tactile sensing + if self.cfg.enable_force_field: + self._initialize_force_field() + + # Initialize visualization + if self.cfg.debug_vis: + self._initialize_visualization() + + def get_initial_render(self) -> dict | None: + """Get the initial tactile sensor render for baseline comparison. + + This method captures the initial state of the tactile sensor when no contact + is occurring. This baseline is used for computing relative changes during + tactile interactions. + + .. warning:: + It is the user's responsibility to ensure that the sensor is in a "no contact" state + when this method is called. If the sensor is in contact with an object, the baseline + will be incorrect, leading to erroneous tactile readings. + + Returns: + dict | None: Dictionary containing initial render data with sensor output keys + and corresponding tensor values. Returns None if camera tactile + sensing is disabled. + + Raises: + RuntimeError: If camera sensor is not initialized or initial render fails. + """ + if not self.cfg.enable_camera_tactile: + return None + + self._camera_sensor.update(dt=0.0) + + # get the initial render + initial_render = self._camera_sensor.data.output + if initial_render is None: + raise RuntimeError("Initial render is None") + + # Store the initial nominal tactile data + self._nominal_tactile = dict() + for key, value in initial_render.items(): + self._nominal_tactile[key] = value.clone() + + return self._nominal_tactile + + def _initialize_camera_tactile(self): + """Initialize camera-based tactile sensing.""" + if self.cfg.camera_cfg is None: + raise ValueError("Camera configuration is None. Please provide a valid camera configuration.") + # check image size is consistent with the render config + if ( + self.cfg.camera_cfg.height != self.cfg.render_cfg.image_height + or self.cfg.camera_cfg.width != self.cfg.render_cfg.image_width + ): + raise ValueError( + "Camera configuration image size is not consistent with the render config. Camera size:" + f" {self.cfg.camera_cfg.height}x{self.cfg.camera_cfg.width}, Render config:" + f" {self.cfg.render_cfg.image_height}x{self.cfg.render_cfg.image_width}" + ) + # check data types + if not all(data_type in ["distance_to_image_plane", "depth"] for data_type in self.cfg.camera_cfg.data_types): + raise ValueError( + f"Camera configuration data types are not supported. Data types: {self.cfg.camera_cfg.data_types}" + ) + if self.cfg.camera_cfg.update_period != self.cfg.update_period: + logger.warning( + f"Camera configuration update period ({self.cfg.camera_cfg.update_period}) is not equal to sensor" + f" update period ({self.cfg.update_period}), changing camera update period to match sensor update" + " period" + ) + self.cfg.camera_cfg.update_period = self.cfg.update_period + + # gelsightRender + self._tactile_rgb_render = GelsightRender(self.cfg.render_cfg, device=self.device) + + # Create camera sensor + self._camera_sensor = TiledCamera(self.cfg.camera_cfg) + + # Initialize camera + if not self._camera_sensor.is_initialized: + self._camera_sensor._initialize_impl() + self._camera_sensor._is_initialized = True + + # Initialize camera buffers + self._data.tactile_rgb_image = torch.zeros( + (self._num_envs, self.cfg.camera_cfg.height, self.cfg.camera_cfg.width, 3), device=self._device + ) + self._data.tactile_depth_image = torch.zeros( + (self._num_envs, self.cfg.camera_cfg.height, self.cfg.camera_cfg.width, 1), device=self._device + ) + + logger.info("Camera-based tactile sensing initialized.") + + def _initialize_force_field(self): + """Initialize force field tactile sensing components. + + This method sets up all components required for force field based tactile sensing: + + 1. Creates PhysX views for elastomer and contact object rigid bodies + 2. Generates tactile sensing points on the elastomer surface using mesh geometry + 3. Initializes SDF (Signed Distance Field) for collision detection + 4. Creates data buffers for storing force field measurements + + The tactile points are generated by ray-casting onto the elastomer mesh surface + to create a grid of sensing points that will be used for force computation. + + """ + + # Generate tactile points on elastomer surface + self._generate_tactile_points( + num_divs=list(self.cfg.tactile_array_size), + margin=getattr(self.cfg, "tactile_margin", 0.003), + visualize=self.cfg.trimesh_vis_tactile_points, + ) + + self._create_physx_views() + + # Initialize force field data buffers + self._initialize_force_field_buffers() + logger.info("Force field tactile sensing initialized.") + + def _create_physx_views(self) -> None: + """Create PhysX views for contact object and elastomer bodies. + + This method sets up the necessary PhysX views for force field computation: + 1. Creates rigid body view for elastomer + 2. If contact object prim path expression is not None, then: + a. Finds and validates the object prim and its collision mesh + b. Creates SDF view for collision detection + c. Creates rigid body view for object + + """ + elastomer_pattern = self._parent_prims[0].GetPath().pathString.replace("env_0", "env_*") + self._elastomer_body_view = self._physics_sim_view.create_rigid_body_view([elastomer_pattern]) + # Get elastomer COM for velocity correction + self._elastomer_com_b = self._elastomer_body_view.get_coms().to(self._device).split([3, 4], dim=-1)[0] + + if self.cfg.contact_object_prim_path_expr is None: + return + + contact_object_mesh, contact_object_rigid_body = self._find_contact_object_components() + # Create SDF view for collision detection + num_query_points = self.cfg.tactile_array_size[0] * self.cfg.tactile_array_size[1] + mesh_path_pattern = contact_object_mesh.GetPath().pathString.replace("env_0", "env_*") + self._contact_object_sdf_view = self._physics_sim_view.create_sdf_shape_view( + mesh_path_pattern, num_query_points + ) + + # Create rigid body views for contact object and elastomer + body_path_pattern = contact_object_rigid_body.GetPath().pathString.replace("env_0", "env_*") + self._contact_object_body_view = self._physics_sim_view.create_rigid_body_view([body_path_pattern]) + # Get contact object COM for velocity correction + self._contact_object_com_b = self._contact_object_body_view.get_coms().to(self._device).split([3, 4], dim=-1)[0] + + def _find_contact_object_components(self) -> tuple[Any, Any]: + """Find and validate contact object SDF mesh and its parent rigid body. + + This method searches for the contact object prim using the configured filter pattern, + then locates the first SDF collision mesh within that prim hierarchy and + identifies its parent rigid body for physics simulation. + + Returns: + Tuple of (contact_object_mesh, contact_object_rigid_body) + Returns None if contact object components are not found. + + Note: + Only SDF meshes are supported for optimal force field computation performance. + If no SDF mesh is found, the method will log a warning and return None. + """ + # Find the contact object prim using the configured pattern + contact_object_prim = sim_utils.find_first_matching_prim(self.cfg.contact_object_prim_path_expr) + if contact_object_prim is None: + raise RuntimeError( + f"No contact object prim found matching pattern: {self.cfg.contact_object_prim_path_expr}" + ) + + def is_sdf_mesh(prim: Usd.Prim) -> bool: + """Check if a mesh prim is configured for SDF approximation.""" + return ( + prim.HasAPI(UsdPhysics.MeshCollisionAPI) + and UsdPhysics.MeshCollisionAPI(prim).GetApproximationAttr().Get() == "sdf" + ) + + # Find the SDF mesh within the contact object + contact_object_mesh = sim_utils.get_first_matching_child_prim( + contact_object_prim.GetPath(), predicate=is_sdf_mesh + ) + if contact_object_mesh is None: + raise RuntimeError( + f"No SDF mesh found under contact object at path: {contact_object_prim.GetPath().pathString}" + ) + + def find_parent_rigid_body(prim: Usd.Prim) -> Usd.Prim | None: + """Find the first parent prim with RigidBodyAPI.""" + current_prim = prim + while current_prim and current_prim.IsValid(): + if current_prim.HasAPI(UsdPhysics.RigidBodyAPI): + return current_prim + current_prim = current_prim.GetParent() + if current_prim.GetPath() == "/": + break + return None + + # Find the rigid body parent of the SDF mesh + contact_object_rigid_body = find_parent_rigid_body(contact_object_mesh) + if contact_object_rigid_body is None: + raise RuntimeError( + f"No contact object rigid body found for mesh at path: {contact_object_mesh.GetPath().pathString}" + ) + + return contact_object_mesh, contact_object_rigid_body + + def _generate_tactile_points(self, num_divs: list, margin: float, visualize: bool): + """Generate tactile sensing points from elastomer mesh geometry. + + This method creates a grid of tactile sensing points on the elastomer surface + by ray-casting onto the mesh geometry. Visual meshes are used for smoother point sampling. + + Args: + num_divs: Number of divisions [rows, cols] for the tactile grid. + margin: Margin distance from mesh edges in meters. + visualize: Whether to show the generated points in trimesh visualization. + + """ + + # Get the elastomer prim path + elastomer_prim_path = self._parent_prims[0].GetPath().pathString + + def is_visual_mesh(prim) -> bool: + """Check if a mesh prim has visual properties (visual mesh, not collision mesh).""" + return prim.IsA(UsdGeom.Mesh) and not prim.HasAPI(UsdPhysics.CollisionAPI) + + elastomer_mesh_prim = sim_utils.get_first_matching_child_prim(elastomer_prim_path, predicate=is_visual_mesh) + if elastomer_mesh_prim is None: + raise RuntimeError(f"No visual mesh found under elastomer at path: {elastomer_prim_path}") + + logger.info(f"Generating tactile points from USD mesh: {elastomer_mesh_prim.GetPath().pathString}") + + # Extract mesh data + usd_mesh = UsdGeom.Mesh(elastomer_mesh_prim) + points = np.asarray(usd_mesh.GetPointsAttr().Get()) + face_indices = np.asarray(usd_mesh.GetFaceVertexIndicesAttr().Get()) + + # Simple triangulation + faces = face_indices.reshape(-1, 3) + + # Create bounds + mesh_bounds = np.array([points.min(axis=0), points.max(axis=0)]) + + # Create trimesh object + mesh = trimesh.Trimesh(vertices=points, faces=faces) + + # Generate grid on elastomer + elastomer_dims = np.diff(mesh_bounds, axis=0).squeeze() + slim_axis = np.argmin(elastomer_dims) # Determine flat axis of elastomer + + # Determine tip direction using dome geometry + # For dome-shaped elastomers, the center of mass is shifted toward the dome (contact) side + mesh_center_of_mass = mesh.center_mass[slim_axis] + bounding_box_center = (mesh_bounds[0, slim_axis] + mesh_bounds[1, slim_axis]) / 2.0 + + tip_direction_sign = 1.0 if mesh_center_of_mass > bounding_box_center else -1.0 + + # Determine gap between adjacent tactile points + axis_idxs = list(range(3)) + axis_idxs.remove(int(slim_axis)) # Remove slim idx + div_sz = (elastomer_dims[axis_idxs] - margin * 2.0) / (np.array(num_divs) + 1) + tactile_points_dx = min(div_sz) + + # Sample points on the flat plane + planar_grid_points = [] + center = (mesh_bounds[0] + mesh_bounds[1]) / 2.0 + idx = 0 + for axis_i in range(3): + if axis_i == slim_axis: + # On the slim axis, place a point far away so ray is pointing at the elastomer tip + planar_grid_points.append([tip_direction_sign]) + else: + axis_grid_points = np.linspace( + center[axis_i] - tactile_points_dx * (num_divs[idx] + 1.0) / 2.0, + center[axis_i] + tactile_points_dx * (num_divs[idx] + 1.0) / 2.0, + num_divs[idx] + 2, + ) + planar_grid_points.append(axis_grid_points[1:-1]) # Leave out the extreme corners + idx += 1 + + grid_corners = itertools.product(planar_grid_points[0], planar_grid_points[1], planar_grid_points[2]) + grid_corners = np.array(list(grid_corners)) + + # Project ray in positive y direction on the mesh + mesh_data = trimesh.ray.ray_triangle.RayMeshIntersector(mesh) + ray_dir = np.array([0, 0, 0]) + ray_dir[slim_axis] = -tip_direction_sign # Ray points towards elastomer (opposite of tip direction) + + # Handle the ray intersection result + index_tri, index_ray, locations = mesh_data.intersects_id( + grid_corners, np.tile([ray_dir], (grid_corners.shape[0], 1)), return_locations=True, multiple_hits=False + ) + + if visualize: + query_pointcloud = trimesh.PointCloud(locations, colors=(0.0, 0.0, 1.0)) + trimesh.Scene([mesh, query_pointcloud]).show() + + # Sort and store tactile points + tactile_points = locations[index_ray.argsort()] + # in the frame of the elastomer + self._tactile_pos_local = torch.tensor(tactile_points, dtype=torch.float32, device=self._device) + self.num_tactile_points = self._tactile_pos_local.shape[0] + if self.num_tactile_points != self.cfg.tactile_array_size[0] * self.cfg.tactile_array_size[1]: + raise RuntimeError( + f"Number of tactile points does not match expected: {self.num_tactile_points} !=" + f" {self.cfg.tactile_array_size[0] * self.cfg.tactile_array_size[1]}" + ) + + # Assume tactile frame rotation are all the same + rotation = torch.tensor([0, 0, -torch.pi], device=self._device) + self._tactile_quat_local = ( + math_utils.quat_from_euler_xyz(rotation[0], rotation[1], rotation[2]) + .unsqueeze(0) + .repeat(len(tactile_points), 1) + ) + + logger.info(f"Generated {len(tactile_points)} tactile points from USD mesh using ray casting") + + def _initialize_force_field_buffers(self): + """Initialize data buffers for force field sensing.""" + num_pts = self.num_tactile_points + + # Initialize force field data tensors + self._data.tactile_points_pos_w = torch.zeros((self._num_envs, num_pts, 3), device=self._device) + self._data.tactile_points_quat_w = torch.zeros((self._num_envs, num_pts, 4), device=self._device) + self._data.penetration_depth = torch.zeros((self._num_envs, num_pts), device=self._device) + self._data.tactile_normal_force = torch.zeros((self._num_envs, num_pts), device=self._device) + self._data.tactile_shear_force = torch.zeros((self._num_envs, num_pts, 2), device=self._device) + # Pre-compute expanded tactile point tensors to avoid repeated unsqueeze/expand operations + self._tactile_pos_expanded = self._tactile_pos_local.unsqueeze(0).expand(self._num_envs, -1, -1) + self._tactile_quat_expanded = self._tactile_quat_local.unsqueeze(0).expand(self._num_envs, -1, -1) + + def _initialize_visualization(self): + """Initialize visualization markers for tactile points.""" + if self.cfg.visualizer_cfg: + self._visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data. + + This method updates both camera-based and force field tactile sensing data + for the specified environments. + + Args: + env_ids: Sequence of environment indices to update. If length equals + total number of environments, all environments are updated. + """ + # Convert to proper indices for internal methods + if len(env_ids) == self._num_envs: + internal_env_ids = slice(None) + else: + internal_env_ids = env_ids + + # Update camera-based tactile data + if self.cfg.enable_camera_tactile: + self._update_camera_tactile(internal_env_ids) + + # Update force field tactile data + if self.cfg.enable_force_field: + self._update_force_field(internal_env_ids) + + def _update_camera_tactile(self, env_ids: Sequence[int] | slice): + """Update camera-based tactile sensing data. + + This method updates the camera sensor and processes the depth information + to compute tactile measurements. It computes the difference from the nominal + (no-contact) state and renders it using the GelSight tactile renderer. + + Args: + env_ids: Environment indices or slice to update. Can be a sequence of + integers or a slice object for batch processing. + """ + if self._nominal_tactile is None: + raise RuntimeError("Nominal tactile is not set. Please call get_initial_render() first.") + # Update camera sensor + self._camera_sensor.update(self._sim_physics_dt) + + # Get camera data + camera_data = self._camera_sensor.data + + # Check for either distance_to_image_plane or depth (they are equivalent) + depth_key = None + if "distance_to_image_plane" in camera_data.output: + depth_key = "distance_to_image_plane" + elif "depth" in camera_data.output: + depth_key = "depth" + + if depth_key: + self._data.tactile_depth_image[env_ids] = camera_data.output[depth_key][env_ids].clone() + diff = self._nominal_tactile[depth_key][env_ids] - self._data.tactile_depth_image[env_ids] + self._data.tactile_rgb_image[env_ids] = self._tactile_rgb_render.render(diff.squeeze(-1)) + + ######################################################################################### + # Force field tactile sensing + ######################################################################################### + + def _update_force_field(self, env_ids: Sequence[int] | slice): + """Update force field tactile sensing data. + + This method computes penalty-based tactile forces using Signed Distance Field (SDF) + queries. It transforms tactile points to contact object local coordinates, queries the SDF of the + contact object for collision detection, and computes normal and shear forces based on + penetration depth and relative velocities. + + Args: + env_ids: Environment indices or slice to update. Can be a sequence of + integers or a slice object for batch processing. + + Note: + Requires both elastomer and contact object body views to be initialized. Returns + early if tactile points or body views are not available. + """ + # Step 1: Get elastomer pose and precompute pose components + elastomer_pos_w, elastomer_quat_w = self._elastomer_body_view.get_transforms().split([3, 4], dim=-1) + elastomer_quat_w = math_utils.convert_quat(elastomer_quat_w, to="wxyz") + + # Transform tactile points to world coordinates, used for visualization + self._transform_tactile_points_to_world(elastomer_pos_w, elastomer_quat_w) + + # earlly return if contact object body view is not available + # this could happen if the contact object is not specified when tactile_points are required for visualization + if self._contact_object_body_view is None: + return + + # Step 2: Transform tactile points to contact object local frame for SDF queries + contact_object_pos_w, contact_object_quat_w = self._contact_object_body_view.get_transforms().split( + [3, 4], dim=-1 + ) + contact_object_quat_w = math_utils.convert_quat(contact_object_quat_w, to="wxyz") + + world_tactile_points = self._data.tactile_points_pos_w + points_contact_object_local, contact_object_quat_inv = self._transform_points_to_contact_object_local( + world_tactile_points, contact_object_pos_w, contact_object_quat_w + ) + + # Step 3: Query SDF for collision detection + sdf_values_and_gradients = self._contact_object_sdf_view.get_sdf_and_gradients(points_contact_object_local) + sdf_values = sdf_values_and_gradients[..., -1] # Last component is SDF value + sdf_gradients = sdf_values_and_gradients[..., :-1] # First 3 components are gradients + + # Step 4: Compute tactile forces from SDF data + self._compute_tactile_forces_from_sdf( + points_contact_object_local, + sdf_values, + sdf_gradients, + contact_object_pos_w, + contact_object_quat_w, + elastomer_quat_w, + env_ids, + ) + + def _transform_tactile_points_to_world(self, pos_w: torch.Tensor, quat_w: torch.Tensor): + """Transform tactile points from local to world coordinates. + + Args: + pos_w: Elastomer positions in world frame. Shape: (num_envs, 3) + quat_w: Elastomer quaternions in world frame. Shape: (num_envs, 4) + """ + num_pts = self.num_tactile_points + + quat_expanded = quat_w.unsqueeze(1).expand(-1, num_pts, -1) + pos_expanded = pos_w.unsqueeze(1).expand(-1, num_pts, -1) + + # Apply transformation + tactile_pos_w = math_utils.quat_apply(quat_expanded, self._tactile_pos_expanded) + pos_expanded + tactile_quat_w = math_utils.quat_mul(quat_expanded, self._tactile_quat_expanded) + + # Store in data + self._data.tactile_points_pos_w = tactile_pos_w + self._data.tactile_points_quat_w = tactile_quat_w + + def _transform_points_to_contact_object_local( + self, world_points: torch.Tensor, contact_object_pos_w: torch.Tensor, contact_object_quat_w: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor]: + """Optimized version: Transform world coordinates to contact object local frame. + + Args: + world_points: Points in world coordinates. Shape: (num_envs, num_points, 3) + contact_object_pos_w: Contact object positions in world frame. Shape: (num_envs, 3) + contact_object_quat_w: Contact object quaternions in world frame. Shape: (num_envs, 4) + + Returns: + Points in contact object local coordinates and inverse quaternions + """ + # Get inverse transformation (per environment) + # wxyz in torch + contact_object_quat_inv, contact_object_pos_inv = torch_utils.tf_inverse( + contact_object_quat_w, contact_object_pos_w + ) + num_pts = self.num_tactile_points + + contact_object_quat_expanded = contact_object_quat_inv.unsqueeze(1).expand(-1, num_pts, 4) + contact_object_pos_expanded = contact_object_pos_inv.unsqueeze(1).expand(-1, num_pts, 3) + + # Apply transformation + points_sdf = torch_utils.tf_apply(contact_object_quat_expanded, contact_object_pos_expanded, world_points) + + return points_sdf, contact_object_quat_inv + + def _get_tactile_points_velocities( + self, linvel_world: torch.Tensor, angvel_world: torch.Tensor, quat_world: torch.Tensor + ) -> torch.Tensor: + """Optimized version: Compute tactile point velocities from precomputed velocities. + + Args: + linvel_world: Elastomer linear velocities. Shape: (num_envs, 3) + angvel_world: Elastomer angular velocities. Shape: (num_envs, 3) + quat_world: Elastomer quaternions. Shape: (num_envs, 4) + + Returns: + Tactile point velocities in world frame. Shape: (num_envs, num_points, 3) + """ + num_pts = self.num_tactile_points + + # Pre-expand all required tensors once + quat_expanded = quat_world.unsqueeze(1).expand(-1, num_pts, 4) + tactile_pos_expanded = self._tactile_pos_expanded + + # Transform local positions to world frame relative vectors + tactile_pos_world_relative = math_utils.quat_apply(quat_expanded, tactile_pos_expanded) + + # Compute velocity due to angular motion: ฯ‰ ร— r + angvel_expanded = angvel_world.unsqueeze(1).expand(-1, num_pts, 3) + angular_velocity_contribution = torch.cross(angvel_expanded, tactile_pos_world_relative, dim=-1) + + # Add linear velocity contribution + linvel_expanded = linvel_world.unsqueeze(1).expand(-1, num_pts, 3) + tactile_velocity_world = angular_velocity_contribution + linvel_expanded + + return tactile_velocity_world + + def _compute_tactile_forces_from_sdf( + self, + points_contact_object_local: torch.Tensor, + sdf_values: torch.Tensor, + sdf_gradients: torch.Tensor, + contact_object_pos_w: torch.Tensor, + contact_object_quat_w: torch.Tensor, + elastomer_quat_w: torch.Tensor, + env_ids: Sequence[int] | slice, + ) -> None: + """Optimized version: Compute tactile forces from SDF values using precomputed parameters. + + This method now operates directly on the pre-allocated data tensors to avoid + unnecessary memory allocation and copying. + + Args: + points_contact_object_local: Points in contact object local frame + sdf_values: SDF values (negative means penetration) + sdf_gradients: SDF gradients (surface normals) + contact_object_pos_w: Contact object positions in world frame + contact_object_quat_w: Contact object quaternions in world frame + elastomer_quat_w: Elastomer quaternions + env_ids: Environment indices being updated + + """ + depth = self._data.penetration_depth[env_ids] + tactile_normal_force = self._data.tactile_normal_force[env_ids] + tactile_shear_force = self._data.tactile_shear_force[env_ids] + + # Clear the output tensors + tactile_normal_force.zero_() + tactile_shear_force.zero_() + depth.zero_() + + # Convert SDF values to penetration depth (positive for penetration) + depth[:] = torch.clamp(-sdf_values[env_ids], min=0.0) # Negative SDF means inside (penetrating) + + # Get collision mask for points that are penetrating + collision_mask = depth > 0.0 + + # Use pre-allocated tensors instead of creating new ones + num_pts = self.num_tactile_points + + if collision_mask.any() or self.cfg.visualize_sdf_closest_pts: + # Get contact object and elastomer velocities (com velocities) + contact_object_velocities = self._contact_object_body_view.get_velocities() + contact_object_linvel_w_com = contact_object_velocities[env_ids, :3] + contact_object_angvel_w = contact_object_velocities[env_ids, 3:] + + elastomer_velocities = self._elastomer_body_view.get_velocities() + elastomer_linvel_w_com = elastomer_velocities[env_ids, :3] + elastomer_angvel_w = elastomer_velocities[env_ids, 3:] + + # Contact object adjustment + contact_object_com_w_offset = math_utils.quat_apply( + contact_object_quat_w[env_ids], self._contact_object_com_b[env_ids] + ) + contact_object_linvel_w = contact_object_linvel_w_com - torch.cross( + contact_object_angvel_w, contact_object_com_w_offset, dim=-1 + ) + # v_origin = v_com - w x (com_world_offset) where com_world_offset = quat_apply(quat, com_b) + elastomer_com_w_offset = math_utils.quat_apply(elastomer_quat_w[env_ids], self._elastomer_com_b[env_ids]) + elastomer_linvel_w = elastomer_linvel_w_com - torch.cross( + elastomer_angvel_w, elastomer_com_w_offset, dim=-1 + ) + + # Normalize gradients to get surface normals in local frame + normals_local = torch.nn.functional.normalize(sdf_gradients[env_ids], dim=-1) + + # Transform normals to world frame (rotate by contact object orientation) - use precomputed quaternions + contact_object_quat_expanded = contact_object_quat_w[env_ids].unsqueeze(1).expand(-1, num_pts, 4) + + # Apply quaternion transformation + normals_world = math_utils.quat_apply(contact_object_quat_expanded, normals_local) + + # Compute normal contact force: F_n = k_n * depth + fc_norm = self.cfg.normal_contact_stiffness * depth + fc_world = fc_norm.unsqueeze(-1) * normals_world + + # Get tactile point velocities using precomputed velocities + tactile_velocity_world = self._get_tactile_points_velocities( + elastomer_linvel_w, elastomer_angvel_w, elastomer_quat_w[env_ids] + ) + + # Use precomputed contact object velocities + closest_points_sdf = points_contact_object_local[env_ids] + depth.unsqueeze(-1) * normals_local + + if self.cfg.visualize_sdf_closest_pts: + debug_closest_points_sdf = ( + points_contact_object_local[env_ids] - sdf_values[env_ids].unsqueeze(-1) * normals_local + ) + self.debug_closest_points_wolrd = math_utils.quat_apply( + contact_object_quat_expanded, debug_closest_points_sdf + ) + contact_object_pos_w[env_ids].unsqueeze(1).expand(-1, num_pts, 3) + + contact_object_linvel_expanded = contact_object_linvel_w.unsqueeze(1).expand(-1, num_pts, 3) + contact_object_angvel_expanded = contact_object_angvel_w.unsqueeze(1).expand(-1, num_pts, 3) + closest_points_vel_world = ( + torch.linalg.cross( + contact_object_angvel_expanded, + math_utils.quat_apply(contact_object_quat_expanded, closest_points_sdf), + ) + + contact_object_linvel_expanded + ) + + # Compute relative velocity at contact points + relative_velocity_world = tactile_velocity_world - closest_points_vel_world + + # Compute tangential velocity (perpendicular to normal) + vt_world = relative_velocity_world - normals_world * torch.sum( + normals_world * relative_velocity_world, dim=-1, keepdim=True + ) + vt_norm = torch.norm(vt_world, dim=-1) + + # Compute friction force: F_t = min(k_t * |v_t|, mu * F_n) + ft_static_norm = self.cfg.tangential_stiffness * vt_norm + ft_dynamic_norm = self.cfg.friction_coefficient * fc_norm + ft_norm = torch.minimum(ft_static_norm, ft_dynamic_norm) + + # Apply friction force opposite to tangential velocity + ft_world = -ft_norm.unsqueeze(-1) * vt_world / (vt_norm.unsqueeze(-1).clamp(min=1e-9)) + + # Total tactile force in world frame + tactile_force_world = fc_world + ft_world + + # Transform forces to tactile frame + tactile_force_tactile = math_utils.quat_apply_inverse( + self._data.tactile_points_quat_w[env_ids], tactile_force_world + ) + + # Extract normal and shear components + # Assume tactile frame has Z as normal direction + tactile_normal_force[:] = tactile_force_tactile[..., 2] # Z component + tactile_shear_force[:] = tactile_force_tactile[..., :2] # X,Y components + + ######################################################################################### + # Debug visualization + ######################################################################################### + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects.""" + # 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 self._tactile_visualizer is None: + self._tactile_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + # set their visibility to true + self._tactile_visualizer.set_visibility(True) + else: + if self._tactile_visualizer: + self._tactile_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + """Callback for debug visualization of tactile sensor data. + + This method is called during each simulation step when debug visualization is enabled. + It visualizes tactile sensing points as 3D markers in the simulation viewport to help + with debugging and understanding sensor behavior. + + The method handles two visualization modes: + + 1. **Standard mode**: Visualizes ``tactile_points_pos_w`` - the world positions of + tactile sensing points on the sensor surface + 2. **SDF debug mode**: When ``cfg.visualize_sdf_closest_pts`` is True, visualizes + ``debug_closest_points_wolrd`` - the closest surface points computed during + SDF-based force calculations + """ + # Safety check - return if not properly initialized + if not hasattr(self, "_tactile_visualizer") or self._tactile_visualizer is None: + return + vis_points = None + + if self.cfg.visualize_sdf_closest_pts and hasattr(self, "debug_closest_points_wolrd"): + vis_points = self.debug_closest_points_wolrd + else: + vis_points = self._data.tactile_points_pos_w + + if vis_points is None or vis_points.numel() == 0: + return + + viz_points = vis_points.view(-1, 3) # Shape: (num_envs * num_points, 3) + + indices = torch.zeros(viz_points.shape[0], dtype=torch.long, device=self._device) + + marker_scales = torch.ones(viz_points.shape[0], 3, device=self._device) + + # Visualize tactile points + self._tactile_visualizer.visualize(translations=viz_points, marker_indices=indices, scales=marker_scales) diff --git a/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor_cfg.py new file mode 100644 index 000000000000..5aaf9cd67311 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor_cfg.py @@ -0,0 +1,192 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +# needed to import for allowing type-hinting: torch.Tensor | None +from __future__ import annotations + +from dataclasses import MISSING + +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import VISUO_TACTILE_SENSOR_MARKER_CFG +from isaaclab.sensors import SensorBaseCfg, TiledCameraCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +from .visuotactile_sensor import VisuoTactileSensor + +## +# GelSight Render Configuration +## + + +@configclass +class GelSightRenderCfg: + """Configuration for GelSight sensor rendering parameters. + + This configuration defines the rendering parameters for example-based tactile image synthesis + using the Taxim approach. + + Reference: + Si, Z., & Yuan, W. (2022). Taxim: An example-based simulation model for GelSight + tactile sensors. IEEE Robotics and Automation Letters, 7(2), 2361-2368. + https://arxiv.org/abs/2109.04027 + + Data Directory Structure: + The sensor data should be organized in the following structure:: + + base_data_path/ + โ””โ”€โ”€ sensor_data_dir_name/ + โ”œโ”€โ”€ bg.jpg # Background image (required) + โ”œโ”€โ”€ polycalib.npz # Polynomial calibration data (required) + โ””โ”€โ”€ real_bg.npy # Real background data (optional) + + Example: + Using predefined sensor configuration:: + + from isaaclab_contrib.sensors.tacsl_sensor import VisuoTactileSensorCfg + + from isaaclab_assets.sensors import GELSIGHT_R15_CFG + + sensor_cfg = VisuoTactileSensorCfg(render_cfg=GELSIGHT_R15_CFG) + + Using custom sensor data:: + + custom_cfg = GelSightRenderCfg( + base_data_path="/path/to/my/sensors", + sensor_data_dir_name="my_custom_sensor", + image_height=480, + image_width=640, + mm_per_pixel=0.05, + ) + """ + + base_data_path: str = f"{ISAACLAB_NUCLEUS_DIR}/TacSL" + """Base path to the directory containing sensor calibration data. Defaults to + Isaac Lab Nucleus directory at ``{ISAACLAB_NUCLEUS_DIR}/TacSL``. + """ + + sensor_data_dir_name: str = MISSING + """Directory name containing the sensor calibration and background data. + + This should be a relative path (directory name) inside the :attr:`base_data_path`. + """ + + background_path: str = "bg.jpg" + """Filename of the background image within the data directory.""" + + calib_path: str = "polycalib.npz" + """Filename of the polynomial calibration data within the data directory.""" + + real_background: str = "real_bg.npy" + """Filename of the real background data within the data directory.""" + + image_height: int = MISSING + """Height of the tactile image in pixels.""" + + image_width: int = MISSING + """Width of the tactile image in pixels.""" + + num_bins: int = 120 + """Number of bins for gradient magnitude and direction quantization.""" + + mm_per_pixel: float = MISSING + """Millimeters per pixel conversion factor for reconstructing 2D tactile image from the height map.""" + + +## +# Visuo-Tactile Sensor Configuration +## + + +@configclass +class VisuoTactileSensorCfg(SensorBaseCfg): + """Configuration for the visuo-tactile sensor. + + This sensor provides both camera-based tactile sensing and force field tactile sensing. + It can capture tactile RGB/depth images and compute penalty-based contact forces. + """ + + class_type: type = VisuoTactileSensor + + # Sensor type and capabilities + render_cfg: GelSightRenderCfg = MISSING + """Configuration for GelSight sensor rendering. + + This defines the rendering parameters for converting depth maps to realistic tactile images. + + For simplicity, you can use the predefined configs for standard sensor models: + + - :attr:`isaaclab_assets.sensors.GELSIGHT_R15_CFG` + - :attr:`isaaclab_assets.sensors.GELSIGHT_MINI_CFG` + + """ + + enable_camera_tactile: bool = True + """Whether to enable camera-based tactile sensing.""" + + enable_force_field: bool = True + """Whether to enable force field tactile sensing.""" + + # Force field configuration + tactile_array_size: tuple[int, int] = MISSING + """Number of tactile points for force field sensing in (rows, cols) format.""" + + tactile_margin: float = MISSING + """Margin for tactile point generation (in meters). + + This parameter defines the exclusion margin from the edges of the elastomer mesh when generating + the tactile point grid. It ensures that force field points are not generated on the very edges + of the sensor surface where geometry might be unstable or less relevant for contact. + """ + + contact_object_prim_path_expr: str | None = None + """Prim path expression to find the contact object for force field computation. + + This specifies the object that will make contact with the tactile sensor. The sensor will automatically + find the SDF collision mesh within this object for optimal force field computation. + + .. note:: + The expression can contain the environment namespace regex ``{ENV_REGEX_NS}`` which + will be replaced with the environment namespace. + + Example: ``{ENV_REGEX_NS}/ContactObject`` will be replaced with ``/World/envs/env_.*/ContactObject``. + + .. attention:: + For force field computation to work properly, the contact object must have an SDF collision mesh. + The sensor will search for the first SDF mesh within the specified prim hierarchy. + """ + + # Force field physics parameters + normal_contact_stiffness: float = 1.0 + """Normal contact stiffness for penalty-based force computation.""" + + friction_coefficient: float = 2.0 + """Friction coefficient for shear forces.""" + + tangential_stiffness: float = 0.1 + """Tangential stiffness for shear forces.""" + + camera_cfg: TiledCameraCfg | None = None + """Camera configuration for tactile RGB/depth sensing. + + If None, camera-based sensing will be disabled even if :attr:`enable_camera_tactile` is True. + """ + + # Visualization + visualizer_cfg: VisualizationMarkersCfg = VISUO_TACTILE_SENSOR_MARKER_CFG.replace( + prim_path="/Visuals/TactileSensor" + ) + """The configuration object for the visualization markers. + + .. note:: + This attribute is only used when debug visualization is enabled. + """ + + trimesh_vis_tactile_points: bool = False + """Whether to visualize tactile points for debugging using trimesh. Defaults to False.""" + + visualize_sdf_closest_pts: bool = False + """Whether to visualize SDF closest points for debugging. Defaults to False.""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor_data.py b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor_data.py new file mode 100644 index 000000000000..1390d0257a0f --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor_data.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +from dataclasses import dataclass + +import torch + + +@dataclass +class VisuoTactileSensorData: + """Data container for the visuo-tactile sensor. + + This class contains the tactile sensor data that includes: + + - Camera-based tactile sensing (RGB and depth images) + - Force field tactile sensing (normal and shear forces) + - Tactile point positions and contact information + + """ + + # Camera-based tactile data + tactile_depth_image: torch.Tensor | None = None + """Tactile depth images. Shape is (num_instances, height, width, 1).""" + + tactile_rgb_image: torch.Tensor | None = None + """Tactile RGB images rendered using the Taxim approach from :cite:t:`si2022taxim`. + Shape is (num_instances, height, width, 3). + """ + + # Force field tactile data + tactile_points_pos_w: torch.Tensor | None = None + """Positions of tactile points in world frame. Shape is (num_instances, num_tactile_points, 3).""" + + tactile_points_quat_w: torch.Tensor | None = None + """Orientations of tactile points in world frame. Shape is (num_instances, num_tactile_points, 4).""" + + penetration_depth: torch.Tensor | None = None + """Penetration depth at each tactile point. Shape is (num_instances, num_tactile_points).""" + + tactile_normal_force: torch.Tensor | None = None + """Normal forces at each tactile point in sensor frame. Shape is (num_instances, num_tactile_points).""" + + tactile_shear_force: torch.Tensor | None = None + """Shear forces at each tactile point in sensor frame. Shape is (num_instances, num_tactile_points, 2).""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/utils/types.py b/source/isaaclab_contrib/isaaclab_contrib/utils/types.py new file mode 100644 index 000000000000..f6cc242fe6d6 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/utils/types.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for multirotor-specific data types. + +This module defines data container classes used for passing multirotor-specific +information between components (e.g., between action terms and actuator models). +""" + +from __future__ import annotations + +from collections.abc import Sequence +from dataclasses import dataclass + +import torch + + +@dataclass +class MultiRotorActions: + """Data container to store multirotor thruster actions. + + This dataclass is used to pass thrust commands and thruster indices between + components in the multirotor control pipeline. It is primarily used internally + by the :class:`~isaaclab_contrib.assets.Multirotor` class to communicate with + :class:`~isaaclab_contrib.actuators.Thruster` actuator models. + + The container supports partial actions by allowing specification of which + thrusters the actions apply to through the :attr:`thruster_indices` field. + + Attributes: + thrusts: Thrust values for the specified thrusters. Shape is typically + ``(num_envs, num_selected_thrusters)``. + thruster_indices: Indices of thrusters that the thrust values apply to. + Can be a tensor of indices, a sequence, a slice, or None for all thrusters. + + Example: + .. code-block:: python + + # Create actions for all thrusters + actions = MultiRotorActions( + thrusts=torch.ones(num_envs, 4) * 5.0, + thruster_indices=slice(None), # All thrusters + ) + + # Create actions for specific thrusters + actions = MultiRotorActions( + thrusts=torch.tensor([[6.0, 7.0]]), + thruster_indices=[0, 2], # Only thrusters 0 and 2 + ) + + Note: + If both fields are ``None``, no action is taken. This is useful for + conditional action application. + + .. seealso:: + - :class:`~isaaclab.utils.types.ArticulationActions`: Similar container for joint actions + - :class:`~isaaclab_contrib.actuators.Thruster`: Thruster actuator that consumes these actions + """ + + thrusts: torch.Tensor | None = None + """Thrust values for the multirotor thrusters. + + Shape: ``(num_envs, num_thrusters)`` or ``(num_envs, num_selected_thrusters)`` + + The units depend on the actuator model configuration: + - For force-based control: Newtons (N) + - For RPS-based control: Revolutions per second (1/s) + + If ``None``, no thrust commands are specified. + """ + + thruster_indices: torch.Tensor | Sequence[int] | slice | None = None + """Indices of thrusters that the thrust values apply to. + + This field specifies which thrusters the :attr:`thrusts` values correspond to. + It can be: + - A torch.Tensor of integer indices: ``torch.tensor([0, 2, 3])`` + - A sequence of integers: ``[0, 2, 3]`` + - A slice: ``slice(None)`` for all thrusters, ``slice(0, 2)`` for first two + - ``None``: Defaults to all thrusters + + Using a slice is more efficient for contiguous thruster ranges as it avoids + creating intermediate index tensors. + + Example: + .. code-block:: python + + # All thrusters (most efficient) + thruster_indices = slice(None) + + # First two thrusters + thruster_indices = slice(0, 2) + + # Specific thrusters + thruster_indices = [0, 2, 3] + """ diff --git a/source/isaaclab_contrib/pyproject.toml b/source/isaaclab_contrib/pyproject.toml new file mode 100644 index 000000000000..d90ac3536f16 --- /dev/null +++ b/source/isaaclab_contrib/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools", "wheel", "toml"] +build-backend = "setuptools.build_meta" diff --git a/source/isaaclab_contrib/setup.py b/source/isaaclab_contrib/setup.py new file mode 100644 index 000000000000..8de11268f8b9 --- /dev/null +++ b/source/isaaclab_contrib/setup.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Installation script for the 'isaaclab_contrib' python package.""" + +import os + +import toml +from setuptools import setup + +# Obtain the extension data from the extension.toml file +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) +# Read the extension.toml file +EXTENSION_TOML_DATA = toml.load(os.path.join(EXTENSION_PATH, "config", "extension.toml")) + +# Installation operation +setup( + name="isaaclab_contrib", + author="Isaac Lab Project Developers", + maintainer="Isaac Lab Project Developers", + url=EXTENSION_TOML_DATA["package"]["repository"], + version=EXTENSION_TOML_DATA["package"]["version"], + description=EXTENSION_TOML_DATA["package"]["description"], + keywords=EXTENSION_TOML_DATA["package"]["keywords"], + include_package_data=True, + python_requires=">=3.10", + packages=["isaaclab_contrib"], + classifiers=[ + "Natural Language :: English", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Isaac Sim :: 4.5.0", + "Isaac Sim :: 5.0.0", + ], + zip_safe=False, +) diff --git a/source/isaaclab_contrib/test/actuators/test_thruster.py b/source/isaaclab_contrib/test/actuators/test_thruster.py new file mode 100644 index 000000000000..322005954737 --- /dev/null +++ b/source/isaaclab_contrib/test/actuators/test_thruster.py @@ -0,0 +1,204 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.app import AppLauncher + +HEADLESS = True + +# if not AppLauncher.instance(): +simulation_app = AppLauncher(headless=HEADLESS).app + +"""Rest of imports follows""" + +from types import SimpleNamespace + +import pytest +import torch + + +def make_thruster_cfg(num_motors: int): + """Create a minimal Thruster-like config object for tests.""" + return SimpleNamespace( + dt=0.01, + num_motors=num_motors, + thrust_range=(0.0, 10.0), + max_thrust_rate=100.0, + thrust_const_range=(0.05, 0.1), + tau_inc_range=(0.01, 0.02), + tau_dec_range=(0.01, 0.02), + torque_to_thrust_ratio=0.0, + use_discrete_approximation=True, + use_rps=True, + integration_scheme="euler", + ) + + +@pytest.mark.parametrize("num_envs", [1, 2, 4]) +@pytest.mark.parametrize("num_motors", [1, 2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_zero_thrust_const_is_handled(num_envs, num_motors, device): + """When thrust_const_range contains zeros, Thruster clamps values and compute returns finite outputs.""" + from isaaclab_contrib.actuators import Thruster + + cfg = make_thruster_cfg(num_motors) + cfg.thrust_const_range = (0.0, 0.0) + + thruster_names = [f"t{i}" for i in range(num_motors)] + thruster_ids = slice(None) + init_rps = torch.ones(num_envs, num_motors, device=device) + + thr = Thruster(cfg, thruster_names, thruster_ids, num_envs, device, init_rps) # type: ignore[arg-type] + + command = torch.full((num_envs, num_motors), 1.0, device=device) + action = SimpleNamespace(thrusts=command.clone(), thruster_indices=thruster_ids) + + thr.compute(action) # type: ignore[arg-type] + + assert torch.isfinite(action.thrusts).all() + + +@pytest.mark.parametrize("num_envs", [1, 2, 4]) +@pytest.mark.parametrize("num_motors", [1, 2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_negative_thrust_range_results_finite(num_envs, num_motors, device): + """Negative configured thrust ranges are clamped and yield finite outputs after hardening.""" + from isaaclab_contrib.actuators import Thruster + + cfg = make_thruster_cfg(num_motors) + cfg.thrust_range = (-5.0, -1.0) + cfg.thrust_const_range = (0.05, 0.05) + + thruster_names = [f"t{i}" for i in range(num_motors)] + thruster_ids = slice(None) + init_rps = torch.ones(num_envs, num_motors, device=device) + + thr = Thruster(cfg, thruster_names, thruster_ids, num_envs, device, init_rps) # type: ignore[arg-type] + + command = torch.full((num_envs, num_motors), -2.0, device=device) + action = SimpleNamespace(thrusts=command.clone(), thruster_indices=thruster_ids) + + thr.compute(action) # type: ignore[arg-type] + + assert torch.isfinite(action.thrusts).all() + + +@pytest.mark.parametrize("num_envs", [2, 3, 4]) +@pytest.mark.parametrize("num_motors", [2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_tensor_vs_slice_indices_and_subset_reset(num_envs, num_motors, device): + """Compute should accept tensor or slice thruster indices, and reset_idx should affect only specified envs.""" + from isaaclab_contrib.actuators import Thruster + + cfg = make_thruster_cfg(num_motors) + + thruster_names = [f"t{i}" for i in range(num_motors)] + # Use motor indices that exist for the given num_motors + motor_indices = [0, min(2, num_motors - 1)] + thruster_ids_tensor = torch.tensor(motor_indices, dtype=torch.int64, device=device) + thruster_ids_slice = slice(None) + init_rps = torch.ones(num_envs, num_motors, device=device) + + thr_tensor = Thruster(cfg, thruster_names, thruster_ids_tensor, num_envs, device, init_rps) # type: ignore[arg-type] + thr_slice = Thruster(cfg, thruster_names, thruster_ids_slice, num_envs, device, init_rps) # type: ignore[arg-type] + + command = torch.full((num_envs, num_motors), cfg.thrust_range[1] * 0.5, device=device) + action_tensor = SimpleNamespace(thrusts=command.clone(), thruster_indices=thruster_ids_tensor) + action_slice = SimpleNamespace(thrusts=command.clone(), thruster_indices=thruster_ids_slice) + + thr_tensor.compute(action_tensor) # type: ignore[arg-type] + thr_slice.compute(action_slice) # type: ignore[arg-type] + + assert action_tensor.thrusts.shape == (num_envs, num_motors) + assert action_slice.thrusts.shape == (num_envs, num_motors) + + # Test reset on the last environment + env_to_reset = num_envs - 1 + prev = thr_tensor.curr_thrust.clone() + thr_tensor.reset_idx(torch.tensor([env_to_reset], dtype=torch.int64, device=device)) + assert not torch.allclose(prev[env_to_reset], thr_tensor.curr_thrust[env_to_reset]) + + +@pytest.mark.parametrize("num_envs", [1, 2, 4]) +@pytest.mark.parametrize("num_motors", [1, 2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_mixing_and_integration_modes(num_envs, num_motors, device): + """Verify mixing factor selection and integration kernel choice reflect the config.""" + from isaaclab_contrib.actuators import Thruster + + cfg = make_thruster_cfg(num_motors) + + thruster_names = [f"t{i}" for i in range(num_motors)] + + # discrete mixing + cfg.use_discrete_approximation = True + cfg.integration_scheme = "euler" + thr_d = Thruster( + cfg, thruster_names, slice(None), num_envs, device, torch.ones(num_envs, num_motors, device=device) + ) # type: ignore[arg-type] + # bound method objects are recreated on access; compare underlying functions instead + assert getattr(thr_d.mixing_factor_function, "__func__", None) is Thruster.discrete_mixing_factor + assert getattr(thr_d._step_thrust, "__func__", None) is Thruster.compute_thrust_with_rpm_time_constant + + # continuous mixing and RK4 + cfg.use_discrete_approximation = False + cfg.integration_scheme = "rk4" + thr_c = Thruster( + cfg, thruster_names, slice(None), num_envs, device, torch.ones(num_envs, num_motors, device=device) + ) # type: ignore[arg-type] + assert getattr(thr_c.mixing_factor_function, "__func__", None) is Thruster.continuous_mixing_factor + assert getattr(thr_c._step_thrust, "__func__", None) is Thruster.compute_thrust_with_rpm_time_constant_rk4 + + +@pytest.mark.parametrize("num_envs", [1, 2, 4]) +@pytest.mark.parametrize("num_motors", [1, 2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_thruster_compute_clamps_and_shapes(num_envs, num_motors, device): + """Thruster.compute should return thrusts with correct shape and within clamp bounds.""" + from isaaclab_contrib.actuators import Thruster + + cfg = make_thruster_cfg(num_motors) + + thruster_names = [f"t{i}" for i in range(num_motors)] + thruster_ids = slice(None) + init_rps = torch.ones(num_envs, num_motors, device=device) + + thr = Thruster(cfg, thruster_names, thruster_ids, num_envs, device, init_rps) # type: ignore[arg-type] + + # command above max to check clamping + command = torch.full((num_envs, num_motors), cfg.thrust_range[1] * 2.0, device=device) + action = SimpleNamespace(thrusts=command.clone(), thruster_indices=thruster_ids) + + out = thr.compute(action) # type: ignore[arg-type] + + assert out.thrusts.shape == (num_envs, num_motors) + # values must be clipped to configured range + assert torch.all(out.thrusts <= cfg.thrust_range[1] + 1e-6) + assert torch.all(out.thrusts >= cfg.thrust_range[0] - 1e-6) + + +@pytest.mark.parametrize("num_envs", [1, 2, 4]) +@pytest.mark.parametrize("num_motors", [1, 2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_thruster_reset_idx_changes_state(num_envs, num_motors, device): + """reset_idx should re-sample parameters for specific env indices.""" + from isaaclab_contrib.actuators import Thruster + + cfg = make_thruster_cfg(num_motors) + + thruster_names = [f"t{i}" for i in range(num_motors)] + thruster_ids = slice(None) + init_rps = torch.ones(num_envs, num_motors, device=device) + + thr = Thruster(cfg, thruster_names, thruster_ids, num_envs, device, init_rps) # type: ignore[arg-type] + + # Mutate an internal sampled parameter so reset produces a measurable change. + thr.tau_inc_s[0, 0] = thr.tau_inc_s[0, 0] + 1.0 + prev_val = thr.tau_inc_s[0, 0].item() + + # reset only environment 0 + thr.reset_idx(torch.tensor([0], dtype=torch.int64, device=device)) + + # at least the first tau_inc value for env 0 should differ from the mutated value + assert not torch.isclose(torch.tensor(prev_val, device=device), thr.tau_inc_s[0, 0]) diff --git a/source/isaaclab_contrib/test/assets/test_multirotor.py b/source/isaaclab_contrib/test/assets/test_multirotor.py new file mode 100644 index 000000000000..84044231b16c --- /dev/null +++ b/source/isaaclab_contrib/test/assets/test_multirotor.py @@ -0,0 +1,415 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +HEADLESS = True + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import contextlib +import types +import warnings + +import pytest +import torch + +import isaaclab.sim as sim_utils +import isaaclab.sim.utils.prims as prim_utils +from isaaclab.sim import build_simulation_context + +from isaaclab_contrib.assets import Multirotor, MultirotorCfg + +# Best-effort: suppress unraisable destructor warnings emitted during +# teardown of partially-constructed assets in CI/dev environments. We still +# perform explicit cleanup where possible, but filter the remaining noisy +# warnings to keep test output clean. +warnings.filterwarnings("ignore", category=pytest.PytestUnraisableExceptionWarning) + +## +# Pre-defined configs +## +from isaaclab_assets.robots.arl_robot_1 import ARL_ROBOT_1_CFG + + +def generate_multirotor_cfg(usd_path: str | None = None) -> MultirotorCfg: + """Generate a multirotor configuration for tests. + + If an ARL-provided config is available, prefer that. Otherwise return a + minimal `MultirotorCfg` so integration tests can still run when a USD is + provided. + """ + if ARL_ROBOT_1_CFG is not None: + return ARL_ROBOT_1_CFG + + if usd_path is None: + return MultirotorCfg() + + return MultirotorCfg(spawn=sim_utils.UsdFileCfg(usd_path=usd_path)) + + +# ----------------------- +# Unit tests (simulator-free) +# ----------------------- + + +def make_multirotor_stub(num_instances: int, num_thrusters: int, device=torch.device("cpu")): + """Create a lightweight Multirotor instance suitable for unit tests that + don't require IsaacSim. We construct via __new__ and inject minimal + attributes the class methods expect. + """ + # Use a plain object (not a Multirotor instance) to avoid assigning to + # properties that only exist on the real class. We'll bind the + # Multirotor methods we need onto this fake object. + m = types.SimpleNamespace() + # runtime attributes the methods expect + m.device = device + m.num_instances = num_instances + m.num_bodies = 1 + + # allocation matrix as a plain Python list (the Multirotor property will + # convert it to a tensor using `self.cfg.allocation_matrix`), so provide + # it on `m.cfg` like the real object expects. + alloc_list = [[1.0 if r < 2 and c == r else 0.0 for c in range(num_thrusters)] for r in range(6)] + m.cfg = types.SimpleNamespace(allocation_matrix=alloc_list) + # Also provide allocation_matrix directly on the fake object so bound methods + # that access `self.allocation_matrix` succeed (properties won't dispatch + # because `m` is not a real Multirotor instance). + m.allocation_matrix = torch.tensor(alloc_list, device=device) + + # lightweight data container + data = types.SimpleNamespace() + data.default_thruster_rps = torch.zeros(num_instances, num_thrusters, device=device) + data.thrust_target = torch.zeros(num_instances, num_thrusters, device=device) + data.computed_thrust = torch.zeros(num_instances, num_thrusters, device=device) + data.applied_thrust = torch.zeros(num_instances, num_thrusters, device=device) + data.thruster_names = [f"thr_{i}" for i in range(num_thrusters)] + m._data = data + + # combined-wrench buffers + m._thrust_target_sim = torch.zeros_like(m._data.thrust_target) + m._internal_wrench_target_sim = torch.zeros(num_instances, 6, device=device) + m._internal_force_target_sim = torch.zeros(num_instances, m.num_bodies, 3, device=device) + m._internal_torque_target_sim = torch.zeros(num_instances, m.num_bodies, 3, device=device) + + # bind class methods we want to test onto the fake object + m._combine_thrusts = types.MethodType(Multirotor._combine_thrusts, m) + m.set_thrust_target = types.MethodType(Multirotor.set_thrust_target, m) + + return m + + +@pytest.mark.parametrize("num_instances", [1, 2, 4]) +@pytest.mark.parametrize("num_thrusters", [1, 2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_multirotor_combine_thrusts_unit(num_instances, num_thrusters, device): + m = make_multirotor_stub(num_instances=num_instances, num_thrusters=num_thrusters, device=torch.device(device)) + + # Create thrust target with predictable values + thrust_values = torch.arange(1.0, num_instances * num_thrusters + 1.0, device=device).reshape( + num_instances, num_thrusters + ) + m._thrust_target_sim = thrust_values + + # allocation maps first two thrusters to force x and y, rest to zero + # Create allocation matrix: 6 rows (wrench dims) x num_thrusters cols + alloc = [ + [ + 1.0 if r == 0 and c == 0 else 0.0 if c >= 2 else (1.0 if r == 1 and c == 1 else 0.0) + for c in range(num_thrusters) + ] + for r in range(6) + ] + m.cfg.allocation_matrix = alloc + m.allocation_matrix = torch.tensor(alloc, device=device) + + m._combine_thrusts() + + # Expected wrench: thrust @ allocation.T + alloc_t = torch.tensor(alloc, device=device) + expected = torch.matmul(thrust_values, alloc_t.T) + + assert torch.allclose(m._internal_wrench_target_sim, expected) + assert torch.allclose(m._internal_force_target_sim[:, 0, :], expected[:, :3]) + assert torch.allclose(m._internal_torque_target_sim[:, 0, :], expected[:, 3:]) + + +@pytest.mark.parametrize("num_instances", [1, 2, 4]) +@pytest.mark.parametrize("num_thrusters", [1, 2, 4]) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_thrust_target_broadcasting_unit(num_instances, num_thrusters, device): + m = make_multirotor_stub(num_instances=num_instances, num_thrusters=num_thrusters, device=torch.device(device)) + + # Set full-row targets for env 0 + targets = torch.arange(1.0, num_thrusters + 1.0, device=device).unsqueeze(0) + m.set_thrust_target(targets, thruster_ids=slice(None), env_ids=slice(0, 1)) + assert torch.allclose(m._data.thrust_target[0], targets[0]) + + # Set a column across all envs (use integer thruster id so broadcasting works) + # Use the last thruster to avoid index out of bounds + thruster_id = num_thrusters - 1 + column_values = torch.full((num_instances,), 9.0, device=device) + m.set_thrust_target(column_values, thruster_ids=thruster_id, env_ids=slice(None)) + assert torch.allclose(m._data.thrust_target[:, thruster_id], column_values) + + +def test_multirotor_data_annotations(): + from isaaclab_contrib.assets.multirotor.multirotor_data import MultirotorData + + # The class defines attributes for thruster state; the defaults should be None + md = MultirotorData.__new__(MultirotorData) + assert getattr(md, "default_thruster_rps", None) is None + assert getattr(md, "thrust_target", None) is None + assert getattr(md, "applied_thrust", None) is None + + +def test_set_thrust_target_env_slice_unit(): + """Setting targets for an env slice updates only those envs.""" + m = make_multirotor_stub(num_instances=4, num_thrusters=3) + + original = m._data.thrust_target.clone() + targets = torch.tensor([[1.0, 2.0, 3.0]], device=m.device) + # Update envs 1 and 2 + m.set_thrust_target(targets, thruster_ids=slice(None), env_ids=slice(1, 3)) + + assert torch.allclose(m._data.thrust_target[1:3], targets.repeat(2, 1)) + # other envs remain unchanged + assert torch.allclose(m._data.thrust_target[0], original[0]) + assert torch.allclose(m._data.thrust_target[3], original[3]) + + +def test_combine_thrusts_with_zero_allocation(): + """When allocation matrix is zero, combined wrench/force/torque are zero.""" + m = make_multirotor_stub(num_instances=2, num_thrusters=3) + + # zero allocation + zero_alloc = [[0.0 for _ in range(3)] for _ in range(6)] + m.cfg.allocation_matrix = zero_alloc + m.allocation_matrix = torch.zeros(6, 3, device=m.device) + + m._thrust_target_sim = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], device=m.device) + m._combine_thrusts() + + assert torch.allclose(m._internal_wrench_target_sim, torch.zeros_like(m._internal_wrench_target_sim)) + assert torch.allclose(m._internal_force_target_sim, torch.zeros_like(m._internal_force_target_sim)) + assert torch.allclose(m._internal_torque_target_sim, torch.zeros_like(m._internal_torque_target_sim)) + + +def test_arl_cfg_structure_and_counts(): + """Validate the ARL robot config structure (or a safe fallback).""" + # Use the ARL-provided config if available, otherwise synthesize a + # lightweight fallback so this test never skips. + cfg = ARL_ROBOT_1_CFG + if cfg is None: + cfg = types.SimpleNamespace() + # default allocation: 4 thrusters + cfg.allocation_matrix = [[1.0 if r < 2 and c == r else 0.0 for c in range(4)] for r in range(6)] + cfg.actuators = types.SimpleNamespace(thrusters=types.SimpleNamespace(dt=0.01)) + + # allocation matrix must be present and be a list of 6 rows (wrench dims) + assert hasattr(cfg, "allocation_matrix") + alloc = cfg.allocation_matrix + assert alloc is None or isinstance(alloc, list) + if alloc is not None: + assert len(alloc) == 6, "Allocation matrix must have 6 rows (wrench dims)" + assert all(isinstance(r, (list, tuple)) for r in alloc) + num_thr = len(alloc[0]) if len(alloc) > 0 else 0 + assert num_thr > 0, "Allocation matrix must contain at least one thruster column" + + # If actuators metadata exists, it should expose thruster timing values + if hasattr(cfg, "actuators") and cfg.actuators is not None: + thr = getattr(cfg.actuators, "thrusters", None) + if thr is not None: + assert hasattr(thr, "dt") + + +def test_arl_allocation_applies_to_stub(): + """Create a stub with the ARL allocation matrix (or fallback) and verify + `_combine_thrusts` produces the expected internal wrench via matrix + multiplication. + """ + cfg = ARL_ROBOT_1_CFG + if cfg is None or getattr(cfg, "allocation_matrix", None) is None: + # fallback allocation: simple mapping for 4 thrusters + alloc = [[1.0 if r < 2 and c == r else 0.0 for c in range(4)] for r in range(6)] + else: + alloc = cfg.allocation_matrix + + num_thr = len(alloc[0]) + m = make_multirotor_stub(num_instances=2, num_thrusters=num_thr) + # push allocation into the stub (both cfg view and tensor view) + m.cfg.allocation_matrix = alloc + m.allocation_matrix = torch.tensor(alloc, device=m.device) + + # Set a predictable thrust pattern and compute expected wrench manually. + m._thrust_target_sim = torch.tensor([[1.0] * num_thr, [2.0] * num_thr], device=m.device) + m._combine_thrusts() + + # expected: thrusts (N x T) @ allocation.T (T x 6) -> (N x 6) + alloc_t = torch.tensor(alloc, device=m.device) + expected = torch.matmul(m._thrust_target_sim, alloc_t.T) + + assert expected.shape == m._internal_wrench_target_sim.shape + assert torch.allclose(m._internal_wrench_target_sim, expected) + # also check split into force/torque matches + assert torch.allclose(m._internal_force_target_sim[:, 0, :], expected[:, :3]) + assert torch.allclose(m._internal_torque_target_sim[:, 0, :], expected[:, 3:]) + + +def generate_multirotor( + multirotor_cfg: MultirotorCfg, num_multirotors: int, device: str +) -> tuple[Multirotor, torch.Tensor]: + """Create scene prims and spawn `Multirotor` assets from a cfg. + + Mirrors the pattern used in `test_articulation.py`. + """ + translations = torch.zeros(num_multirotors, 3, device=device) + translations[:, 0] = torch.arange(num_multirotors) * 2.5 + + for i in range(num_multirotors): + prim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) + + # Replace the prim_path like other tests do and try to spawn a real + # Multirotor. If creating a full Multirotor fails (missing cfg fields + # or simulator not available) fall back to the simulator-free stub so + # tests can still run and validate behavior without IsaacSim. + try: + multirotor = Multirotor(multirotor_cfg.replace(prim_path="/World/Env_.*/Robot")) + return multirotor, translations + except Exception: + # Determine a reasonable number of thrusters for the stub from the + # cfg allocation matrix if provided, otherwise default to 4. + alloc = getattr(multirotor_cfg, "allocation_matrix", None) + num_thrusters = 4 + if isinstance(alloc, list) and len(alloc) > 0 and isinstance(alloc[0], (list, tuple)): + num_thrusters = len(alloc[0]) + + # Create a simulator-free multirotor stub bound to the same device. + stub = make_multirotor_stub(num_multirotors, num_thrusters, device=torch.device(device)) + return stub, translations + + +@pytest.fixture +def sim(request): + """Create a simulation context for integration tests (app + sim). + + Uses `build_simulation_context` from the project utils so tests match + `test_articulation.py` behaviour. + """ + device = request.getfixturevalue("device") if "device" in request.fixturenames else "cpu" + gravity_enabled = request.getfixturevalue("gravity_enabled") if "gravity_enabled" in request.fixturenames else True + add_ground_plane = ( + request.getfixturevalue("add_ground_plane") if "add_ground_plane" in request.fixturenames else False + ) + + with build_simulation_context( + device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane + ) as sim: + sim._app_control_on_stop_handle = None + yield sim + + +@pytest.mark.parametrize("num_multirotors", [1]) +@pytest.mark.parametrize("device", ["cpu"]) # restrict to cpu for CI without GPUs +@pytest.mark.isaacsim_ci +def test_multirotor_thruster_buffers_and_actuators(sim, num_multirotors, device): + """Check thruster buffers and actuator wiring in an integration environment. + + This test will be skipped automatically when `ARL_ROBOT_1_CFG` is not + available in the test environment (lightweight setups). + """ + cfg = generate_multirotor_cfg() + + # Try to create either a real multirotor or fall back to a stub; the + # generate_multirotor helper will return a stub when IsaacSim or a full + # cfg is not available so the test never skips. + multirotor, _ = generate_multirotor(cfg, num_multirotors, device=sim.device) + + # If we created a real multirotor, it should be initialized by the test + # scaffolding. If we got a stub, it won't have `is_initialized`. + if hasattr(multirotor, "is_initialized"): + sim.reset() + assert multirotor.is_initialized + + # If thruster buffers exist they should have the expected 2D shape + if hasattr(multirotor, "data") and getattr(multirotor.data, "thrust_target", None) is not None: + assert multirotor.data.thrust_target.ndim == 2 + + # Determine number of thrusters exposed by the asset or stub + try: + num_thr = multirotor.num_thrusters + except Exception: + # Stub exposes `_data` shape + num_thr = multirotor._data.thrust_target.shape[1] + + # Broadcast a simple thrust target and either step the sim (real) or + # emulate the actuator by combining thrusts on the stub. + multirotor.set_thrust_target(torch.ones(num_multirotors, num_thr, device=sim.device)) + if hasattr(multirotor, "update") and hasattr(sim, "step"): + for _ in range(3): + sim.step() + multirotor.update(sim.cfg.dt) + else: + # For the stub, emulate a single actuator update by combining thrusts + if hasattr(multirotor, "_combine_thrusts"): + multirotor._thrust_target_sim = multirotor._data.thrust_target.clone() + multirotor._combine_thrusts() + # set applied_thrust to computed_thrust to mimic an actuator + if hasattr(multirotor._data, "computed_thrust"): + multirotor._data.applied_thrust = multirotor._data.computed_thrust.clone() + + data_container = multirotor.data if hasattr(multirotor, "data") else multirotor._data + assert hasattr(data_container, "applied_thrust") + applied = data_container.applied_thrust + assert applied.shape == (num_multirotors, num_thr) + + +@pytest.mark.parametrize("num_multirotors", [1]) +@pytest.mark.parametrize("device", ["cpu"]) +@pytest.mark.isaacsim_ci +def test_set_thrust_target_broadcasting_integration(sim, num_multirotors, device): + """Ensure `set_thrust_target` broadcasting works in the integration context.""" + cfg = generate_multirotor_cfg() + multirotor, _ = generate_multirotor(cfg, num_multirotors, device=sim.device) + + # Determine number of thrusters for assertion (stub vs real asset) + # try: + # num_thr = multirotor.num_thrusters + # except Exception: + # num_thr = multirotor._data.thrust_target.shape[1] + + # Set a single-thruster column across all envs + multirotor.set_thrust_target( + torch.tensor([9.0] * num_multirotors, device=sim.device), thruster_ids=0, env_ids=slice(None) + ) + # Check that the first column of thrust_target has been updated + data = multirotor.data if hasattr(multirotor, "data") else multirotor._data + assert torch.allclose(data.thrust_target[:, 0], torch.tensor([9.0] * num_multirotors, device=sim.device)) + + # Minimal cleanup to avoid unraisable destructor warnings when a real + # Multirotor was created during the test. + if hasattr(multirotor, "_clear_callbacks"): + try: + for _a in ( + "_prim_deletion_callback_id", + "_initialize_handle", + "_invalidate_initialize_handle", + "_debug_vis_handle", + ): + if not hasattr(multirotor, _a): + setattr(multirotor, _a, None) + multirotor._clear_callbacks() + except Exception: + pass + with contextlib.suppress(Exception): + del multirotor diff --git a/source/isaaclab_contrib/test/sensors/test_visuotactile_render.py b/source/isaaclab_contrib/test/sensors/test_visuotactile_render.py new file mode 100644 index 000000000000..6b9902ea09fa --- /dev/null +++ b/source/isaaclab_contrib/test/sensors/test_visuotactile_render.py @@ -0,0 +1,133 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for GelSight utility functions - primarily focused on GelsightRender.""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +import os +import tempfile + +import cv2 +import numpy as np +import pytest +import torch + +from isaaclab_contrib.sensors.tacsl_sensor.visuotactile_render import GelsightRender +from isaaclab_contrib.sensors.tacsl_sensor.visuotactile_sensor_cfg import GelSightRenderCfg + + +def test_gelsight_render_custom_path_missing_file(): + """Test initializing GelsightRender with custom path when file doesn't exist.""" + # Assuming 'non_existent_path' is treated as a local path or Nucleus path + # If we pass a path that definitely doesn't exist locally or on Nucleus, it should fail + cfg = GelSightRenderCfg( + base_data_path="non_existent_path", + sensor_data_dir_name="dummy", + image_height=100, + image_width=100, + mm_per_pixel=0.1, + ) + # This should raise FileNotFoundError because the directory/files won't exist + with pytest.raises(FileNotFoundError): + GelsightRender(cfg, device="cpu") + + +def test_gelsight_render_custom_path_success(): + """Test initializing GelsightRender with valid custom path and files.""" + with tempfile.TemporaryDirectory() as tmpdir: + data_dir = "gelsight_r15_data" + full_dir = os.path.join(tmpdir, data_dir) + os.makedirs(full_dir, exist_ok=True) + + # Create dummy configuration + width, height = 10, 10 + cfg = GelSightRenderCfg( + base_data_path=tmpdir, + sensor_data_dir_name=data_dir, + image_width=width, + image_height=height, + num_bins=5, + mm_per_pixel=0.1, + ) + + # 1. Create dummy background image + bg_path = os.path.join(full_dir, cfg.background_path) + dummy_img = np.zeros((height, width, 3), dtype=np.uint8) + cv2.imwrite(bg_path, dummy_img) + + # 2. Create dummy calibration file + calib_path = os.path.join(full_dir, cfg.calib_path) + # Calibration gradients shape: (num_bins, num_bins, 6) + dummy_grad = np.zeros((cfg.num_bins, cfg.num_bins, 6), dtype=np.float32) + np.savez(calib_path, grad_r=dummy_grad, grad_g=dummy_grad, grad_b=dummy_grad) + + # Test initialization + try: + device = torch.device("cpu") + render = GelsightRender(cfg, device=device) + assert render is not None + assert render.device == device + # Verify loaded background dimensions + assert render.background.shape == (height, width, 3) + except Exception as e: + pytest.fail(f"GelsightRender initialization failed with valid custom files: {e}") + + +@pytest.fixture +def gelsight_render_setup(): + """Fixture to set up GelsightRender for testing with default (Nucleus/Cache) files.""" + # Use default GelSight R1.5 configuration + cfg = GelSightRenderCfg( + sensor_data_dir_name="gelsight_r15_data", image_height=320, image_width=240, mm_per_pixel=0.0877 + ) + device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") + + # Create render instance + try: + render = GelsightRender(cfg, device=device) + yield render, device + except Exception as e: + # If initialization fails (e.g., missing data files), skip tests + pytest.skip(f"GelsightRender initialization failed (likely network/Nucleus issue): {e}") + + +def test_gelsight_render_initialization(gelsight_render_setup): + """Test GelsightRender initialization with default files.""" + render, device = gelsight_render_setup + + # Check that render object was created + assert render is not None + assert render.device == device + + # Check that background was loaded (non-empty) + assert render.background is not None + assert render.background.size > 0 + assert render.background.shape[2] == 3 # RGB + + +def test_gelsight_render_compute(gelsight_render_setup): + """Test the render method of GelsightRender.""" + render, device = gelsight_render_setup + + # Create dummy height map + height, width = render.cfg.image_height, render.cfg.image_width + height_map = torch.zeros((1, height, width), device=device, dtype=torch.float32) + + # Add some features to height map + height_map[0, height // 4 : height // 2, width // 4 : width // 2] = 0.001 # 1mm bump + + # Render + output = render.render(height_map) + + # Check output + assert output is not None + assert output.shape == (1, height, width, 3) + assert output.dtype == torch.uint8 diff --git a/source/isaaclab_contrib/test/sensors/test_visuotactile_sensor.py b/source/isaaclab_contrib/test/sensors/test_visuotactile_sensor.py new file mode 100644 index 000000000000..d9929e7d6e14 --- /dev/null +++ b/source/isaaclab_contrib/test/sensors/test_visuotactile_sensor.py @@ -0,0 +1,450 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +"""Rest everything follows.""" + +import math + +import pytest +import torch + +import omni.replicator.core as rep + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, ArticulationCfg, RigidObject, RigidObjectCfg +from isaaclab.sensors.camera import TiledCameraCfg +from isaaclab.terrains.trimesh.utils import make_plane +from isaaclab.terrains.utils import create_prim_from_mesh +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +from isaaclab_contrib.sensors.tacsl_sensor import VisuoTactileSensor, VisuoTactileSensorCfg +from isaaclab_contrib.sensors.tacsl_sensor.visuotactile_sensor_cfg import GelSightRenderCfg + +# Sample sensor poses + +TEST_RENDER_CFG = GelSightRenderCfg( + sensor_data_dir_name="gelsight_r15_data", + image_height=320, + image_width=240, + mm_per_pixel=0.0877, +) + + +def get_sensor_cfg_by_type(sensor_type: str) -> VisuoTactileSensorCfg: + """Return a sensor configuration based on the input type. + + Args: + sensor_type: Type of sensor configuration. Options: "minimum_config", "tactile_cam", "nut_rgb_ff". + + Returns: + The sensor configuration for the specified type. + + Raises: + ValueError: If the sensor_type is not supported. + """ + + if sensor_type == "minimum_config": + return VisuoTactileSensorCfg( + prim_path="/World/Robot/elastomer/sensor_minimum_config", + enable_camera_tactile=False, + enable_force_field=False, + render_cfg=TEST_RENDER_CFG, + tactile_array_size=(10, 10), + tactile_margin=0.003, + ) + elif sensor_type == "tactile_cam": + return VisuoTactileSensorCfg( + prim_path="/World/Robot/elastomer/tactile_cam", + enable_force_field=False, + camera_cfg=TiledCameraCfg( + height=320, + width=240, + prim_path="/World/Robot/elastomer_tip/cam", + update_period=0, + data_types=["distance_to_image_plane"], + spawn=None, + ), + render_cfg=TEST_RENDER_CFG, + tactile_array_size=(10, 10), + tactile_margin=0.003, + ) + + elif sensor_type == "nut_rgb_ff": + return VisuoTactileSensorCfg( + prim_path="/World/Robot/elastomer/sensor_nut", + update_period=0, + debug_vis=False, + enable_camera_tactile=True, + enable_force_field=True, + camera_cfg=TiledCameraCfg( + height=320, + width=240, + prim_path="/World/Robot/elastomer_tip/cam", + update_period=0, + data_types=["distance_to_image_plane"], + spawn=None, + ), + render_cfg=TEST_RENDER_CFG, + tactile_array_size=(5, 10), + tactile_margin=0.003, + contact_object_prim_path_expr="/World/Nut", + ) + + else: + raise ValueError( + f"Unsupported sensor type: {sensor_type}. Supported types: 'minimum_config', 'tactile_cam', 'nut_rgb_ff'" + ) + + +def setup(sensor_type: str = "cube"): + """Create a new stage and setup simulation environment with robot, objects, and sensor. + + Args: + sensor_type: Type of sensor configuration. Options: "minimum_config", "tactile_cam", "nut_rgb_ff". + + Returns: + Tuple containing simulation context, sensor config, timestep, robot config, cube config, and nut config. + """ + # Create a new stage + sim_utils.create_new_stage() + + # Simulation time-step + dt = 0.01 + + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt) + sim = sim_utils.SimulationContext(sim_cfg) + + # Ground-plane + mesh = make_plane(size=(100, 100), height=0.0, center_zero=True) + create_prim_from_mesh("/World/defaultGroundPlane", mesh) + + # gelsightr15 filter + usd_file_path = f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd" + + # robot + robot_cfg = ArticulationCfg( + prim_path="/World/Robot", + spawn=sim_utils.UsdFileWithCompliantContactCfg( + usd_path=usd_file_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + compliant_contact_stiffness=10.0, + compliant_contact_damping=1.0, + physics_material_prim_path="elastomer", + ), + actuators={}, + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(math.sqrt(2) / 2, -math.sqrt(2) / 2, 0.0, 0.0), # 90ยฐ rotation + joint_pos={}, + joint_vel={}, + ), + ) + # Cube + cube_cfg = RigidObjectCfg( + prim_path="/World/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.1, 0.1, 0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + ) + # Nut + nut_cfg = RigidObjectCfg( + prim_path="/World/Nut", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_nut_m16.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False), + articulation_props=sim_utils.ArticulationRootPropertiesCfg(articulation_enabled=False), + mass_props=sim_utils.MassPropertiesCfg(mass=0.1), + ), + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.0, 0.0 + 0.06776, 0.52), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + + # Get the requested sensor configuration using the factory function + sensor_cfg = get_sensor_cfg_by_type(sensor_type) + + # load stage + sim_utils.update_stage() + return sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg + + +def teardown(sim): + """Teardown simulation environment.""" + # close all the opened viewport from before. + rep.vp_manager.destroy_hydra_textures("Replicator") + # stop simulation + # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( + sim._timeline.stop() + # clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.fixture +def setup_minimum_config(): + """Create simulation context with minimum config sensor.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("minimum_config") + yield sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg + teardown(sim) + + +@pytest.fixture +def setup_tactile_cam(): + """Create simulation context with tactile camera sensor.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("tactile_cam") + yield sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg + teardown(sim) + + +@pytest.fixture +def setup_nut_rgb_ff(): + """Create simulation context with nut RGB force field sensor.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg = setup("nut_rgb_ff") + yield sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_minimum_config(setup_minimum_config): + """Test sensor with minimal configuration (no camera, no force field).""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup_minimum_config + _ = Articulation(cfg=robot_cfg) + sensor_minimum = VisuoTactileSensor(cfg=sensor_cfg) + sim.reset() + # Simulate physics + for _ in range(10): + sim.step() + sensor_minimum.update(dt) + + # check data should be None, since both camera and force field are disabled + assert sensor_minimum.data.tactile_depth_image is None + assert sensor_minimum.data.tactile_rgb_image is None + assert sensor_minimum.data.tactile_points_pos_w is None + assert sensor_minimum.data.tactile_points_quat_w is None + assert sensor_minimum.data.penetration_depth is None + assert sensor_minimum.data.tactile_normal_force is None + assert sensor_minimum.data.tactile_shear_force is None + + # Check reset functionality + sensor_minimum.reset() + + for i in range(10): + sim.step() + sensor_minimum.update(dt) + sensor_minimum.reset(env_ids=[0]) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_size_false(setup_tactile_cam): + """Test sensor initialization fails with incorrect camera image size.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup_tactile_cam + sensor_cfg.camera_cfg.height = 80 + _ = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(ValueError) as excinfo: + sim.reset() + assert "Camera configuration image size is not consistent with the render config" in str(excinfo.value) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_type_false(setup_tactile_cam): + """Test sensor initialization fails with unsupported camera data types.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup_tactile_cam + sensor_cfg.camera_cfg.data_types = ["rgb"] + _ = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(ValueError) as excinfo: + sim.reset() + assert "Camera configuration data types are not supported" in str(excinfo.value) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_set(setup_tactile_cam): + """Test sensor with camera configuration using existing camera prim.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup_tactile_cam + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt, force_recompute=True) + robot.update(dt) + assert sensor.is_initialized + assert sensor.data.tactile_depth_image.shape == (1, 320, 240, 1) + assert sensor.data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert sensor.data.tactile_points_pos_w is None + + sensor.reset() + for _ in range(10): + sim.step() + sensor.update(dt, force_recompute=True) + robot.update(dt) + sensor.reset(env_ids=[0]) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_set_wrong_prim(setup_tactile_cam): + """Test sensor initialization fails with invalid camera prim path.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup_tactile_cam + sensor_cfg.camera_cfg.prim_path = "/World/Robot/elastomer_tip/cam_wrong" + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(RuntimeError) as excinfo: + sim.reset() + robot.update(dt) + sensor.update(dt) + assert "Could not find prim with path" in str(excinfo.value) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_new_spawn(setup_tactile_cam): + """Test sensor with camera configuration that spawns a new camera.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup_tactile_cam + sensor_cfg.camera_cfg.prim_path = "/World/Robot/elastomer_tip/cam_new" + sensor_cfg.camera_cfg.spawn = sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.01, 1.0e5) + ) + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt) + robot.update(dt) + # test lazy sensor update + data = sensor.data + assert data is not None + assert data.tactile_depth_image.shape == (1, 320, 240, 1) + assert data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert data.tactile_points_pos_w is None + + assert sensor.is_initialized + + +@pytest.mark.isaacsim_ci +def test_sensor_rgb_forcefield(setup_nut_rgb_ff): + """Test sensor with both camera and force field enabled, detecting contact forces.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg = setup_nut_rgb_ff + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + nut = RigidObject(cfg=nut_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt, force_recompute=True) + robot.update(dt) + nut.update(dt) + # check str + print(sensor) + assert sensor.is_initialized + assert sensor.data.tactile_depth_image.shape == (1, 320, 240, 1) + assert sensor.data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert sensor.data.tactile_points_pos_w.shape == (1, 50, 3) + assert sensor.data.penetration_depth.shape == (1, 50) + assert sensor.data.tactile_normal_force.shape == (1, 50) + assert sensor.data.tactile_shear_force.shape == (1, 50, 2) + sum_depth = torch.sum(sensor.data.penetration_depth) # 0.020887471735477448 + normal_force_sum = torch.sum(sensor.data.tactile_normal_force.abs()) + shear_force_sum = torch.sum(sensor.data.tactile_shear_force.abs()) + assert normal_force_sum > 0.0 + assert sum_depth > 0.0 + assert shear_force_sum > 0.0 + + +@pytest.mark.isaacsim_ci +def test_sensor_no_contact_object(setup_nut_rgb_ff): + """Test sensor with force field but no contact object specified.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg = setup_nut_rgb_ff + sensor_cfg.contact_object_prim_path_expr = None + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + nut = RigidObject(cfg=nut_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt, force_recompute=True) + robot.update(dt) + nut.update(dt) + + assert sensor.is_initialized + assert sensor.data.tactile_depth_image.shape == (1, 320, 240, 1) + assert sensor.data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert sensor.data.tactile_points_pos_w.shape == (1, 50, 3) + # check no forces are detected + assert torch.all(torch.abs(sensor.data.penetration_depth) < 1e-9) + assert torch.all(torch.abs(sensor.data.tactile_normal_force) < 1e-9) + assert torch.all(torch.abs(sensor.data.tactile_shear_force) < 1e-9) + + +@pytest.mark.isaacsim_ci +def test_sensor_force_field_contact_object_not_found(setup_nut_rgb_ff): + """Test sensor initialization fails when contact object prim path is not found.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, NutCfg = setup_nut_rgb_ff + + sensor_cfg.enable_camera_tactile = False + sensor_cfg.contact_object_prim_path_expr = "/World/Nut/wrong_prim" + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(RuntimeError) as excinfo: + sim.reset() + robot.update(dt) + sensor.update(dt) + assert "No contact object prim found matching pattern" in str(excinfo.value) + + +@pytest.mark.isaacsim_ci +def test_sensor_force_field_contact_object_no_sdf(setup_nut_rgb_ff): + """Test sensor initialization fails when contact object has no SDF mesh.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, NutCfg = setup_nut_rgb_ff + sensor_cfg.enable_camera_tactile = False + sensor_cfg.contact_object_prim_path_expr = "/World/Cube" + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + cube = RigidObject(cfg=cube_cfg) + with pytest.raises(RuntimeError) as excinfo: + sim.reset() + robot.update(dt) + sensor.update(dt) + cube.update(dt) + assert "No SDF mesh found under contact object at path" in str(excinfo.value) + + +@pytest.mark.isaacsim_ci +def test_sensor_update_period_mismatch(setup_nut_rgb_ff): + """Test sensor with both camera and force field enabled, detecting contact forces.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg = setup_nut_rgb_ff + sensor_cfg.update_period = dt + sensor_cfg.camera_cfg.update_period = dt * 2 + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + nut = RigidObject(cfg=nut_cfg) + sim.reset() + sensor.get_initial_render() + assert sensor.cfg.camera_cfg.update_period == sensor.cfg.update_period + for i in range(10): + sim.step() + sensor.update(dt, force_recompute=True) + robot.update(dt) + nut.update(dt) + assert torch.allclose(sensor._timestamp_last_update, torch.tensor((i + 1) * dt, device=sensor.device)) + assert torch.allclose( + sensor._camera_sensor._timestamp_last_update, torch.tensor((i + 1) * dt, device=sensor.device) + ) diff --git a/source/isaaclab_mimic/config/extension.toml b/source/isaaclab_mimic/config/extension.toml index 7be1bdb07238..5b498ae58652 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.3" +version = "1.0.16" # Description category = "isaaclab" diff --git a/source/isaaclab_mimic/docs/CHANGELOG.rst b/source/isaaclab_mimic/docs/CHANGELOG.rst index fe44f89f2c9b..09b79c8cdd80 100644 --- a/source/isaaclab_mimic/docs/CHANGELOG.rst +++ b/source/isaaclab_mimic/docs/CHANGELOG.rst @@ -1,7 +1,111 @@ Changelog --------- -1.0.3 (2025-03-10) + +1.0.16 (2025-11-10) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Add body end effector to Mimic data generation to enable loco-manipulation data generation when a navigation p-controller is provided. + + +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) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added SkillGen integration for automated demonstration generation using cuRobo; enable via ``--use_skillgen`` in ``scripts/imitation_learning/isaaclab_mimic/generate_dataset.py``. +* Added cuRobo motion planner interface (:class:`CuroboPlanner`, :class:`CuroboPlannerCfg`) +* Added manual subtask start boundary annotation for SkillGen; enable via ``--annotate_subtask_start_signals`` in ``scripts/imitation_learning/isaaclab_mimic/annotate_demos.py``. +* Added Rerun integration for motion plan visualization and debugging; enable via ``visualize_plan = True`` in :class:`CuroboPlannerCfg`. + + +1.0.13 (2025-08-14) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`PickPlaceGR1T2WaistEnabledEnvCfg` and :class:`PickPlaceGR1T2WaistEnabledMimicEnvCfg` for GR1T2 robot manipulation tasks with waist joint control enabled. + +1.0.12 (2025-07-31) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``from __future__ import annotations`` to utils.py to fix Sphinx + doc warnings for IsaacLab Mimic docs. + + +1.0.11 (2025-07-17) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated test_selection_strategy.py and test_generate_dataset.py test cases to pytest format. +* Updated annotate_demos.py script to return the number of successful task completions as the exit code to support check in test_generate_dataset.py test case. + + +1.0.10 (2025-07-08) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated generate dataset script to cancel remaining async tasks before closing the simulation app. + + +1.0.9 (2025-05-20) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-Mimic-v0`` environment for Cosmos vision stacking. + + +1.0.8 (2025-05-01) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`NutPourGR1T2MimicEnv` and :class:`ExhaustPipeGR1T2MimicEnv` for the GR1T2 nut pouring and exhaust pipe tasks. +* Updated instruction display to support all XR handtracking devices. + + +1.0.7 (2025-03-19) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Moved the GR1T2 robot task to a separate directory to prevent import of pinocchio when not needed. This allows use of IsaacLab Mimic in windows. + + +1.0.6 (2025-03-10) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`FrankaCubeStackIKAbsMimicEnv` and support for the GR1T2 robot task (:class:`PickPlaceGR1T2MimicEnv`). + + +1.0.5 (2025-03-10) ~~~~~~~~~~~~~~~~~~ Changed @@ -15,6 +119,25 @@ Added * Added ``Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-Mimic-v0`` environment for blueprint vision stacking. +1.0.4 (2025-03-07) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated data generator to support environments with multiple end effectors. +* Updated data generator to support subtask constraints based on DexMimicGen. + + +1.0.3 (2025-03-06) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^^ + +* Added absolute pose mimic environment for Franka cube stacking task (:class:`FrankaCubeStackIKAbsMimicEnv`) + + 1.0.2 (2025-01-10) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_mimic/isaaclab_mimic/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/__init__.py index 277355fe335f..17f1264a6b59 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/__init__.py index dd377380185d..3ede9a65cb67 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py index 2681e2d9a6f6..37fabe4a4b47 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py @@ -1,60 +1,174 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 -""" -Base class for data generator. -""" +"""Base class for data generator.""" + import asyncio +import copy +import logging +from typing import Any + import numpy as np import torch import isaaclab.utils.math as PoseUtils -from isaaclab.envs.mimic_env_cfg import MimicEnvCfg + +logger = logging.getLogger(__name__) + +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 WaypointSequence, WaypointTrajectory +from isaaclab_mimic.datagen.waypoint import MultiWaypoint, Waypoint, WaypointSequence, WaypointTrajectory from .datagen_info_pool import DataGenInfoPool -class DataGenerator: +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]) """ - The main data generator object that loads a source dataset, parses it, and - generates new trajectories. + 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 + :class:`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, - src_demo_datagen_info_pool=None, - dataset_path=None, - demo_keys=None, + env: ManagerBasedRLMimicEnv, + src_demo_datagen_info_pool: DataGenInfoPool | None = None, + dataset_path: str | None = None, + demo_keys: list[str] | None = None, ): """ Args: - env (Isaac Lab ManagerBasedEnv instance): environment to use for data generation - src_demo_datagen_info_pool (DataGenInfoPool): source demo datagen info pool - dataset_path (str): path to hdf5 dataset to use for generation - demo_keys (list of str): list of demonstration keys to use - in file. If not provided, all demonstration keys will be - used. + 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 - if len(self.env_cfg.subtask_configs) != 1: - raise ValueError("Data generation currently supports only one end-effector.") - - (self.eef_name,) = self.env_cfg.subtask_configs.keys() - (self.subtask_configs,) = self.env_cfg.subtask_configs.values() - # sanity check on task spec offset ranges - final subtask should not have any offset randomization - assert self.subtask_configs[-1].subtask_term_offset_range[0] == 0 - assert self.subtask_configs[-1].subtask_term_offset_range[1] == 0 + # 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 @@ -69,97 +183,116 @@ def __init__( raise ValueError("Either src_demo_datagen_info_pool or dataset_path must be provided") def __repr__(self): - """ - Pretty print this object. - """ + """Pretty print this object.""" msg = str(self.__class__.__name__) - msg += " (\n\tdataset_path={}\n\tdemo_keys={}\n)".format( - self.dataset_path, - self.demo_keys, - ) + msg += f" (\n\tdataset_path={self.dataset_path}\n\tdemo_keys={self.demo_keys}\n)" return msg - def randomize_subtask_boundaries(self): - """ - Apply random offsets to sample subtask boundaries according to the task spec. + 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. + end index (and start index when skillgen is enabled) of each subtask can have a random offset. """ - # initial subtask start and end indices - shape (N, S, 2) - src_subtask_indices = np.array(self.src_demo_datagen_info_pool.subtask_indices) + 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) - # 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(src_subtask_indices.shape[1] - 1): - end_offsets = np.random.randint( - low=self.subtask_configs[i].subtask_term_offset_range[0], - high=self.subtask_configs[i].subtask_term_offset_range[1] + 1, - size=src_subtask_indices.shape[0], + # 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], ) - src_subtask_indices[:, i, 1] = src_subtask_indices[:, i, 1] + end_offsets - # don't forget to set these as start indices for next subtask too - src_subtask_indices[:, i + 1, 0] = src_subtask_indices[:, i, 1] + subtask_boundaries[:, 0, 0] += first_subtask_start_offsets + + # For each subtask, 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]): + # If skillgen is enabled, sample a random start offset to increase demonstration variety. + if self.env_cfg.datagen_config.use_skillgen: + start_offset = np.random.randint( + low=self.env_cfg.subtask_configs[eef_name][i].subtask_start_offset_range[0], + high=self.env_cfg.subtask_configs[eef_name][i].subtask_start_offset_range[1] + 1, + size=subtask_boundaries.shape[0], + ) + subtask_boundaries[:, i, 0] += start_offset + elif i > 0: + # Without skillgen, the start of a subtask is the end of the previous one. + subtask_boundaries[:, i, 0] = subtask_boundaries[:, i - 1, 1] + + # Sample end offset for each demonstration + 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 - # ensure non-empty subtasks - assert np.all((src_subtask_indices[:, :, 1] - src_subtask_indices[:, :, 0]) > 0), "got empty subtasks!" + # 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 subtask indices increase (both starts and ends) - assert np.all( - (src_subtask_indices[:, 1:, :] - src_subtask_indices[:, :-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" - # ensure subtasks are in order - subtask_inds_flat = src_subtask_indices.reshape(src_subtask_indices.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 src_subtask_indices + return randomized_subtask_boundaries def select_source_demo( self, - eef_pose, - object_pose, - subtask_ind, - src_subtask_inds, - subtask_object_name, - selection_strategy_name, - selection_strategy_kwargs=None, - ): - """ - Helper method to run source subtask segment selection. + 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_pose (np.array): current end effector pose - object_pose (np.array): current object pose for this subtask - subtask_ind (int): index of subtask - src_subtask_inds (np.array): start and end indices for subtask segment in source demonstrations of shape (N, 2) - subtask_object_name (str): name of reference object for this subtask - selection_strategy_name (str): name of selection strategy - selection_strategy_kwargs (dict): extra kwargs for running selection strategy + 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 (int): selected source demo index + The selected source demo index """ if subtask_object_name is None: # no reference object - only random selection is supported - assert selection_strategy_name == "random" + 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 + # 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_subtask_inds[i][0] - subtask_end_ind = src_subtask_inds[i][1] + # 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 + # Get subtask segment using indices src_subtask_datagen_infos.append( DatagenInfo( - eef_pose=src_ep_datagen_info.eef_pose[subtask_start_ind:subtask_end_ind], - # only include object pose for relevant object in subtask + 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][ @@ -169,17 +302,17 @@ def select_source_demo( if (subtask_object_name is not None) else None ), - # subtask termination signal is unused + # Subtask termination signal is unused subtask_term_signals=None, - target_eef_pose=src_ep_datagen_info.target_eef_pose[subtask_start_ind:subtask_end_ind], - gripper_action=src_ep_datagen_info.gripper_action[subtask_start_ind:subtask_end_ind], + 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 + # Make selection strategy object selection_strategy_obj = make_selection_strategy(selection_strategy_name) - # run selection + # Run selection if selection_strategy_kwargs is None: selection_strategy_kwargs = dict() selected_src_demo_ind = selection_strategy_obj.select_source_demo( @@ -191,244 +324,669 @@ def select_source_demo( return selected_src_demo_ind - async def generate( + def generate_eef_subtask_trajectory( self, - env_id, - success_term, - env_action_queue: asyncio.Queue | None = None, - select_src_per_subtask=False, - transform_first_robot_pose=False, - interpolate_from_last_target_pose=True, - pause_subtask=False, - export_demo=True, - ): - """ - Attempt to generate a new demonstration. + env_id: int, + eef_name: str, + subtask_ind: int, + all_randomized_subtask_boundaries: dict, + runtime_subtask_constraints_dict: dict, + selected_src_demo_inds: dict, + ) -> WaypointTrajectory: + """Build a transformed waypoint trajectory for a single subtask of an end-effector. + + This method selects a source demonstration segment for the specified subtask, + slices the corresponding EEF poses/targets/gripper actions using the randomized + subtask boundaries, optionally prepends the first robot EEF pose (to interpolate + from the robot pose instead of the first target), applies an object/coordination + based transform to the pose sequence, and returns the result as a `WaypointTrajectory`. + + Selection and transforms: + + - Source demo selection is controlled by `SubTaskConfig.selection_strategy` (and kwargs) and by + `datagen_config.generation_select_src_per_subtask` / `generation_select_src_per_arm`. + - For coordination constraints, the method reuses/sets the selected source demo ID across + concurrent subtasks, computes `synchronous_steps`, and stores the pose `transform` used + to ensure consistent relative motion between tasks. + - Pose transforms are computed either from object poses (`object_ref`) or via a delta pose + provided by a concurrent task/coordination scheme. + Args: - env_id (int): environment ID + env_id: Environment index used to query current robot/object poses. + eef_name: End-effector key whose subtask trajectory is being generated. + subtask_ind: Index of the subtask within `subtask_configs[eef_name]`. + all_randomized_subtask_boundaries: For each EEF, an array of per-demo + randomized (start, end) indices for every subtask. + runtime_subtask_constraints_dict: In/out dictionary carrying runtime fields + for constraints (e.g., selected source ID, delta transform, synchronous steps). + selected_src_demo_inds: Per-EEF mapping for the currently selected source demo index + (may be reused across arms if configured). + + Returns: + WaypointTrajectory: The transformed trajectory for the selected subtask segment. + """ + 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 + # This transform is set at after the first data generation iteration/first + # run of the main while loop + 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, + ) - success_term (TerminationTermCfg): success function to check if the task is successful + assert selected_src_demo_inds[eef_name] is not None + selected_src_demo_ind = selected_src_demo_inds[eef_name] - env_action_queue (asyncio.Queue): queue to store actions for each environment + 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 - select_src_per_subtask (bool): if True, select a different source demonstration for each subtask - during data generation, else keep the same one for the entire episode + # Selected subtask segment time indices + selected_src_subtask_boundary = all_randomized_subtask_boundaries[eef_name][selected_src_demo_ind, subtask_ind] - transform_first_robot_pose (bool): if True, each subtask segment will consist of the first - robot pose and the target poses instead of just the target poses. Can sometimes help - improve data generation quality as the interpolation segment will interpolate to where - the robot started in the source segment instead of the first target pose. Note that the - first subtask segment of each episode will always include the first robot pose, regardless - of this argument. + 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 + ) - interpolate_from_last_target_pose (bool): if True, each interpolation segment will start from - the last target pose in the previous subtask segment, instead of the current robot pose. Can - sometimes improve data generation quality. + # 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 + ) - pause_subtask (bool): if True, pause after every subtask during generation, for - debugging. + 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 + ) + + 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 + + # 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) + + return transformed_traj + + def merge_eef_subtask_trajectory( + self, + env_id: int, + eef_name: str, + subtask_index: int, + prev_executed_traj: list[Waypoint] | None, + subtask_trajectory: WaypointTrajectory, + ) -> list[Waypoint]: + """Merge a subtask trajectory into an executable trajectory for the robot end-effector. + + This constructs a new `WaypointTrajectory` by first creating an initial + interpolation segment, then merging the provided `subtask_trajectory` onto it. + The initial segment begins either from the last executed target waypoint of the + previous subtask (if configured) or from the robot's current end-effector pose. + + Behavior: + + - If `datagen_config.generation_interpolate_from_last_target_pose` is True and + this is not the first subtask, interpolation starts from the last waypoint of + `prev_executed_traj`. + - Otherwise, interpolation starts from the current robot EEF pose (queried from the env) + and uses the first waypoint's gripper action and the subtask's action noise. + - The merge uses `num_interpolation_steps`, `num_fixed_steps`, and optionally + `apply_noise_during_interpolation` from the corresponding `SubTaskConfig`. + - The temporary initial waypoint used to enable interpolation is removed before returning. + + Args: + env_id: Environment index to query the current robot EEF pose when needed. + eef_name: Name/key of the end-effector whose trajectory is being merged. + subtask_index: Index of the subtask within `subtask_configs[eef_name]` driving interpolation parameters. + prev_executed_traj: The previously executed trajectory used to + seed interpolation from its last target waypoint. Required when interpolation-from-last-target + is enabled and this is not the first subtask. + subtask_trajectory: + Trajectory segment for the current subtask that will be merged after the initial interpolation segment. + + Returns: + The full sequence of waypoints to execute (initial interpolation segment followed by the subtask segment), + with the temporary initial waypoint removed. + """ + is_first_subtask = subtask_index == 0 + # We will construct a WaypointTrajectory instance to keep track of robot control targets + # and then execute it once we have the trajectory. + 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 is not None + last_waypoint = prev_executed_traj[-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=subtask_trajectory[0].gripper_action.unsqueeze(0), + action_noise=self.env_cfg.subtask_configs[eef_name][subtask_index].action_noise, + ) + traj_to_execute.add_waypoint_sequence(init_sequence) + + # 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( + subtask_trajectory, + num_steps_interp=self.env_cfg.subtask_configs[eef_name][subtask_index].num_interpolation_steps, + num_steps_fixed=self.env_cfg.subtask_configs[eef_name][subtask_index].num_fixed_steps, + action_noise=( + float(self.env_cfg.subtask_configs[eef_name][subtask_index].apply_noise_during_interpolation) + * self.env_cfg.subtask_configs[eef_name][subtask_index].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( # noqa: C901 + 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, + motion_planner: Any | None = None, + ) -> dict: + """Attempt to generate a new demonstration. + + Args: + env_id: environment ID + 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: whether to pause the subtask generation + export_demo: whether to export the demo + motion_planner: motion planner to use for motion planning Returns: - results (dict): 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 + A dictionary containing 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. """ - eef_names = list(self.env_cfg.subtask_configs.keys()) - eef_name = eef_names[0] + # With skillgen, a motion planner is required to generate collision-free transitions between subtasks. + if self.env_cfg.datagen_config.use_skillgen and motion_planner is None: + raise ValueError("motion_planner must be provided if use_skillgen is True") # 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) - self.env.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) - # some state variables used during generation - selected_src_demo_ind = None - prev_executed_traj = None + # 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 - generated_src_demo_inds = [] # store selected src demo ind for each subtask in each trajectory - generated_src_demo_labels = ( - [] - ) # like @generated_src_demo_inds, but padded to align with size of @generated_actions - prev_src_demo_datagen_info_pool_size = 0 - for subtask_ind in range(len(self.subtask_configs)): - - # some things only happen on first subtask - is_first_subtask = subtask_ind == 0 + # some eef-specific state variables used during generation + current_eef_selected_src_demo_indices = {} + current_eef_subtask_trajectories: dict[str, list[Waypoint]] = {} + current_eef_subtask_indices = {} + next_eef_subtask_indices_after_motion = {} + next_eef_subtask_trajectories_after_motion = {} + 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 + current_eef_subtask_trajectories[eef_name] = [] # type of list of Waypoint + current_eef_subtask_indices[eef_name] = 0 + next_eef_subtask_indices_after_motion[eef_name] = None + next_eef_subtask_trajectories_after_motion[eef_name] = None + current_eef_subtask_step_indices[eef_name] = None + eef_subtasks_done[eef_name] = False - # name of object for this subtask - subtask_object_name = self.subtask_configs[subtask_ind].object_ref + prev_src_demo_datagen_info_pool_size = 0 - # corresponding current object pose - cur_object_pose = ( - self.env.get_object_poses(env_ids=[env_id])[subtask_object_name][0] - if (subtask_object_name is not None) - else None - ) + if self.env_cfg.datagen_config.use_navigation_controller: + was_navigating = False + # 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 - all_subtask_inds = ( + # So we need to update 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) - # We need source demonstration selection for the first subtask (always), and possibly for - # other subtasks if @select_src_per_subtask is set. - need_source_demo_selection = is_first_subtask or select_src_per_subtask - - # Run source demo selection or use selected demo from previous iteration - if need_source_demo_selection: - selected_src_demo_ind = self.select_source_demo( - eef_pose=self.env.get_robot_eef_pose(eef_name, env_ids=[env_id])[0], - object_pose=cur_object_pose, - subtask_ind=subtask_ind, - src_subtask_inds=all_subtask_inds[:, subtask_ind], - subtask_object_name=subtask_object_name, - selection_strategy_name=self.subtask_configs[subtask_ind].selection_strategy, - selection_strategy_kwargs=self.subtask_configs[subtask_ind].selection_strategy_kwargs, - ) - assert selected_src_demo_ind is not None - - # selected subtask segment time indices - selected_src_subtask_inds = all_subtask_inds[selected_src_demo_ind, subtask_ind] - - # 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[ - selected_src_subtask_inds[0] : selected_src_subtask_inds[1] - ] - src_subtask_target_poses = src_ep_datagen_info.target_eef_pose[ - selected_src_subtask_inds[0] : selected_src_subtask_inds[1] - ] - src_subtask_gripper_actions = src_ep_datagen_info.gripper_action[ - selected_src_subtask_inds[0] : selected_src_subtask_inds[1] - ] - - # get reference object pose from source demo - src_subtask_object_pose = ( - src_ep_datagen_info.object_poses[subtask_object_name][selected_src_subtask_inds[0]] - if (subtask_object_name is not None) - else None - ) - - if is_first_subtask or transform_first_robot_pose: - # Source segment consists of first robot eef pose and the target poses. - src_eef_poses = torch.cat([src_subtask_eef_poses[0:1], src_subtask_target_poses], dim=0) - else: - # Source segment consists of just the target poses. - src_eef_poses = src_subtask_target_poses.clone() - - # 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 + # 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: + # Trajectory stored in current_eef_subtask_trajectories[eef_name] has been executed, + # So we need to determine the next trajectory + # Note: This condition is the "resume-after-motion-plan" gate for skillgen. When + # use_skillgen=False (vanilla Mimic), next_eef_subtask_indices_after_motion[eef_name] + # remains None, so this condition is always True and the else-branch below is never taken. + # The else-branch is only used right after executing a motion-planned transition (skillgen) + # to resume the actual subtask trajectory. + if next_eef_subtask_indices_after_motion[eef_name] is None: + # This is the beginning of a new subtask, so generate a new trajectory accordingly + eef_subtask_trajectory = self.generate_eef_subtask_trajectory( + env_id, + eef_name, + current_eef_subtask_indices[eef_name], + randomized_subtask_boundaries, + runtime_subtask_constraints_dict, + current_eef_selected_src_demo_indices, # updated in the method + ) + # With skillgen, use a motion planner to transition between subtasks. + if self.env_cfg.datagen_config.use_skillgen: + # Define the goal for the motion planner: the start of the next subtask. + target_eef_pose = eef_subtask_trajectory[0].pose + target_gripper_action = eef_subtask_trajectory[0].gripper_action + + # Determine expected object attachment using environment-specific logic (optional) + expected_attached_object = None + if hasattr(self.env, "get_expected_attached_object"): + expected_attached_object = self.env.get_expected_attached_object( + eef_name, current_eef_subtask_indices[eef_name], self.env.cfg + ) + + # Plan motion using motion planner with comprehensive world update + # and attachment handling + if motion_planner: + print(f"\n--- Environment {env_id}: Planning motion to target pose ---") + print(f"Target pose: {target_eef_pose}") + print(f"Expected attached object: {expected_attached_object}") + + # This call updates the planner's world model and computes the trajectory. + planning_success = motion_planner.update_world_and_plan_motion( + target_pose=target_eef_pose, + expected_attached_object=expected_attached_object, + env_id=env_id, + step_size=getattr(motion_planner, "step_size", None), + enable_retiming=hasattr(motion_planner, "step_size") + and motion_planner.step_size is not None, + ) + + # If planning succeeds, execute the planner's trajectory first. + if planning_success: + print(f"Env {env_id}: Motion planning succeeded") + # The original subtask trajectory is stored to be executed after the transition. + next_eef_subtask_trajectories_after_motion[eef_name] = eef_subtask_trajectory + next_eef_subtask_indices_after_motion[eef_name] = current_eef_subtask_indices[ + eef_name + ] + # Mark the current subtask as invalid (-1) until the transition is done. + current_eef_subtask_indices[eef_name] = -1 + + # Convert the planner's output into a sequence of waypoints to be executed. + current_eef_subtask_trajectories[eef_name] = ( + self._convert_planned_trajectory_to_waypoints( + motion_planner, target_gripper_action + ) + ) + current_eef_subtask_step_indices[eef_name] = 0 + print( + f"Generated {len(current_eef_subtask_trajectories[eef_name])} waypoints" + " from motion plan" + ) + + else: + # If planning fails, abort the data generation trial. + print(f"Env {env_id}: Motion planning failed for {eef_name}") + return {"success": False} + else: + # Without skillgen, transition using simple interpolation. + current_eef_subtask_trajectories[eef_name] = self.merge_eef_subtask_trajectory( + env_id, + eef_name, + current_eef_subtask_indices[eef_name], + current_eef_subtask_trajectories[eef_name], + eef_subtask_trajectory, + ) + current_eef_subtask_step_indices[eef_name] = 0 + else: + # Motion-planned trajectory has been executed, so we are ready to move to + # execute the next subtask + print("Finished executing motion-planned trajectory") + # It is important to pass the prev_executed_traj to merge_eef_subtask_trajectory + # so that it can correctly interpolate from the last pose of the motion-planned trajectory + prev_executed_traj = current_eef_subtask_trajectories[eef_name] + current_eef_subtask_indices[eef_name] = next_eef_subtask_indices_after_motion[eef_name] + current_eef_subtask_trajectories[eef_name] = self.merge_eef_subtask_trajectory( + env_id, + eef_name, + current_eef_subtask_indices[eef_name], + prev_executed_traj, + next_eef_subtask_trajectories_after_motion[eef_name], + ) + current_eef_subtask_step_indices[eef_name] = 0 + next_eef_subtask_trajectories_after_motion[eef_name] = None + next_eef_subtask_indices_after_motion[eef_name] = None + + # 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] + + # Update visualization if motion planner is available + if motion_planner and motion_planner.visualize_spheres: + current_joints = self.env.scene["robot"].data.joint_pos[env_id] + motion_planner._update_visualization_at_joint_positions(current_joints) + + 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, ) - # Transform source demonstration segment using relevant object pose. - if subtask_object_name is not None: - transformed_eef_poses = PoseUtils.transform_poses_from_frame_A_to_frame_B( - src_poses=src_eef_poses, - frame_A=cur_object_pose, - frame_B=src_subtask_object_pose, - ) - else: - # 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 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 is not None - last_waypoint = prev_executed_traj.last_waypoint - init_sequence = WaypointSequence(sequence=[last_waypoint]) - else: - # Interpolation segment will start from current robot eef pose. - init_sequence = WaypointSequence.from_poses( - eef_names=eef_names, - poses=self.env.get_robot_eef_pose(eef_name, env_ids=[env_id])[0][None], - gripper_actions=src_subtask_gripper_actions[0:1], - action_noise=self.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( - eef_names=eef_names, - poses=transformed_eef_poses, - gripper_actions=src_subtask_gripper_actions, - action_noise=self.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, - eef_names=eef_names, - num_steps_interp=self.subtask_configs[subtask_ind].num_interpolation_steps, - num_steps_fixed=self.subtask_configs[subtask_ind].num_fixed_steps, - action_noise=( - float(self.subtask_configs[subtask_ind].apply_noise_during_interpolation) - * self.subtask_configs[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() - - # Execute the trajectory and collect data. - exec_results = await traj_to_execute.execute( - env=self.env, env_id=env_id, env_action_queue=env_action_queue, success_term=success_term - ) - - # check that trajectory is non-empty + # Update execution state buffers if len(exec_results["states"]) > 0: - generated_states += exec_results["states"] - generated_obs += exec_results["observations"] - generated_actions.append(exec_results["actions"]) + 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"] - generated_src_demo_inds.append(selected_src_demo_ind) - generated_src_demo_labels.append( - selected_src_demo_ind - * torch.ones( - (exec_results["actions"].shape[0], 1), dtype=torch.int, device=exec_results["actions"].device - ) - ) - - # remember last trajectory - prev_executed_traj = traj_to_execute - - if pause_subtask: - input(f"Pausing after subtask {subtask_ind} execution. Press any key to continue...") - # merge numpy arrays + # Get the navigation state + if self.env_cfg.datagen_config.use_navigation_controller: + processed_nav_subtask = False + navigation_state = self.env.get_navigation_state(env_id) + assert navigation_state is not None, "Navigation state cannot be None when using navigation controller" + is_navigating = navigation_state["is_navigating"] + navigation_goal_reached = navigation_state["navigation_goal_reached"] + + for eef_name in self.env_cfg.subtask_configs.keys(): + current_eef_subtask_step_indices[eef_name] += 1 + + # Execute locomanip navigation controller if it is enabled via the use_navigation_controller flag + if self.env_cfg.datagen_config.use_navigation_controller: + if "body" not in self.env_cfg.subtask_configs.keys(): + error_msg = ( + 'End effector with name "body" not found in subtask configs. "body" must be a valid end' + " effector to use the navigation controller.\n" + ) + logger.error(error_msg) + raise RuntimeError(error_msg) + + # Repeat the last nav subtask action if the robot is navigating and hasn't reached the waypoint goal + if ( + current_eef_subtask_step_indices["body"] == len(current_eef_subtask_trajectories["body"]) - 1 + and not processed_nav_subtask + ): + if is_navigating and not navigation_goal_reached: + for name in self.env_cfg.subtask_configs.keys(): + current_eef_subtask_step_indices[name] -= 1 + processed_nav_subtask = True + + # Else skip to the end of the nav subtask if the robot has reached the waypoint goal before the end + # of the human recorded trajectory + elif was_navigating and not is_navigating and not processed_nav_subtask: + number_of_steps_to_skip = len(current_eef_subtask_trajectories["body"]) - ( + current_eef_subtask_step_indices["body"] + 1 + ) + for name in self.env_cfg.subtask_configs.keys(): + if current_eef_subtask_step_indices[name] + number_of_steps_to_skip < len( + current_eef_subtask_trajectories[name] + ): + current_eef_subtask_step_indices[name] = ( + current_eef_subtask_step_indices[name] + number_of_steps_to_skip + ) + else: + current_eef_subtask_step_indices[name] = len(current_eef_subtask_trajectories[name]) - 1 + processed_nav_subtask = True + + 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 + + if self.env_cfg.datagen_config.use_navigation_controller: + was_navigating = copy.deepcopy(is_navigating) + + # 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) - generated_src_demo_labels = torch.cat(generated_src_demo_labels, dim=0) - # set success to the recorded episode data and export to file + # 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) ) @@ -441,7 +999,35 @@ async def generate( observations=generated_obs, actions=generated_actions, success=generated_success, - src_demo_inds=generated_src_demo_inds, - src_demo_labels=generated_src_demo_labels, ) return results + + def _convert_planned_trajectory_to_waypoints( + self, motion_planner: Any, gripper_action: torch.Tensor + ) -> list[Waypoint]: + """ + (skillgen) Convert a motion planner's output trajectory into a list of Waypoint objects. + + The motion planner provides a sequence of planned 4x4 poses. This method wraps each + pose into a `Waypoint`, pairing it with the provided `gripper_action` and an optional + per-timestep noise value sourced from the planner config (`motion_noise_scale`). + + Args: + motion_planner: Planner instance exposing `get_planned_poses()` and an optional + `config.motion_noise_scale` float. + gripper_action: Gripper actuation to associate with each planned pose. + + Returns: + list[Waypoint]: Sequence of waypoints corresponding to the planned trajectory. + """ + # Get motion noise scale from the planner's configuration + motion_noise_scale = getattr(motion_planner.config, "motion_noise_scale", 0.0) + + waypoints = [] + planned_poses = motion_planner.get_planned_poses() + + for planned_pose in planned_poses: + waypoint = Waypoint(pose=planned_pose, gripper_action=gripper_action, noise=motion_noise_scale) + waypoints.append(waypoint) + + return waypoints diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info.py index 7af41f331e46..7e94f3e93838 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info.py @@ -1,26 +1,28 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 -""" -Defines structure of information that is needed from an environment for data generation. -""" -import torch +"""Defines the structure of information required from an environment for data generation processes.""" + 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. + """Defines the structure of information required from an environment for data generation processes. + + The :class:`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_start_signals**: Captures subtask start signals. Used by skillgen to identify + the precise start of a subtask from a demonstration. - **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. @@ -31,14 +33,19 @@ def __init__( eef_pose=None, object_poses=None, subtask_term_signals=None, + subtask_start_signals=None, target_eef_pose=None, gripper_action=None, ): - """ + """Initialize the DatagenInfo object. + 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_start_signals (dict or None): dictionary mapping subtask name to a binary + indicator (0 or 1) on whether subtask has started. This is required when using skillgen. + Each value in the dictionary could be an int, float, or torch.Tensor of shape [..., 1]. 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]. @@ -46,40 +53,6 @@ def __init__( 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 """ - # Type checks using assert - if eef_pose is not None: - assert isinstance( - eef_pose, torch.Tensor - ), f"Expected 'eef_pose' to be of type torch.Tensor, but got {type(eef_pose)}" - - if object_poses is not None: - assert isinstance( - object_poses, dict - ), f"Expected 'object_poses' to be a dictionary, but got {type(object_poses)}" - for k, v in object_poses.items(): - assert isinstance( - v, torch.Tensor - ), f"Expected 'object_poses[{k}]' to be of type torch.Tensor, but got {type(v)}" - - if subtask_term_signals is not None: - assert isinstance( - subtask_term_signals, dict - ), f"Expected 'subtask_term_signals' to be a dictionary, but got {type(subtask_term_signals)}" - for k, v in subtask_term_signals.items(): - assert isinstance( - v, (torch.Tensor, int, float) - ), f"Expected 'subtask_term_signals[{k}]' to be of type torch.Tensor, int, or float, but got {type(v)}" - - if target_eef_pose is not None: - assert isinstance( - target_eef_pose, torch.Tensor - ), f"Expected 'target_eef_pose' to be of type torch.Tensor, but got {type(target_eef_pose)}" - - if gripper_action is not None: - assert isinstance( - gripper_action, torch.Tensor - ), f"Expected 'gripper_action' to be of type torch.Tensor, but got {type(gripper_action)}" - self.eef_pose = None if eef_pose is not None: self.eef_pose = eef_pose @@ -88,6 +61,17 @@ def __init__( if object_poses is not None: self.object_poses = {k: object_poses[k] for k in object_poses} + # When using skillgen, demonstrations must be annotated with subtask start signals. + self.subtask_start_signals = None + if subtask_start_signals is not None: + self.subtask_start_signals = dict() + for k in subtask_start_signals: + if isinstance(subtask_start_signals[k], (float, int)): + self.subtask_start_signals[k] = subtask_start_signals[k] + else: + # Only create torch tensor if value is not a single value + self.subtask_start_signals[k] = subtask_start_signals[k] + self.subtask_term_signals = None if subtask_term_signals is not None: self.subtask_term_signals = dict() @@ -106,15 +90,19 @@ def __init__( 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. + def to_dict(self) -> dict: + """Convert this instance to a dictionary containing the same information. + + Returns: + A dictionary containing the same information as this instance. """ 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_start_signals is not None: + ret["subtask_start_signals"] = deepcopy(self.subtask_start_signals) 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: diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info_pool.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info_pool.py index 69e86c744813..5901944929d6 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info_pool.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/datagen_info_pool.py @@ -1,11 +1,10 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, 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 asyncio -import isaaclab.utils.math as PoseUtils from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler from isaaclab_mimic.datagen.datagen_info import DatagenInfo @@ -28,7 +27,9 @@ def __init__(self, env, env_cfg, device, asyncio_lock: asyncio.Lock | None = Non asyncio_lock (asyncio.Lock or None): asyncio lock to use for thread safety """ self._datagen_infos = [] - self._subtask_indices = [] + + # 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 @@ -36,14 +37,21 @@ def __init__(self, env, env_cfg, device, asyncio_lock: asyncio.Lock | None = Non self._asyncio_lock = asyncio_lock - if len(env_cfg.subtask_configs) != 1: - raise ValueError("Data generation currently supports only one end-effector.") - - (subtask_configs,) = env_cfg.subtask_configs.values() - self.subtask_term_signals = [subtask_config.subtask_term_signal for subtask_config in subtask_configs] - self.subtask_term_offset_ranges = [ - subtask_config.subtask_term_offset_range for subtask_config in subtask_configs - ] + # 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]]] = {} + self.subtask_start_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_start_offset_ranges[eef_name] = [ + subtask_config.subtask_start_offset_range 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): @@ -51,9 +59,9 @@ def datagen_infos(self): return self._datagen_infos @property - def subtask_indices(self): - """Returns the subtask indices.""" - return self._subtask_indices + def subtask_boundaries(self) -> dict[str, list[list[tuple[int, int]]]]: + """Returns the subtask boundaries.""" + return self._subtask_boundaries @property def asyncio_lock(self): @@ -83,101 +91,117 @@ def _add_episode(self, episode: EpisodeData): Add a datagen info from the given episode. Args: - episode (EpisodeData): episode to add + episode: Episode to add. + + Raises: + ValueError: Episode lacks 'datagen_info' annotations in observations. + ValueError: Subtask termination signal is not increasing. """ ep_grp = episode.data - eef_name = list(self.env.cfg.subtask_configs.keys())[0] - # extract datagen info + # Extract datagen info if "datagen_info" in ep_grp["obs"]: - eef_pose = ep_grp["obs"]["datagen_info"]["eef_pose"][eef_name] + 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"][eef_name] + target_eef_pose = ep_grp["obs"]["datagen_info"]["target_eef_pose"] subtask_term_signals_dict = ep_grp["obs"]["datagen_info"]["subtask_term_signals"] + # subtask_start_signals is optional + subtask_start_signals_dict = ep_grp["obs"]["datagen_info"].get("subtask_start_signals") else: - # Extract eef poses - eef_pos = ep_grp["obs"]["eef_pos"] - eef_quat = ep_grp["obs"]["eef_quat"] # format (w, x, y, z) - eef_rot_matrices = PoseUtils.matrix_from_quat(eef_quat) # shape (N, 3, 3) - # Create pose matrices for all environments - eef_pose = PoseUtils.make_pose(eef_pos, eef_rot_matrices) # shape (N, 4, 4) - - # Object poses - object_poses_dict = dict() - for object_name, value in ep_grp["obs"]["object_pose"].items(): - # object_pose - value = value["root_pose"] - # Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_steps, 13). - # Quaternion ordering is wxyz - - # Convert to rotation matrices - object_rot_matrices = PoseUtils.matrix_from_quat(value[:, 3:7]) # shape (N, 3, 3) - object_rot_positions = value[:, 0:3] # shape (N, 3) - object_poses_dict[object_name] = PoseUtils.make_pose(object_rot_positions, object_rot_matrices) - - # Target eef pose - target_eef_pose = ep_grp["obs"]["target_eef_pose"] - - # Subtask termination signalsS - subtask_term_signals_dict = (ep_grp["obs"]["subtask_term_signals"],) + 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"])[eef_name] + 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_start_signals=subtask_start_signals_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 indices using subtask termination signals - ep_subtask_indices = [] - prev_subtask_term_ind = 0 - for subtask_ind in range(len(self.subtask_term_signals)): - subtask_term_signal = self.subtask_term_signals[subtask_ind] - if subtask_term_signal is None: - # final subtask, finishes at end of demo - subtask_term_ind = ep_grp["actions"].shape[0] + # 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_index = 0 + eef_subtask_boundaries = [] + for eef_subtask_index, eef_subtask_signal_name in enumerate(self.subtask_term_signal_names[eef_name]): + if self.env_cfg.datagen_config.use_skillgen: + # For skillgen, the start of a subtask is explicitly defined in the demonstration data. + if ep_datagen_info_obj.subtask_start_signals is None: + raise ValueError( + "subtask_start_signals field is not present in datagen_info for subtask" + f" {eef_subtask_signal_name} in the loaded episode when use_skillgen is enabled" + ) + # Find the first time step where the start signal transitions from 0 to 1. + subtask_start_indicators = ( + ep_datagen_info_obj.subtask_start_signals[eef_subtask_signal_name].flatten().int() + ) + # Compute the difference between consecutive elements to find the transition point. + diffs = subtask_start_indicators[1:] - subtask_start_indicators[:-1] + # The first non-zero element's index gives the start of the subtask. + start_index = int(diffs.nonzero()[0][0]) + 1 + else: + # Without skillgen, subtasks are assumed to be sequential. + start_index = prev_subtask_term_index + + if eef_subtask_index == len(self.subtask_term_signal_names[eef_name]) - 1: + # Last subtask has no termination signal from the datagen_info + end_index = 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_term_indicators = ( + ep_datagen_info_obj.subtask_term_signals[eef_subtask_signal_name].flatten().int() + ) + diffs = subtask_term_indicators[1:] - subtask_term_indicators[:-1] + end_index = int(diffs.nonzero()[0][0]) + 1 + end_index = end_index + 1 # increment to support indexing like demo[start:end] + + if end_index <= start_index: + raise ValueError( + f"subtask termination signal is not increasing: {end_index} should be greater than" + f" {start_index}" + ) + eef_subtask_boundaries.append((start_index, end_index)) + prev_subtask_term_index = end_index + + if self.env_cfg.datagen_config.use_skillgen: + # With skillgen, both start and end boundaries can be randomized. + # This checks if the randomized boundaries could result in an invalid (e.g., empty) subtask. + for i in range(len(eef_subtask_boundaries)): + # Ensure that a subtask is not empty in the worst-case randomization scenario. + assert ( + eef_subtask_boundaries[i][0] + self.subtask_start_offset_ranges[eef_name][i][1] + < eef_subtask_boundaries[i][1] + self.subtask_term_offset_ranges[eef_name][i][0] + ), f"subtask {i} is empty in the worst case" + if i == len(eef_subtask_boundaries) - 1: + break + # Make sure that subtasks are not overlapped with the largest offsets + assert ( + eef_subtask_boundaries[i][1] + self.subtask_term_offset_ranges[eef_name][i][1] + < eef_subtask_boundaries[i + 1][0] + self.subtask_start_offset_ranges[eef_name][i + 1][0] + ), f"subtasks {i} and {i + 1} are overlapped with the largest offsets" 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].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] - ep_subtask_indices.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 - # - assert len(ep_subtask_indices) == len( - self.subtask_term_signals - ), "mismatch in length of extracted subtask info and number of subtasks" - for i in range(1, len(ep_subtask_indices)): - prev_max_offset_range = self.subtask_term_offset_ranges[i - 1][1] - assert ( - ep_subtask_indices[i - 1][1] + prev_max_offset_range - < ep_subtask_indices[i][1] + self.subtask_term_offset_ranges[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, - ep_subtask_indices[i - 1][1], - i - 1, - prev_max_offset_range, - i, - ep_subtask_indices[i][1], - i, - self.subtask_term_offset_ranges[i][0], - ) - ) - - self._subtask_indices.append(ep_subtask_indices) + # Run sanity check on subtask_term_offset_range in task spec + for i in range(1, len(eef_subtask_boundaries)): + prev_max_offset_range = self.subtask_term_offset_ranges[eef_name][i - 1][1] + # Make sure that subtasks are not overlapped with the largest offsets + 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] + ), ( + f"subtask sanity check violation in demo with subtask {i - 1} end ind" + f" {eef_subtask_boundaries[i - 1][1]}, subtask {i - 1} max offset {prev_max_offset_range}," + f" subtask {i} end ind {eef_subtask_boundaries[i][1]}, and subtask {i} min offset" + f" {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): """ diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py index c8536f885cbc..18d1f6716d4a 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py @@ -1,16 +1,20 @@ -# Copyright (c) 2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, 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 asyncio import contextlib -import torch +import sys +import traceback from typing import Any -from isaaclab.envs import ManagerBasedEnv +import torch + +from isaaclab.envs import ManagerBasedRLMimicEnv from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg -from isaaclab.managers import DatasetExportMode +from isaaclab.managers import DatasetExportMode, TerminationTermCfg +from isaaclab.managers.recorder_manager import RecorderManagerBaseCfg from isaaclab_mimic.datagen.data_generator import DataGenerator from isaaclab_mimic.datagen.datagen_info_pool import DataGenInfoPool @@ -24,25 +28,43 @@ async def run_data_generator( - env: ManagerBasedEnv, + env: ManagerBasedRLMimicEnv, env_id: int, + env_reset_queue: asyncio.Queue, env_action_queue: asyncio.Queue, data_generator: DataGenerator, - success_term: Any, + success_term: TerminationTermCfg, pause_subtask: bool = False, + motion_planner: Any = None, ): - """Run data generator.""" + """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. + motion_planner: The motion planner to use. + """ global num_success, num_failures, num_attempts while True: - results = await data_generator.generate( - env_id=env_id, - success_term=success_term, - env_action_queue=env_action_queue, - select_src_per_subtask=env.unwrapped.cfg.datagen_config.generation_select_src_per_subtask, - transform_first_robot_pose=env.unwrapped.cfg.datagen_config.generation_transform_first_robot_pose, - interpolate_from_last_target_pose=env.unwrapped.cfg.datagen_config.generation_interpolate_from_last_target_pose, - pause_subtask=pause_subtask, - ) + try: + 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, + motion_planner=motion_planner, + ) + except Exception as e: + sys.stderr.write(traceback.format_exc()) + sys.stderr.flush() + raise e + if bool(results["success"]): num_success += 1 else: @@ -51,22 +73,39 @@ async def run_data_generator( def env_loop( - env: ManagerBasedEnv, + env: ManagerBasedRLMimicEnv, + env_reset_queue: asyncio.Queue, env_action_queue: asyncio.Queue, shared_datagen_info_pool: DataGenInfoPool, asyncio_event_loop: asyncio.AbstractEventLoop, -) -> None: - """Main loop for the environment.""" +): + """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.unwrapped.action_space.shape) + actions = torch.zeros(env.action_space.shape) # get actions from all the data generators - for i in range(env.unwrapped.num_envs): + 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 @@ -75,27 +114,30 @@ def env_loop( env.step(actions) # mark done so the data generators can continue with the step results - for i in range(env.unwrapped.num_envs): + 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) - print(f"have {num_success} successes out of {num_attempts} trials so far") - print(f"have {num_failures} failures out of {num_attempts} trials so far") - print("*" * 50) + 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.unwrapped.cfg.datagen_config.generation_guarantee - generation_num_trials = env.unwrapped.cfg.datagen_config.generation_num_trials + 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.unwrapped.sim.is_stopped(): + if env.sim.is_stopped(): break env.close() @@ -108,6 +150,7 @@ def setup_env_config( num_envs: int, device: str, generation_num_trials: int | None = None, + recorder_cfg: RecorderManagerBaseCfg | None = None, ) -> tuple[Any, Any]: """Configure the environment for data generation. @@ -147,7 +190,10 @@ def setup_env_config( env_cfg.observations.policy.concatenate_terms = False # Setup recorders - env_cfg.recorders = ActionStateRecorderManagerCfg() + if recorder_cfg is None: + env_cfg.recorders = ActionStateRecorderManagerCfg() + else: + env_cfg.recorders = recorder_cfg env_cfg.recorders.dataset_export_dir_path = output_dir env_cfg.recorders.dataset_filename = output_file_name @@ -160,7 +206,12 @@ def setup_env_config( def setup_async_generation( - env: Any, num_envs: int, input_file: str, success_term: Any, pause_subtask: bool = False + env: Any, + num_envs: int, + input_file: str, + success_term: Any, + pause_subtask: bool = False, + motion_planners: Any = None, ) -> dict[str, Any]: """Setup async data generation tasks. @@ -170,31 +221,42 @@ def setup_async_generation( input_file: Path to input dataset file success_term: Success termination condition pause_subtask: Whether to pause after subtasks + motion_planners: Motion planner instances for all environments 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.unwrapped, env.unwrapped.cfg, env.unwrapped.device, asyncio_lock=shared_datagen_info_pool_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.unwrapped, src_demo_datagen_info_pool=shared_datagen_info_pool) + data_generator = DataGenerator(env=env, src_demo_datagen_info_pool=shared_datagen_info_pool) data_generator_asyncio_tasks = [] for i in range(num_envs): + env_motion_planner = motion_planners[i] if motion_planners else None task = asyncio_event_loop.create_task( - run_data_generator(env, i, env_action_queue, data_generator, success_term, pause_subtask=pause_subtask) + run_data_generator( + env, + i, + env_reset_queue, + env_action_queue, + data_generator, + success_term, + pause_subtask=pause_subtask, + motion_planner=env_motion_planner, + ) ) 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/source/isaaclab_mimic/isaaclab_mimic/datagen/selection_strategy.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/selection_strategy.py index dcc6f1bbee0c..7c8683c64379 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/selection_strategy.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/selection_strategy.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -7,7 +7,9 @@ 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 diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/utils.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/utils.py index d62a09c10fd6..5430b75597cb 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/utils.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/utils.py @@ -1,7 +1,8 @@ -# Copyright (c) 2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, 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 __future__ import annotations import os from collections.abc import Callable @@ -63,18 +64,20 @@ def get_parameter_input( 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", - ), - ]) + 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]) @@ -96,18 +99,20 @@ def on_value_change(change): 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", - ), - ]) + 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"]) diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py index faab4af86e7e..964cc2a49dda 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -6,11 +6,16 @@ """ A collection of classes used to represent waypoints and trajectories. """ + import asyncio -import torch +import inspect from copy import deepcopy +import torch + import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv +from isaaclab.managers import TerminationTermCfg class Waypoint: @@ -18,7 +23,7 @@ class Waypoint: Represents a single desired 6-DoF waypoint, along with corresponding gripper actuation for this point. """ - def __init__(self, eef_names, pose, gripper_action, noise=None): + def __init__(self, pose, gripper_action, noise=None): """ Args: pose (torch.Tensor): 4x4 pose target for robot controller @@ -26,7 +31,6 @@ def __init__(self, eef_names, pose, gripper_action, noise=None): noise (float or None): action noise amplitude to apply during execution at this timestep (for arm actions, not gripper actions) """ - self.eef_names = eef_names self.pose = pose self.gripper_action = gripper_action self.noise = noise @@ -54,7 +58,7 @@ def __init__(self, sequence=None): self.sequence = deepcopy(sequence) @classmethod - def from_poses(cls, eef_names, poses, gripper_actions, action_noise): + def from_poses(cls, poses, gripper_actions, action_noise): """ Instantiate a WaypointSequence object given a sequence of poses, gripper actions, and action noise. @@ -79,7 +83,6 @@ def from_poses(cls, eef_names, poses, gripper_actions, action_noise): # make WaypointSequence instance sequence = [ Waypoint( - eef_names=eef_names, pose=poses[t], gripper_action=gripper_actions[t], noise=action_noise[t, 0], @@ -202,7 +205,6 @@ def add_waypoint_sequence(self, sequence): def add_waypoint_sequence_for_target_pose( self, - eef_names, pose, gripper_action, num_steps, @@ -254,7 +256,6 @@ def add_waypoint_sequence_for_target_pose( # add waypoint sequence for this set of poses sequence = WaypointSequence.from_poses( - eef_names=eef_names, poses=poses, gripper_actions=gripper_actions, action_noise=action_noise, @@ -281,7 +282,6 @@ def pop_first(self): def merge( self, other, - eef_names, num_steps_interp=None, num_steps_fixed=None, action_noise=0.0, @@ -315,7 +315,6 @@ def merge( if need_interp: # interpolation segment self.add_waypoint_sequence_for_target_pose( - eef_names=eef_names, pose=target_for_interpolation.pose, gripper_action=target_for_interpolation.gripper_action, num_steps=num_steps_interp, @@ -326,10 +325,10 @@ def merge( 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 + # 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( - eef_names=eef_names, pose=target_for_interpolation.pose, gripper_action=target_for_interpolation.gripper_action, num_steps=num_steps_fixed_to_use, @@ -343,88 +342,88 @@ def merge( # 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, - env_id, - success_term, + env: ManagerBasedRLMimicEnv, + success_term: TerminationTermCfg, + env_id: int = 0, env_action_queue: asyncio.Queue | None = None, ): """ - Main function to execute the trajectory. Will use env_interface.target_eef_pose_to_action to - convert each target pose at each waypoint to an action command, and pass that along to - env.step. + Executes the multi-waypoint eef actions in the environment. Args: - env (Isaac Lab ManagerBasedEnv instance): environment to use for executing trajectory - env_id (int): environment index - success_term: success term to check if the task is successful - env_action_queue (asyncio.Queue): queue for sending actions to the environment + 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: - results (dict): dictionary with the following items 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 (list): action executed at each timestep - success (bool): whether the trajectory successfully solved the task or not - """ - - states = [] - actions = [] - observations = [] - success = False - - # iterate over waypoint sequences - for seq in self.waypoint_sequences: - - # iterate over waypoints in each sequence - for j in range(len(seq)): - - # current waypoint - waypoint = seq[j] - - # current state and observation - obs = env.obs_buf - state = env.scene.get_state(is_relative=True) - - # convert target pose and gripper action to env action - target_eef_pose_dict = {waypoint.eef_names[0]: waypoint.pose} - gripper_action_dict = {waypoint.eef_names[0]: waypoint.gripper_action} - play_action = env.target_eef_pose_to_action( - target_eef_pose_dict=target_eef_pose_dict, - gripper_action_dict=gripper_action_dict, - noise=waypoint.noise, - env_id=env_id, - ) + 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]) - # step environment - if not isinstance(play_action, torch.Tensor): - play_action = torch.tensor(play_action) - if play_action.dim() == 1 and play_action.size(0) == 7: - play_action = play_action.unsqueeze(0) # Reshape to [1, 7] - - 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 - - # collect data - states.append(state) - actions.append(play_action) - observations.append(obs) - - cur_success_metric = bool(success_term.func(env, **success_term.params)[env_id]) - - # If the task success metric is True once during the execution, then the task is considered successful - success = success or cur_success_metric - - results = dict( - states=states, - observations=observations, - actions=torch.stack(actions), + result = dict( + states=[state], + observations=[obs], + actions=[play_action], success=success, ) - return results + return result diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py index b90727d1e97c..f3a2705a68b6 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -7,28 +7,155 @@ import gymnasium as gym -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 - ## # Inverse Kinematics - Relative Pose Control ## gym.register( id="Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0", - entry_point="isaaclab_mimic.envs:FrankaCubeStackIKRelMimicEnv", + entry_point=f"{__name__}.franka_stack_ik_rel_mimic_env:FrankaCubeStackIKRelMimicEnv", kwargs={ - "env_cfg_entry_point": franka_stack_ik_rel_mimic_env_cfg.FrankaCubeStackIKRelMimicEnvCfg, + "env_cfg_entry_point": f"{__name__}.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", + entry_point=f"{__name__}.franka_stack_ik_rel_mimic_env:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.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=f"{__name__}.franka_stack_ik_abs_mimic_env:FrankaCubeStackIKAbsMimicEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.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=f"{__name__}.franka_stack_ik_rel_mimic_env:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.franka_stack_ik_rel_visuomotor_mimic_env_cfg:FrankaCubeStackIKRelVisuomotorMimicEnvCfg" + ), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-Mimic-v0", + entry_point=f"{__name__}.franka_stack_ik_rel_mimic_env:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.franka_stack_ik_rel_visuomotor_cosmos_mimic_env_cfg:FrankaCubeStackIKRelVisuomotorCosmosMimicEnvCfg", + }, + disable_env_checker=True, +) + + +## +# SkillGen +## + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0", + entry_point=f"{__name__}.franka_stack_ik_rel_mimic_env:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.franka_stack_ik_rel_skillgen_env_cfg:FrankaCubeStackIKRelSkillgenEnvCfg", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Bin-Franka-IK-Rel-Mimic-v0", + entry_point=f"{__name__}.franka_stack_ik_rel_mimic_env:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.franka_bin_stack_ik_rel_mimic_env_cfg:FrankaBinStackIKRelMimicEnvCfg", + }, + disable_env_checker=True, +) + +## +# Galbot Stack Cube with RmpFlow - Relative Pose Control +## + +gym.register( + id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-Rel-Mimic-v0", + entry_point=f"{__name__}.galbot_stack_rmp_rel_mimic_env:RmpFlowGalbotCubeStackRelMimicEnv", + kwargs={ + "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=f"{__name__}.galbot_stack_rmp_rel_mimic_env:RmpFlowGalbotCubeStackRelMimicEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.galbot_stack_rmp_rel_mimic_env_cfg:RmpFlowGalbotRightArmSuctionCubeStackRelMimicEnvCfg" + ), + }, + disable_env_checker=True, +) + +## +# Galbot Stack Cube with RmpFlow - Absolute Pose Control +## + +gym.register( + id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-Abs-Mimic-v0", + 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:RmpFlowGalbotLeftArmGripperCubeStackAbsMimicEnvCfg" + ), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow-Abs-Mimic-v0", + 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": franka_stack_ik_rel_blueprint_mimic_env_cfg.FrankaCubeStackIKRelBlueprintMimicEnvCfg, + "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 000000000000..76557802f463 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_toy2box_mimic_env_cfg.py @@ -0,0 +1,84 @@ +# Copyright (c) 2024-2026, 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 000000000000..2bfadef874c3 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_upright_mug_mimic_env_cfg.py @@ -0,0 +1,81 @@ +# Copyright (c) 2024-2026, 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/franka_bin_stack_ik_rel_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_bin_stack_ik_rel_mimic_env_cfg.py new file mode 100644 index 000000000000..ca28719d7306 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_bin_stack_ik_rel_mimic_env_cfg.py @@ -0,0 +1,92 @@ +# Copyright (c) 2024-2026, 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.stack.config.franka.bin_stack_ik_rel_env_cfg import FrankaBinStackEnvCfg + + +@configclass +class FrankaBinStackIKRelMimicEnvCfg(FrankaBinStackEnvCfg, 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 = "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( + object_ref="cube_2", + subtask_term_signal="grasp_1", + subtask_term_offset_range=(0, 0), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.03, + num_interpolation_steps=0, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + description="Grasp red cube", + next_subtask_description="Stack red cube on top of blue cube", + ) + ) + subtask_configs.append( + SubTaskConfig( + object_ref="cube_1", + subtask_term_signal="stack_1", + subtask_term_offset_range=(0, 0), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.03, + num_interpolation_steps=0, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + next_subtask_description="Grasp green cube", + ) + ) + subtask_configs.append( + SubTaskConfig( + object_ref="cube_3", + subtask_term_signal="grasp_2", + subtask_term_offset_range=(0, 0), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.03, + num_interpolation_steps=0, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + next_subtask_description="Stack green cube on top of red cube", + ) + ) + subtask_configs.append( + SubTaskConfig( + object_ref="cube_2", + subtask_term_signal="stack_2", + subtask_term_offset_range=(0, 0), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.03, + num_interpolation_steps=0, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["franka"] = subtask_configs diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env.py new file mode 100644 index 000000000000..45efc58bfc20 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env.py @@ -0,0 +1,99 @@ +# Copyright (c) 2024-2026, 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 collections.abc import Sequence + +import torch + +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/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env_cfg.py new file mode 100644 index 000000000000..93e51d8f673e --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env_cfg.py @@ -0,0 +1,87 @@ +# Copyright (c) 2024-2026, 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.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/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_blueprint_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_blueprint_mimic_env_cfg.py index 3038adee6e92..cd75bea018a1 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_blueprint_mimic_env_cfg.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_blueprint_mimic_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env.py index beec40499f61..336db05ca174 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env.py @@ -1,11 +1,12 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, 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 torch + import isaaclab.utils.math as PoseUtils from isaaclab.envs import ManagerBasedRLMimicEnv @@ -36,7 +37,11 @@ def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None 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 + 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 @@ -75,8 +80,8 @@ def target_eef_pose_to_action( # add noise to action pose_action = torch.cat([delta_position, delta_rotation], dim=0) - if noise is not None: - noise = noise * torch.randn_like(pose_action) + if action_noise_dict is not None: + noise = action_noise_dict[eef_name] * torch.randn_like(pose_action) pose_action += noise pose_action = torch.clamp(pose_action, -1.0, 1.0) @@ -159,3 +164,26 @@ def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict 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 + + def get_expected_attached_object(self, eef_name: str, subtask_index: int, env_cfg) -> str | None: + """ + (SkillGen) Return the expected attached object for the given EEF/subtask. + + Assumes 'stack' subtasks place the object grasped in the preceding 'grasp' subtask. + Returns None for 'grasp' (or others) at subtask start. + """ + if eef_name not in env_cfg.subtask_configs: + return None + + subtask_configs = env_cfg.subtask_configs[eef_name] + if not (0 <= subtask_index < len(subtask_configs)): + return None + + current_cfg = subtask_configs[subtask_index] + # If stacking, expect we are holding the object grasped in the prior subtask + if "stack" in str(current_cfg.subtask_term_signal).lower(): + if subtask_index > 0: + prev_cfg = subtask_configs[subtask_index - 1] + if "grasp" in str(prev_cfg.subtask_term_signal).lower(): + return prev_cfg.object_ref + return None diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env_cfg.py index 0a724106023b..197852602d39 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env_cfg.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -30,6 +30,7 @@ def __post_init__(self): 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 @@ -57,6 +58,8 @@ def __post_init__(self): 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( @@ -79,6 +82,7 @@ def __post_init__(self): 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( @@ -101,6 +105,7 @@ def __post_init__(self): 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( diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_skillgen_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_skillgen_env_cfg.py new file mode 100644 index 000000000000..9d26039126be --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_skillgen_env_cfg.py @@ -0,0 +1,137 @@ +# Copyright (c) 2024-2026, 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.stack.config.franka.stack_ik_rel_env_cfg_skillgen import ( + FrankaCubeStackSkillgenEnvCfg, +) + + +@configclass +class FrankaCubeStackIKRelSkillgenEnvCfg(FrankaCubeStackSkillgenEnvCfg, 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=(0, 0), # (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=0, # 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=(0, 0), # (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=0, # 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=(0, 0), # (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=0, # 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 for MimicGen + # Needs to be detected for SkillGen + # Setting this doesn't affect the data generation for MimicGen + subtask_term_signal="stack_2", + # 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=0, # 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/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_cosmos_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_cosmos_mimic_env_cfg.py new file mode 100644 index 000000000000..2f2ebe5ab339 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_cosmos_mimic_env_cfg.py @@ -0,0 +1,128 @@ +# Copyright (c) 2025-2026, 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.stack.config.franka.stack_ik_rel_visuomotor_cosmos_env_cfg import ( + FrankaCubeStackVisuomotorCosmosEnvCfg, +) + + +@configclass +class FrankaCubeStackIKRelVisuomotorCosmosMimicEnvCfg(FrankaCubeStackVisuomotorCosmosEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Franka Cube Stack IK Rel Visuomotor Cosmos 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_cosmos_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/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_mimic_env_cfg.py new file mode 100644 index 000000000000..ed63e973b2d0 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_mimic_env_cfg.py @@ -0,0 +1,128 @@ +# Copyright (c) 2024-2026, 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.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/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_abs_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_abs_mimic_env.py new file mode 100644 index 000000000000..b3d03a149317 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_abs_mimic_env.py @@ -0,0 +1,47 @@ +# Copyright (c) 2024-2026, 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 collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils + +from .franka_stack_ik_abs_mimic_env import FrankaCubeStackIKAbsMimicEnv + + +class RmpFlowGalbotCubeStackAbsMimicEnv(FrankaCubeStackIKAbsMimicEnv): + """ + Isaac Lab Mimic environment wrapper class for Galbot Cube Stack RmpFlow Absolute env. + """ + + def get_object_poses(self, env_ids: Sequence[int] | None = None): + """ + Rewrite this function to get the pose of each object in robot base frame, + 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 in base frame of robot (4x4 torch.Tensor) + """ + + if env_ids is None: + env_ids = slice(None) + + rigid_object_states = self.scene.get_state(is_relative=True)["rigid_object"] + robot_states = self.scene.get_state(is_relative=True)["articulation"]["robot"] + root_pose = robot_states["root_pose"] + root_pos = root_pose[env_ids, :3] + root_quat = root_pose[env_ids, 3:7] + + object_pose_matrix = dict() + 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) + + return object_pose_matrix diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_abs_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_abs_mimic_env_cfg.py new file mode 100644 index 000000000000..d3de8a9aa3d2 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_abs_mimic_env_cfg.py @@ -0,0 +1,255 @@ +# Copyright (c) 2024-2026, 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.stack.config.galbot.stack_rmp_rel_env_cfg import ( + RmpFlowGalbotLeftArmCubeStackEnvCfg, + RmpFlowGalbotRightArmCubeStackEnvCfg, +) + + +@configclass +class RmpFlowGalbotLeftArmGripperCubeStackAbsMimicEnvCfg(RmpFlowGalbotLeftArmCubeStackEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Galbot Gripper Cube Stack IK Rel 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( + # 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=( + 18, + 25, + ), + # 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.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="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=( + 18, + 25, + ), + # 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="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=( + 25, + 30, + ), # This should be larger than the other subtasks, because the gripper + # should be lifted higher than two blocks + # 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="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.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["galbot"] = subtask_configs + + +@configclass +class RmpFlowGalbotRightArmSuctionCubeStackAbsMimicEnvCfg(RmpFlowGalbotRightArmCubeStackEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Galbot Suction Gripper Cube Stack RmpFlow 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( + # 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=(5, 10), + # 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.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="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=(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="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=(5, 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="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.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["galbot"] = subtask_configs diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_rel_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_rel_mimic_env.py new file mode 100644 index 000000000000..c839cd655b13 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_rel_mimic_env.py @@ -0,0 +1,48 @@ +# Copyright (c) 2024-2026, 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 collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils + +from .franka_stack_ik_rel_mimic_env import FrankaCubeStackIKRelMimicEnv + + +class RmpFlowGalbotCubeStackRelMimicEnv(FrankaCubeStackIKRelMimicEnv): + """ + Isaac Lab Mimic environment wrapper class for Galbot Cube Stack RmpFlow Rel env. + """ + + def get_object_poses(self, env_ids: Sequence[int] | None = None): + """ + Rewrite this function to get the pose of each object in robot base frame, + 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 in base frame of robot (4x4 torch.Tensor) + """ + + if env_ids is None: + env_ids = slice(None) + + rigid_object_states = self.scene.get_state(is_relative=True)["rigid_object"] + robot_states = self.scene.get_state(is_relative=True)["articulation"]["robot"] + root_pose = robot_states["root_pose"] + root_pos = root_pose[env_ids, :3] + root_quat = root_pose[env_ids, 3:7] + + object_pose_matrix = dict() + 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) + + return object_pose_matrix diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_rel_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_rel_mimic_env_cfg.py new file mode 100644 index 000000000000..ce4d00015a3e --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/galbot_stack_rmp_rel_mimic_env_cfg.py @@ -0,0 +1,253 @@ +# Copyright (c) 2024-2026, 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.stack.config.galbot.stack_rmp_rel_env_cfg import ( + RmpFlowGalbotLeftArmCubeStackEnvCfg, + RmpFlowGalbotRightArmCubeStackEnvCfg, +) + + +@configclass +class RmpFlowGalbotLeftArmGripperCubeStackRelMimicEnvCfg(RmpFlowGalbotLeftArmCubeStackEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Galbot Gripper Cube Stack IK Rel 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( + # 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=( + 18, + 25, + ), + # 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.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="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=( + 18, + 25, + ), + # 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="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 + # This should be larger than the other subtasks, because the gripper + # should be lifted higher than two blocks + subtask_term_offset_range=(25, 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=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="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.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["galbot"] = subtask_configs + + +@configclass +class RmpFlowGalbotRightArmSuctionCubeStackRelMimicEnvCfg(RmpFlowGalbotRightArmCubeStackEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Galbot Suction Gripper Cube Stack RmpFlow Rel 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( + # 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=(5, 10), + # 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.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="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=(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="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=(5, 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="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.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["galbot"] = 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 000000000000..1776528b7970 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pick_place_mimic_env.py @@ -0,0 +1,179 @@ +# Copyright (c) 2024-2026, 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 collections.abc import Sequence + +import torch + +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 new file mode 100644 index 000000000000..e7fe847c42e8 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py @@ -0,0 +1,49 @@ +# Copyright (c) 2024-2026, 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 Isaac Lab Mimic.""" + +import gymnasium as gym + +gym.register( + id="Isaac-PickPlace-GR1T2-Abs-Mimic-v0", + entry_point=f"{__name__}.pickplace_gr1t2_mimic_env:PickPlaceGR1T2MimicEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.pickplace_gr1t2_mimic_env_cfg:PickPlaceGR1T2MimicEnvCfg", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-PickPlace-GR1T2-WaistEnabled-Abs-Mimic-v0", + entry_point=f"{__name__}.pickplace_gr1t2_mimic_env:PickPlaceGR1T2MimicEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.pickplace_gr1t2_waist_enabled_mimic_env_cfg:PickPlaceGR1T2WaistEnabledMimicEnvCfg" + ), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic-v0", + entry_point=f"{__name__}.pickplace_gr1t2_mimic_env:PickPlaceGR1T2MimicEnv", + kwargs={"env_cfg_entry_point": f"{__name__}.nutpour_gr1t2_mimic_env_cfg:NutPourGR1T2MimicEnvCfg"}, + disable_env_checker=True, +) + +gym.register( + id="Isaac-ExhaustPipe-GR1T2-Pink-IK-Abs-Mimic-v0", + entry_point=f"{__name__}.pickplace_gr1t2_mimic_env:PickPlaceGR1T2MimicEnv", + kwargs={"env_cfg_entry_point": f"{__name__}.exhaustpipe_gr1t2_mimic_env_cfg:ExhaustPipeGR1T2MimicEnvCfg"}, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Locomanipulation-G1-Abs-Mimic-v0", + entry_point=f"{__name__}.locomanipulation_g1_mimic_env:LocomanipulationG1MimicEnv", + kwargs={"env_cfg_entry_point": f"{__name__}.locomanipulation_g1_mimic_env_cfg:LocomanipulationG1MimicEnvCfg"}, + disable_env_checker=True, +) diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/exhaustpipe_gr1t2_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/exhaustpipe_gr1t2_mimic_env_cfg.py new file mode 100644 index 000000000000..ed37975c6afe --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/exhaustpipe_gr1t2_mimic_env_cfg.py @@ -0,0 +1,113 @@ +# Copyright (c) 2025-2026, 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.pick_place.exhaustpipe_gr1t2_pink_ik_env_cfg import ( + ExhaustPipeGR1T2PinkIKEnvCfg, +) + + +@configclass +class ExhaustPipeGR1T2MimicEnvCfg(ExhaustPipeGR1T2PinkIKEnvCfg, MimicEnvCfg): + """Configuration for GR1T2 Exhaust Pipe Mimic environment.""" + + def __post_init__(self): + # Calling post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "gr1t2_exhaust_pipe_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 = 10 + + # 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="blue_exhaust_pipe", + # 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_1", + 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", + # 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=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="blue_exhaust_pipe", + # 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). + 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", + # 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=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["right"] = subtask_configs + + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="blue_exhaust_pipe", + # 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). + 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", + # 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=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["left"] = subtask_configs 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 000000000000..89b13167315c --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env.py @@ -0,0 +1,131 @@ +# Copyright (c) 2024-2026, 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 collections.abc import Sequence + +import torch + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class LocomanipulationG1MimicEnv(ManagerBasedRLMimicEnv): + """G1 Locomanipulation Mimic environment.""" + + 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 000000000000..2aa766dec33c --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env_cfg.py @@ -0,0 +1,113 @@ +# Copyright (c) 2024-2026, 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): + """Configuration for G1 Locomanipulation Mimic environment.""" + + 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/envs/pinocchio_envs/nutpour_gr1t2_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/nutpour_gr1t2_mimic_env_cfg.py new file mode 100644 index 000000000000..683d4be09e44 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/nutpour_gr1t2_mimic_env_cfg.py @@ -0,0 +1,157 @@ +# Copyright (c) 2025-2026, 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.pick_place.nutpour_gr1t2_pink_ik_env_cfg import NutPourGR1T2PinkIKEnvCfg + + +@configclass +class NutPourGR1T2MimicEnvCfg(NutPourGR1T2PinkIKEnvCfg, MimicEnvCfg): + """Configuration for GR1T2 Nut Pouring Mimic environment.""" + + def __post_init__(self): + # Calling post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "gr1t2_nut_pouring_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 = 10 + + # 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="sorting_bowl", + # 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", + # 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=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="sorting_bowl", + # 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_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", + # 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, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="sorting_scale", + # 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 = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="sorting_beaker", + # 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_left", + 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 generatio + 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=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="sorting_bowl", + # 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=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["left"] = subtask_configs diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py new file mode 100644 index 000000000000..9ec65040ef95 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py @@ -0,0 +1,131 @@ +# Copyright (c) 2024-2026, 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 collections.abc import Sequence + +import torch + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class PickPlaceGR1T2MimicEnv(ManagerBasedRLMimicEnv): + """GR1T2 Pick Place Mimic environment.""" + + 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, # Unused, but required to conform to interface + ) -> 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:25], "right": actions[:, 25:]} diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py new file mode 100644 index 000000000000..0297fb72a1bc --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py @@ -0,0 +1,110 @@ +# Copyright (c) 2024-2026, 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.pick_place.pickplace_gr1t2_env_cfg import PickPlaceGR1T2EnvCfg + + +@configclass +class PickPlaceGR1T2MimicEnvCfg(PickPlaceGR1T2EnvCfg, MimicEnvCfg): + """Configuration for GR1T2 Pick Place Mimic environment.""" + + def __post_init__(self): + # Calling post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "gr1t2_pick_place_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.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 = [] + 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/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py new file mode 100644 index 000000000000..f9528b277dba --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py @@ -0,0 +1,112 @@ +# Copyright (c) 2024-2026, 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.pick_place.pickplace_gr1t2_waist_enabled_env_cfg import ( + PickPlaceGR1T2WaistEnabledEnvCfg, +) + + +@configclass +class PickPlaceGR1T2WaistEnabledMimicEnvCfg(PickPlaceGR1T2WaistEnabledEnvCfg, MimicEnvCfg): + """Configuration for GR1T2 Pick Place Waist Enabled Mimic environment.""" + + def __post_init__(self): + # Calling post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "gr1t2_pick_place_waist_enabled_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.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 = [] + 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 000000000000..3af586c7dfb3 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2024-2026, 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 000000000000..2ea2cf68b85e --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/data_classes.py @@ -0,0 +1,86 @@ +# Copyright (c) 2024-2026, 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 dataclasses import dataclass + +import torch + + +@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 000000000000..8d16a5ca14b8 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2024-2026, 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 000000000000..dca2945822a3 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py @@ -0,0 +1,279 @@ +# Copyright (c) 2024-2026, 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" # noqa: E501 + + # 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 000000000000..83ae8e656846 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env.py @@ -0,0 +1,91 @@ +# Copyright (c) 2024-2026, 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): + """Recorder for Locomanipulation SDG output data.""" + + 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 000000000000..f3c5dd6c47cb --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py @@ -0,0 +1,47 @@ +# Copyright (c) 2024-2026, 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 000000000000..b2fdbdfb8527 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/occupancy_map_utils.py @@ -0,0 +1,742 @@ +# Copyright (c) 2024-2026, 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 os +import tempfile +from dataclasses import dataclass + +import cv2 +import numpy as np +import PIL.Image +import torch +import yaml +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 000000000000..f3e203f401e8 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/path_utils.py @@ -0,0 +1,215 @@ +# Copyright (c) 2024-2026, 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 000000000000..dc9c969171c0 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py @@ -0,0 +1,190 @@ +# Copyright (c) 2024-2026, 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 random + +import numpy as np +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 000000000000..73406a187ffd --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/transform_utils.py @@ -0,0 +1,48 @@ +# Copyright (c) 2024-2026, 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/motion_planners/curobo/curobo_planner.py b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py new file mode 100644 index 000000000000..a7e1ebb36252 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py @@ -0,0 +1,1951 @@ +# Copyright (c) 2024-2026, 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 logging +from dataclasses import dataclass +from typing import Any + +import numpy as np +import torch + +from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModelState +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.sphere_fit import SphereFitType +from curobo.geom.types import WorldConfig +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.state import JointState +from curobo.util.logger import setup_curobo_logger +from curobo.util.usd_helper import UsdHelper +from curobo.util_file import load_yaml +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig + +import isaaclab.utils.math as PoseUtils +from isaaclab.assets import Articulation +from isaaclab.envs.manager_based_env import ManagerBasedEnv +from isaaclab.managers import SceneEntityCfg +from isaaclab.sim.spawners.materials import PreviewSurfaceCfg +from isaaclab.sim.spawners.meshes import MeshSphereCfg, spawn_mesh_sphere + +from isaaclab_mimic.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg +from isaaclab_mimic.motion_planners.motion_planner_base import MotionPlannerBase + + +class PlannerLogger: + """Logger class for motion planner debugging and monitoring. + + This class provides standard logging functionality while maintaining isolation from + the main application's logging configuration. The logger supports configurable verbosity + levels and formats messages consistently for debugging motion planning operations, + collision checking, and object manipulation. + """ + + def __init__(self, name: str, level: int = logging.INFO): + """Initialize the logger with specified name and level. + + Args: + name: Logger name for identification in log messages + level: Logging level (DEBUG, INFO, WARNING, ERROR) + """ + self._name = name + self._level = level + self._logger = None + + @property + def logger(self): + """Get the underlying logger instance, initializing it if needed. + + Returns: + Configured Python logger instance with stream handler and formatter + """ + if self._logger is None: + self._logger = logging.getLogger(self._name) + if not self._logger.handlers: + handler = logging.StreamHandler() + formatter = logging.Formatter("%(name)s - %(levelname)s - %(message)s") + handler.setFormatter(formatter) + self._logger.addHandler(handler) + self._logger.setLevel(self._level) + return self._logger + + def debug(self, msg, *args, **kwargs): + """Log debug-level message for detailed internal state information. + + Args: + msg: Message string or format string + *args: Positional arguments for message formatting + **kwargs: Keyword arguments passed to underlying logger + """ + self.logger.debug(msg, *args, **kwargs) + + def info(self, msg, *args, **kwargs): + """Log info-level message for important operational events. + + Args: + msg: Message string or format string + *args: Positional arguments for message formatting + **kwargs: Keyword arguments passed to underlying logger + """ + self.logger.info(msg, *args, **kwargs) + + def warning(self, msg, *args, **kwargs): + """Log warning-level message for potentially problematic conditions. + + Args: + msg: Message string or format string + *args: Positional arguments for message formatting + **kwargs: Keyword arguments passed to underlying logger + """ + self.logger.warning(msg, *args, **kwargs) + + def error(self, msg, *args, **kwargs): + """Log error-level message for serious problems and failures. + + Args: + msg: Message string or format string + *args: Positional arguments for message formatting + **kwargs: Keyword arguments passed to underlying logger + """ + self.logger.error(msg, *args, **kwargs) + + +@dataclass +class Attachment: + """Stores object attachment information for robot manipulation. + + This dataclass tracks the relative pose between an attached object and its parent link, + enabling the robot to maintain consistent object positioning during motion planning. + """ + + pose: Pose # Relative pose from parent link to object + parent: str # Parent link name + + +class CuroboPlanner(MotionPlannerBase): + """Motion planner for robot manipulation using cuRobo. + + This planner provides collision-aware motion planning capabilities for robotic manipulation tasks. + It integrates with Isaac Lab environments to: + + - Update collision world from current stage state + - Plan collision-free paths to target poses + - Handle object attachment and detachment during manipulation + - Execute planned motions with proper collision checking + + The planner uses cuRobo for fast motion generation and supports + multi-phase planning for contact scenarios like grasping and placing objects. + """ + + def __init__( + self, + env: ManagerBasedEnv, + robot: Articulation, + config: CuroboPlannerCfg, + task_name: str | None = None, + env_id: int = 0, + collision_checker: CollisionCheckerType = CollisionCheckerType.MESH, + num_trajopt_seeds: int = 12, + num_graph_seeds: int = 12, + interpolation_dt: float = 0.05, + ) -> None: + """Initialize the motion planner for a specific environment. + + Sets up the cuRobo motion generator with collision checking, configures the robot model, + and prepares visualization components if enabled. The planner is isolated to CUDA device + regardless of Isaac Lab's device configuration. + + Args: + env: The Isaac Lab environment instance containing the robot and scene + robot: Robot articulation to plan motions for + config: Configuration object containing planner parameters and settings + task_name: Task name for auto-configuration + env_id: Environment ID for multi-environment setups (0 to num_envs-1) + collision_checker: Type of collision checker + num_trajopt_seeds: Number of seeds for trajectory optimization + num_graph_seeds: Number of seeds for graph search + interpolation_dt: Time step for interpolating waypoints + + Raises: + ValueError: If ``robot_config_file`` is not provided + """ + # Initialize base class + super().__init__(env=env, robot=robot, env_id=env_id, debug=config.debug_planner) + + # Initialize planner logger with debug level based on config + log_level = logging.DEBUG if config.debug_planner else logging.INFO + self.logger = PlannerLogger(f"CuroboPlanner_{env_id}", log_level) + + # Store instance variables + self.config: CuroboPlannerCfg = config + self.n_repeat: int | None = self.config.n_repeat + self.step_size: float | None = self.config.motion_step_size + self.visualize_plan: bool = self.config.visualize_plan + self.visualize_spheres: bool = self.config.visualize_spheres + + # Log the config parameter values + self.logger.info(f"Config parameter values: {self.config}") + + # Initialize plan visualizer if enabled + if self.visualize_plan: + from isaaclab_mimic.motion_planners.curobo.plan_visualizer import PlanVisualizer + + # Use env-local base translation for multi-env rendering consistency + env_origin = self.env.scene.env_origins[env_id, :3] + base_translation = (self.robot.data.root_pos_w[env_id, :3] - env_origin).detach().cpu().numpy() + self.plan_visualizer = PlanVisualizer( + robot_name=self.config.robot_name, + recording_id=f"curobo_plan_{env_id}", + debug=config.debug_planner, + base_translation=base_translation, + ) + + # Store attached objects as Attachment objects + self.attached_objects: dict[str, Attachment] = {} # object_name -> Attachment + + # Initialize cuRobo components - FORCE CUDA DEVICE FOR ISOLATION + setup_curobo_logger("warn") + + # Force cuRobo to always use CUDA device regardless of Isaac Lab device + # This isolates the motion planner from Isaac Lab's device configuration + self.tensor_args: TensorDeviceType + if torch.cuda.is_available(): + idx = self.config.cuda_device if self.config.cuda_device is not None else torch.cuda.current_device() + self.tensor_args = TensorDeviceType(device=torch.device(f"cuda:{idx}"), dtype=torch.float32) + self.logger.debug(f"cuRobo motion planner initialized on CUDA device {idx}") + else: + # Fallback to CPU if CUDA not available, but this may cause issues + self.tensor_args = TensorDeviceType() + self.logger.warning("CUDA not available, cuRobo using CPU - this may cause device compatibility issues") + + # Load robot configuration + if self.config.robot_config_file is None: + raise ValueError("robot_config_file is required") + robot_cfg_file = self.config.robot_config_file + robot_cfg: dict[str, Any] = load_yaml(robot_cfg_file)["robot_cfg"] + self.logger.info(f"Loaded robot configuration from {robot_cfg_file}") + + # Configure collision spheres + if self.config.collision_spheres_file: + robot_cfg["kinematics"]["collision_spheres"] = self.config.collision_spheres_file + + # Configure extra collision spheres + if self.config.extra_collision_spheres: + robot_cfg["kinematics"]["extra_collision_spheres"] = self.config.extra_collision_spheres + + self.robot_cfg: dict[str, Any] = robot_cfg + + # Load world configuration using the config's method + world_cfg: WorldConfig = self.config.get_world_config() + + # Create motion generator config with parameters from configuration + motion_gen_config: MotionGenConfig = MotionGenConfig.load_from_robot_config( + robot_cfg, + world_cfg, + tensor_args=self.tensor_args, + collision_checker_type=self.config.collision_checker_type, + num_trajopt_seeds=self.config.num_trajopt_seeds, + num_graph_seeds=self.config.num_graph_seeds, + interpolation_dt=self.config.interpolation_dt, + collision_cache=self.config.collision_cache_size, + trajopt_tsteps=self.config.trajopt_tsteps, + collision_activation_distance=self.config.collision_activation_distance, + position_threshold=self.config.position_threshold, + rotation_threshold=self.config.rotation_threshold, + ) + + # Create motion generator + self.motion_gen: MotionGen = MotionGen(motion_gen_config) + + # Set motion generator reference for plan visualizer if enabled + if self.visualize_plan: + self.plan_visualizer.set_motion_generator_reference(self.motion_gen) + + # Create plan config with parameters from configuration + self.plan_config: MotionGenPlanConfig = MotionGenPlanConfig( + enable_graph=self.config.enable_graph, + enable_graph_attempt=self.config.enable_graph_attempt, + max_attempts=self.config.max_planning_attempts, + enable_finetune_trajopt=self.config.enable_finetune_trajopt, + time_dilation_factor=self.config.time_dilation_factor, + ) + + # Create USD helper + self.usd_helper: UsdHelper = UsdHelper() + self.usd_helper.load_stage(env.scene.stage) + + # Initialize planning state + self._current_plan: JointState | None = None + self._plan_index: int = 0 + + # Initialize visualization state + self.frame_counter: int = 0 + self.spheres: list[tuple[str, float]] | None = None + self.sphere_update_freq: int = self.config.sphere_update_freq + + # Warm up planner + self.logger.info("Warming up motion planner...") + self.motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False) + + # Read static world geometry once + self._initialize_static_world() + + # Defer object validation baseline until first update_world() call when scene is fully loaded + self._expected_objects: set[str] | None = None + + # Define supported cuRobo primitive types for object discovery and pose synchronization + self.primitive_types: list[str] = ["mesh", "cuboid", "sphere", "capsule", "cylinder", "voxel", "blox"] + + # Cache object mappings + # Only recompute when objects are added/removed, not when poses change + self._cached_object_mappings: dict[str, str] | None = None + + # ===================================================================================== + # DEVICE CONVERSION UTILITIES + # ===================================================================================== + + def _to_curobo_device(self, tensor: torch.Tensor) -> torch.Tensor: + """Convert tensor to cuRobo device for isolated device management. + + Ensures all tensors used by cuRobo are on CUDA device, providing device isolation + from Isaac Lab's potentially different device configuration. This prevents device + mismatch errors and optimizes cuRobo performance. + + Args: + tensor: Input tensor (may be on any device) + + Returns: + Tensor converted to cuRobo's CUDA device with appropriate dtype + """ + return tensor.to(device=self.tensor_args.device, dtype=self.tensor_args.dtype) + + def _to_env_device(self, tensor: torch.Tensor) -> torch.Tensor: + """Convert tensor back to environment device for Isaac Lab compatibility. + + Converts cuRobo tensors back to the environment's device to ensure compatibility + with Isaac Lab operations that expect tensors on the environment's configured device. + + Args: + tensor: Input tensor from cuRobo operations (typically on CUDA) + + Returns: + Tensor converted to environment's device while preserving dtype + """ + return tensor.to(device=self.env.device, dtype=tensor.dtype) + + # ===================================================================================== + # INITIALIZATION AND CONFIGURATION + # ===================================================================================== + + def _initialize_static_world(self) -> None: + """Initialize static world geometry from USD stage. + + Reads static environment geometry once during planner initialization to establish + the base collision world. This includes walls, tables, bins, and other fixed obstacles + that don't change during the simulation. Dynamic objects are synchronized separately + in update_world() to maintain performance. + """ + env_prim_path = f"/World/envs/env_{self.env_id}" + robot_prim_path = self.config.robot_prim_path or f"{env_prim_path}/Robot" + + ignore_list = self.config.world_ignore_substrings or [ + f"{env_prim_path}/Robot", + f"{env_prim_path}/target", + "/World/defaultGroundPlane", + "/curobo", + ] + + self._static_world_config = self.usd_helper.get_obstacles_from_stage( + only_paths=[env_prim_path], + reference_prim_path=robot_prim_path, + ignore_substring=ignore_list, + ) + self._static_world_config = self._static_world_config.get_collision_check_world() + + # Initialize cuRobo world with static geometry + self.motion_gen.update_world(self._static_world_config) + + # ===================================================================================== + # PROPERTIES AND BASIC GETTERS + # ===================================================================================== + + @property + def attached_link(self) -> str: + """Default link name for object attachment operations.""" + return self.config.attached_object_link_name + + @property + def attachment_links(self) -> set[str]: + """Set of parent link names that currently have attached objects.""" + return {attachment.parent for attachment in self.attached_objects.values()} + + @property + def current_plan(self) -> JointState | None: + """Current plan from cuRobo motion generator.""" + return self._current_plan + + # ===================================================================================== + # WORLD AND OBJECT MANAGEMENT, ATTACHMENT, AND DETACHMENT + # ===================================================================================== + + def get_object_pose(self, object_name: str) -> Pose | None: + """Retrieve object pose from cuRobo's collision world model. + + Searches the collision world model for the specified object and returns its current + pose. This is useful for attachment calculations and debugging collision world state. + The method handles both mesh and cuboid object types automatically. + + Args: + object_name: Short object name used in Isaac Lab scene (e.g., "cube_1") + + Returns: + Object pose in cuRobo coordinate frame, or None if object not found + """ + # Get cached object mappings + object_mappings = self._get_object_mappings() + world_model = self.motion_gen.world_coll_checker.world_model + + object_path = object_mappings.get(object_name) + if not object_path: + self.logger.debug(f"Object {object_name} not found in world model") + return None + + # Search for object in world model + for obj_list, _ in [ + (world_model.mesh, "mesh"), + (world_model.cuboid, "cuboid"), + ]: + if not obj_list: + continue + + for obj in obj_list: + if obj.name and object_path in str(obj.name): + if obj.pose is not None: + return Pose.from_list(obj.pose, tensor_args=self.tensor_args) + + self.logger.debug(f"Object {object_name} found in mappings but pose not available") + return None + + def get_attached_pose(self, link_name: str, joint_state: JointState | None = None) -> Pose: + """Calculate pose of specified link using forward kinematics. + + Computes the world pose of any robot link at the given joint configuration. + This is essential for attachment calculations where we need to know the exact + pose of the parent link to compute relative object positions. + + Args: + link_name: Name of the robot link to get pose for + joint_state: Joint configuration to use for calculation, uses current state if None + + Returns: + World pose of the specified link in cuRobo coordinate frame + + Raises: + KeyError: If link_name is not found in the computed link poses + """ + if joint_state is None: + joint_state = self._get_current_joint_state_for_curobo() + + # Get all link states using the robot model + link_state = self.motion_gen.kinematics.get_state( + q=joint_state.position.detach().clone().to(device=self.tensor_args.device, dtype=self.tensor_args.dtype), + calculate_jacobian=False, + ) + + # Extract all link poses + link_poses = {} + if link_state.links_position is not None and link_state.links_quaternion is not None: + for i, link in enumerate(link_state.link_names): + link_poses[link] = self._make_pose( + position=link_state.links_position[..., i, :], + quaternion=link_state.links_quaternion[..., i, :], + name=link, + ) + + # For attached object link, use ee_link from robot config as parent + if link_name == self.config.attached_object_link_name: + ee_link = self.config.ee_link_name or self.robot_cfg["kinematics"]["ee_link"] + if ee_link in link_poses: + self.logger.debug(f"Using {ee_link} for {link_name}") + return link_poses[ee_link] + + # Return directly for other links + if link_name in link_poses: + return link_poses[link_name] + raise KeyError(f"Link {link_name} not found in computed link poses") + + def create_attachment( + self, object_name: str, link_name: str | None = None, joint_state: JointState | None = None + ) -> Attachment: + """Create attachment relationship between object and robot link. + + Computes the relative pose between an object and a robot link to enable the robot + to carry the object consistently during motion planning. The attachment stores the transform + from the parent link frame to the object frame, which remains constant while grasped. + + Args: + object_name: Name of the object to attach + link_name: Parent link for attachment, uses default attached_object_link if None + joint_state: Robot configuration for calculation, uses current state if None + + Returns: + Attachment object containing relative pose and parent link information + """ + if link_name is None: + link_name = self.attached_link + if joint_state is None: + joint_state = self._get_current_joint_state_for_curobo() + + # Get current link pose + link_pose = self.get_attached_pose(link_name, joint_state) + self.logger.info(f"Getting object pose for {object_name}") + obj_pose = self.get_object_pose(object_name) + + # Compute relative pose + attach_pose = link_pose.inverse().multiply(obj_pose) + + self.logger.debug(f"Creating attachment for {object_name} to {link_name}") + self.logger.debug(f"Link pose: {link_pose.position}") + self.logger.debug(f"Object pose (ACTUAL): {obj_pose.position}") + self.logger.debug(f"Computed relative pose: {attach_pose.position}") + + return Attachment(attach_pose, link_name) + + def update_world(self) -> None: + """Synchronize collision world with current Isaac Lab scene state. + + Updates all dynamic object poses in cuRobo's collision world to match their current + positions in Isaac Lab. This ensures collision checking uses accurate object positions + after simulation steps, resets, or manual object movements. Static world geometry + is loaded once during initialization and not updated here for performance. + + The method validates that the set of objects hasn't changed at runtime, as cuRobo + requires world model reinitialization when objects are added or removed. + + Raises: + RuntimeError: If the set of objects has changed at runtime + """ + + # Establish validation baseline on first call, validate on subsequent calls + if self._expected_objects is None: + self._expected_objects = set(self._get_world_object_names()) + self.logger.debug(f"Established object validation baseline: {len(self._expected_objects)} objects") + else: + # Subsequent calls: validate no changes + current_objects = set(self._get_world_object_names()) + if current_objects != self._expected_objects: + added = current_objects - self._expected_objects + removed = self._expected_objects - current_objects + + error_msg = "World objects changed at runtime!\n" + if added: + error_msg += f"Added: {added}\n" + if removed: + error_msg += f"Removed: {removed}\n" + error_msg += "cuRobo world model must be reinitialized." + + # Invalidate cached mappings since object set changed + self._cached_object_mappings = None + + raise RuntimeError(error_msg) + + # Sync object poses with Isaac Lab + self._sync_object_poses_with_isaaclab() + + if self.visualize_spheres: + self._update_sphere_visualization(force_update=True) + + if torch.cuda.is_available(): + torch.cuda.synchronize() + + def _get_world_object_names(self) -> list[str]: + """Extract all object names from cuRobo's collision world model. + + Iterates through all supported primitive types (mesh, cuboid, sphere, etc.) in the + collision world and collects their names. This is used for world validation to detect + when objects are added or removed at runtime. + + Returns: + List of all object names currently in the collision world model + """ + try: + world_model = self.motion_gen.world_coll_checker.world_model + + # Handle case where world_model might be a list + if isinstance(world_model, list): + if len(world_model) <= self.env_id: + return [] + world_model = world_model[self.env_id] + + object_names = [] + + # Get all primitive object names using the defined primitive types + for primitive_type in self.primitive_types: + if hasattr(world_model, primitive_type) and getattr(world_model, primitive_type): + primitive_list = getattr(world_model, primitive_type) + for primitive in primitive_list: + if primitive.name: + object_names.append(str(primitive.name)) + + return object_names + + except Exception as e: + self.logger.debug(f"ERROR getting world object names: {e}") + return [] + + def _sync_object_poses_with_isaaclab(self) -> None: + """Synchronize cuRobo collision world with Isaac Lab object positions. + + Updates all dynamic object poses in cuRobo's world model to match their current + positions in Isaac Lab. This ensures accurate collision checking after simulation + steps or manual object movements. Static objects (bins, tables, walls) are skipped + for performance as they shouldn't move during simulation. + + The method updates both the world model and the collision checker to ensure + consistency across all cuRobo components. + """ + # Get cached object mappings and world model + object_mappings = self._get_object_mappings() + world_model = self.motion_gen.world_coll_checker.world_model + rigid_objects = self.env.scene.rigid_objects + + updated_count = 0 + + for object_name, object_path in object_mappings.items(): + if object_name not in rigid_objects: + continue + + # Skip static mesh objects - they should not be dynamically updated + static_objects = getattr(self.config, "static_objects", []) + if any(static_name in object_name.lower() for static_name in static_objects): + self.logger.debug(f"SYNC: Skipping static object {object_name}") + continue + + # Get current pose from Lab (may be on CPU or CUDA depending on --device flag) + obj = rigid_objects[object_name] + env_origin = self.env.scene.env_origins[self.env_id] + current_pos_raw = obj.data.root_pos_w[self.env_id] - env_origin + current_quat_raw = obj.data.root_quat_w[self.env_id] # (w, x, y, z) + + # Convert to cuRobo device and extract float values for pose list + current_pos = self._to_curobo_device(current_pos_raw) + current_quat = self._to_curobo_device(current_quat_raw) + + # Convert to cuRobo pose format [x, y, z, w, x, y, z] + pose_list = [ + float(current_pos[0].item()), + float(current_pos[1].item()), + float(current_pos[2].item()), + float(current_quat[0].item()), + float(current_quat[1].item()), + float(current_quat[2].item()), + float(current_quat[3].item()), + ] + + # Update object pose in cuRobo's world model + if self._update_object_in_world_model(world_model, object_name, object_path, pose_list): + updated_count += 1 + + self.logger.debug(f"SYNC: Updated {updated_count} object poses in cuRobo world model") + + # Sync object poses with collision checker + if updated_count > 0: + # Update individual obstacle poses in collision checker + # This preserves static mesh objects unlike load_collision_model which rebuilds everything + for object_name, object_path in object_mappings.items(): + if object_name not in rigid_objects: + continue + + # Skip static mesh objects - they should not be dynamically updated + static_objects = getattr(self.config, "static_objects", []) + if any(static_name in object_name.lower() for static_name in static_objects): + continue + + # Get current pose and update in collision checker + obj = rigid_objects[object_name] + env_origin = self.env.scene.env_origins[self.env_id] + current_pos_raw = obj.data.root_pos_w[self.env_id] - env_origin + current_quat_raw = obj.data.root_quat_w[self.env_id] + + current_pos = self._to_curobo_device(current_pos_raw) + current_quat = self._to_curobo_device(current_quat_raw) + + # Create cuRobo pose and update collision checker directly + curobo_pose = self._make_pose(position=current_pos, quaternion=current_quat) + self.motion_gen.world_coll_checker.update_obstacle_pose( # type: ignore + object_path, curobo_pose, update_cpu_reference=True + ) + + self.logger.debug(f"Updated {updated_count} object poses in collision checker") + + def _get_object_mappings(self) -> dict[str, str]: + """Get object mappings with caching for performance optimization. + + Returns cached mappings if available, otherwise computes and caches them. + Cache is invalidated when the object set changes. + + Returns: + Dictionary mapping Isaac Lab object names to their corresponding USD paths + """ + if self._cached_object_mappings is None: + world_model = self.motion_gen.world_coll_checker.world_model + rigid_objects = self.env.scene.rigid_objects + self._cached_object_mappings = self._discover_object_mappings(world_model, rigid_objects) + self.logger.debug(f"Computed and cached object mappings: {len(self._cached_object_mappings)} objects") + + return self._cached_object_mappings + + def _discover_object_mappings(self, world_model, rigid_objects) -> dict[str, str]: + """Build mapping between Isaac Lab object names and cuRobo world paths. + + Automatically discovers the correspondence between Isaac Lab's rigid object names + and their full USD paths in cuRobo's world model. This mapping is essential for + pose synchronization and attachment operations, as cuRobo uses full USD paths + while Isaac Lab uses short object names. + + Args: + world_model: cuRobo's collision world model containing primitive objects + rigid_objects: Isaac Lab's rigid objects dictionary + + Returns: + Dictionary mapping Isaac Lab object names to their corresponding USD paths + """ + mappings = {} + env_prefix = f"/World/envs/env_{self.env_id}/" + world_object_paths = [] + + # Collect all primitive objects from cuRobo world model + for primitive_type in self.primitive_types: + primitive_list = getattr(world_model, primitive_type) + for primitive in primitive_list: + if primitive.name and env_prefix in str(primitive.name): + world_object_paths.append(str(primitive.name)) + + # Match Isaac Lab object names to world paths + for object_name in rigid_objects.keys(): + # Direct name matching + for path in world_object_paths: + if object_name.lower().replace("_", "") in path.lower().replace("_", ""): + mappings[object_name] = path + self.logger.debug(f"MAPPING: {object_name} -> {path}") + break + else: + self.logger.debug(f"WARNING: Could not find world path for {object_name}") + + return mappings + + def _update_object_in_world_model( + self, world_model, object_name: str, object_path: str, pose_list: list[float] + ) -> bool: + """Update a single object's pose in cuRobo's collision world model. + + Searches through all primitive types in the world model to find the specified object + and updates its pose. Uses flexible matching to handle variations in path naming + between Isaac Lab and cuRobo representations. + + Args: + world_model: cuRobo's collision world model + object_name: Short object name from Isaac Lab (e.g., "cube_1") + object_path: Full USD path for the object in cuRobo world + pose_list: New pose as [x, y, z, w, x, y, z] list in cuRobo format + + Returns: + True if object was found and successfully updated, False otherwise + """ + # Handle case where world_model might be a list + if isinstance(world_model, list): + if len(world_model) > self.env_id: + world_model = world_model[self.env_id] + else: + return False + + # Update all primitive types + for primitive_type in self.primitive_types: + primitive_list = getattr(world_model, primitive_type) + for primitive in primitive_list: + if primitive.name: + primitive_name = str(primitive.name) + # Use bidirectional matching for robust path matching + if object_path == primitive_name or object_path in primitive_name or primitive_name in object_path: + primitive.pose = pose_list + self.logger.debug(f"Updated {primitive_type} {object_name} pose") + return True + + self.logger.debug(f"WARNING: Object {object_name} not found in world model") + return False + + def _attach_object(self, object_name: str, object_path: str, env_id: int) -> bool: + """Attach an object to the robot for manipulation planning. + + Establishes an attachment between the specified object and the robot's end-effector + or configured attachment link. This enables the robot to carry the object during + motion planning while maintaining proper collision checking. The object's collision + geometry is disabled in the world model since it's now part of the robot. + + Args: + object_name: Short name of the object to attach (e.g., "cube_2") + object_path: Full USD path for the object in cuRobo world model + env_id: Environment ID for multi-environment support + + Returns: + True if attachment succeeded, False if attachment failed + """ + current_joint_state = self._get_current_joint_state_for_curobo() + + self.logger.debug(f"Attaching {object_name} at path {object_path}") + + # Create attachment record (relative pose object-frame to parent link) + attachment = self.create_attachment( + object_name, + self.config.attached_object_link_name, + current_joint_state, + ) + self.attached_objects[object_name] = attachment + success = self.motion_gen.attach_objects_to_robot( + joint_state=current_joint_state, + object_names=[object_path], + link_name=self.config.attached_object_link_name, + surface_sphere_radius=self.config.surface_sphere_radius, + sphere_fit_type=SphereFitType.SAMPLE_SURFACE, + world_objects_pose_offset=None, + ) + + if success: + self.logger.debug(f"Successfully attached {object_name}") + self.logger.debug(f"Current attached objects: {list(self.attached_objects.keys())}") + + # Force sphere visualization update + if self.visualize_spheres: + self._update_sphere_visualization(force_update=True) + + self.logger.info(f"Sphere count after attach is successful: {self._count_active_spheres()}") + + # Deactivate the original obstacle as it's now carried by the robot + self.motion_gen.world_coll_checker.enable_obstacle(object_path, enable=False) + + return True + else: + self.logger.error(f"cuRobo attach_objects_to_robot failed for {object_name}") + # Clean up on failure + if object_name in self.attached_objects: + del self.attached_objects[object_name] + return False + + def _detach_objects(self, link_names: set[str] | None = None) -> bool: + """Detach objects from robot and restore collision checking. + + Removes object attachments from specified links and re-enables collision checking + for both the objects and the parent links. This is necessary when placing objects + or changing grasps. All attached objects are detached if no specific links are provided. + + Args: + link_names: Set of parent link names to detach objects from, detaches all if None + + Returns: + True if detachment operations completed successfully, False otherwise + """ + if link_names is None: + link_names = self.attachment_links + + self.logger.debug(f"Detaching objects from links: {link_names}") + self.logger.debug(f"Current attached objects: {list(self.attached_objects.keys())}") + + # Get cached object mappings to find the USD path for re-enabling + object_mappings = self._get_object_mappings() + + detached_info = [] + detached_links = set() + for object_name, attachment in list(self.attached_objects.items()): + if attachment.parent not in link_names: + continue + + # Find object path and re-enable it in the world + object_path = object_mappings.get(object_name) + if object_path: + self.motion_gen.world_coll_checker.enable_obstacle(object_path, enable=True) # type: ignore + self.logger.debug(f"Re-enabled obstacle {object_path}") + + # Collect the link that will need re-enabling + detached_links.add(attachment.parent) + + # Remove from attached objects and log info + del self.attached_objects[object_name] + detached_info.append((object_name, attachment.parent)) + + if detached_info: + for obj_name, parent_link in detached_info: + self.logger.debug(f"Detached {obj_name} from {parent_link}") + + # Re-enable collision checking for the attachment links (following the planning pattern) + if detached_links: + self._set_active_links(list(detached_links), active=True) + self.logger.debug(f"Re-enabled collision for attachment links: {detached_links}") + + # Call cuRobo's detach for each link + for link_name in link_names: + self.motion_gen.detach_object_from_robot(link_name=link_name) + self.logger.debug(f"Called cuRobo detach for link {link_name}") + + return True + + def get_attached_objects(self) -> list[str]: + """Get list of currently attached object names. + + Returns the short names of all objects currently attached to the robot. + These names correspond to Isaac Lab scene object names, not full USD paths. + + Returns: + List of attached object names (e.g., ["cube_1", "cube_2"])""" + return list(self.attached_objects.keys()) + + def has_attached_objects(self) -> bool: + """Check if any objects are currently attached to the robot. + + Useful for determining gripper state and collision checking configuration + before planning motions. + + Returns: + True if one or more objects are attached, False if no attachments exist + """ + return len(self.attached_objects) != 0 + + # ===================================================================================== + # JOINT STATE AND KINEMATICS + # ===================================================================================== + + def _get_current_joint_state_for_curobo(self) -> JointState: + """ + Construct the current joint state for cuRobo with zero velocity and acceleration. + + This helper reads the robot's joint positions from Isaac Lab for the current environment + and pairs them with zero velocities and accelerations as required by cuRobo planning. + All tensors are moved to the cuRobo device and reordered to match the kinematic chain + used by the cuRobo motion generator. + + Returns: + JointState on the cuRobo device, ordered according to + `self.motion_gen.kinematics.joint_names`, with position from the robot + and zero velocity/acceleration. + """ + # Fetch joint position (shape: [1, num_joints]) + joint_pos_raw: torch.Tensor = self.robot.data.joint_pos[self.env_id, :].unsqueeze(0) + joint_vel_raw: torch.Tensor = torch.zeros_like(joint_pos_raw) + joint_acc_raw: torch.Tensor = torch.zeros_like(joint_pos_raw) + + # Move to cuRobo device + joint_pos: torch.Tensor = self._to_curobo_device(joint_pos_raw) + joint_vel: torch.Tensor = self._to_curobo_device(joint_vel_raw) + joint_acc: torch.Tensor = self._to_curobo_device(joint_acc_raw) + + cu_js: JointState = JointState( + position=joint_pos, + velocity=joint_vel, + acceleration=joint_acc, + joint_names=self.robot.data.joint_names, + tensor_args=self.tensor_args, + ) + return cu_js.get_ordered_joint_state(self.motion_gen.kinematics.joint_names) + + def get_ee_pose(self, joint_state: JointState) -> Pose: + """Compute end-effector pose from joint configuration. + + Uses cuRobo's forward kinematics to calculate the end-effector pose + at the specified joint configuration. Handles device conversion to ensure + compatibility with cuRobo's CUDA-based computations. + + Args: + joint_state: Robot joint configuration to compute end-effector pose from + + Returns: + End-effector pose in world coordinates + """ + # Ensure joint state is on CUDA device for cuRobo + if isinstance(joint_state.position, torch.Tensor): + cuda_position = self._to_curobo_device(joint_state.position) + else: + cuda_position = self._to_curobo_device(torch.tensor(joint_state.position)) + + # Create new joint state with CUDA tensors + cuda_joint_state = JointState( + position=cuda_position, + velocity=( + self._to_curobo_device(joint_state.velocity.detach().clone()) + if joint_state.velocity is not None + else torch.zeros_like(cuda_position) + ), + acceleration=( + self._to_curobo_device(joint_state.acceleration.detach().clone()) + if joint_state.acceleration is not None + else torch.zeros_like(cuda_position) + ), + joint_names=joint_state.joint_names, + tensor_args=self.tensor_args, + ) + + kin_state: Any = self.motion_gen.rollout_fn.compute_kinematics(cuda_joint_state) + return kin_state.ee_pose + + # ===================================================================================== + # PLANNING CORE METHODS + # ===================================================================================== + + def _make_pose( + self, + position: torch.Tensor | np.ndarray | list[float] | None = None, + quaternion: torch.Tensor | np.ndarray | list[float] | None = None, + *, + name: str | None = None, + normalize_rotation: bool = False, + ) -> Pose: + """Create a cuRobo Pose with sensible defaults and device/dtype alignment. + + Auto-populates missing fields with identity values and ensures tensors are + on the cuRobo device with the correct dtype. + + Args: + position: Optional position as Tensor/ndarray/list. Defaults to [0, 0, 0]. + quaternion: Optional quaternion as Tensor/ndarray/list (w, x, y, z). Defaults to [1, 0, 0, 0]. + name: Optional name of the link that this pose represents. + normalize_rotation: Whether to normalize the quaternion inside Pose. + + Returns: + Pose: A cuRobo Pose on the configured cuRobo device and dtype. + """ + # Defaults + if position is None: + position = torch.tensor([0.0, 0.0, 0.0], dtype=self.tensor_args.dtype, device=self.tensor_args.device) + if quaternion is None: + quaternion = torch.tensor( + [1.0, 0.0, 0.0, 0.0], dtype=self.tensor_args.dtype, device=self.tensor_args.device + ) + + # Convert to tensors if needed + if not isinstance(position, torch.Tensor): + position = torch.tensor(position, dtype=self.tensor_args.dtype, device=self.tensor_args.device) + else: + position = self._to_curobo_device(position) + + if not isinstance(quaternion, torch.Tensor): + quaternion = torch.tensor(quaternion, dtype=self.tensor_args.dtype, device=self.tensor_args.device) + else: + quaternion = self._to_curobo_device(quaternion) + + return Pose(position=position, quaternion=quaternion, name=name, normalize_rotation=normalize_rotation) + + def _set_active_links(self, links: list[str], active: bool) -> None: + """Configure collision checking for specific robot links. + + Enables or disables collision sphere checking for the specified links. + This is essential for contact scenarios where certain links (like fingers + or attachment points) need collision checking disabled to allow contact + with objects being grasped. + + Args: + links: List of link names to enable or disable collision checking for + active: True to enable collision checking, False to disable + """ + for link in links: + if active: + self.motion_gen.kinematics.kinematics_config.enable_link_spheres(link) + else: + self.motion_gen.kinematics.kinematics_config.disable_link_spheres(link) + + def plan_motion( + self, + target_pose: torch.Tensor, + step_size: float | None = None, + enable_retiming: bool | None = None, + ) -> bool: + """Plan collision-free motion to target pose. + + Plans a trajectory from the current robot configuration to the specified target pose. + The method assumes that world updates and locked joint configurations have already + been handled. Supports optional linear retiming for consistent execution speeds. + + Args: + target_pose: Target end-effector pose as 4x4 transformation matrix + step_size: Step size for linear retiming, enables retiming if provided + enable_retiming: Whether to enable linear retiming, auto-detected from step_size if None + + Returns: + True if planning succeeded and a valid trajectory was found, False otherwise + """ + if enable_retiming is None: + enable_retiming = step_size is not None + + # Ensure target pose is on cuRobo device (CUDA) for device isolation + target_pose_cuda = self._to_curobo_device(target_pose) + + target_pos: torch.Tensor + target_rot: torch.Tensor + target_pos, target_rot = PoseUtils.unmake_pose(target_pose_cuda) + target_curobo_pose: Pose = self._make_pose( + position=target_pos, + quaternion=PoseUtils.quat_from_matrix(target_rot), + ) + + start_state: JointState = self._get_current_joint_state_for_curobo() + + self.logger.debug(f"Retiming enabled: {enable_retiming}, Step size: {step_size}") + + success: bool = self._plan_to_contact( + start_state=start_state, + goal_pose=target_curobo_pose, + retreat_distance=self.config.retreat_distance, + approach_distance=self.config.approach_distance, + retime_plan=enable_retiming, + step_size=step_size, + contact=False, + ) + + # Visualize plan if enabled + if success and self.visualize_plan and self._current_plan is not None: + # Get current spheres for visualization + self._sync_object_poses_with_isaaclab() + cu_js = self._get_current_joint_state_for_curobo() + sphere_list = self.motion_gen.kinematics.get_robot_as_spheres(cu_js.position)[0] + + # Split spheres into robot and attached object spheres + robot_spheres = [] + attached_spheres = [] + robot_link_count = 0 + + # Count robot link spheres + robot_links = [ + link + for link in self.robot_cfg["kinematics"]["collision_link_names"] + if link != self.config.attached_object_link_name + ] + for link_name in robot_links: + link_spheres = self.motion_gen.kinematics.kinematics_config.get_link_spheres(link_name) + if link_spheres is not None: + robot_link_count += int(torch.sum(link_spheres[:, 3] > 0).item()) + + # Split spheres + for i, sphere in enumerate(sphere_list): + if i < robot_link_count: + robot_spheres.append(sphere) + else: + attached_spheres.append(sphere) + + # Compute end-effector positions for visualization + ee_positions_list = [] + try: + for i in range(len(self._current_plan.position)): + js: JointState = self._current_plan[i] + kin = self.motion_gen.compute_kinematics(js) + ee_pos = kin.ee_position if hasattr(kin, "ee_position") else kin.ee_pose.position + ee_positions_list.append(ee_pos.cpu().numpy().squeeze()) + + self.logger.debug( + f"Link names from kinematics: {kin.link_names if len(ee_positions_list) > 0 else 'No EE positions'}" + ) + + except Exception as e: + self.logger.debug(f"Failed to compute EE positions for visualization: {e}") + ee_positions_list = None + + try: + world_scene = WorldConfig.get_scene_graph(self.motion_gen.world_coll_checker.world_model) + except Exception: + world_scene = None + + # Visualize plan + self.plan_visualizer.visualize_plan( + plan=self._current_plan, + target_pose=target_pose, + robot_spheres=robot_spheres, + attached_spheres=attached_spheres, + ee_positions=np.array(ee_positions_list) if ee_positions_list else None, + world_scene=world_scene, + ) + + # Animate EE positions over the timeline for playback + if ee_positions_list: + self.plan_visualizer.animate_plan(np.array(ee_positions_list)) + + # Animate spheres along the path for collision visualization + self.plan_visualizer.animate_spheres_along_path( + plan=self._current_plan, + robot_spheres_at_start=robot_spheres, + attached_spheres_at_start=attached_spheres, + timeline="sphere_animation", + interpolation_steps=15, # More steps for smoother animation + ) + + return success + + def _plan_to_contact_pose( + self, + start_state: JointState, + goal_pose: Pose, + contact: bool = True, + ) -> bool: + """Plan motion with configurable collision checking for contact scenarios. + + Plans a trajectory while optionally disabling collision checking for hand links and + attached objects. This is crucial for grasping and placing operations where contact + is expected and collision checking would prevent successful planning. + + Args: + start_state: Starting joint configuration for planning + goal_pose: Target pose to reach in cuRobo coordinate frame + contact: True to disable hand/attached object collisions for contact planning + retime_plan: Whether to apply linear retiming to the resulting trajectory + step_size: Step size for retiming if retime_plan is True + + Returns: + True if planning succeeded, False if no valid trajectory found + """ + # Use configured hand link names instead of hardcoded ones + disable_link_names: list[str] = self.config.hand_link_names.copy() + link_spheres: dict[str, torch.Tensor] = {} + + # Count spheres before planning + sphere_counts_before = self._count_active_spheres() + self.logger.debug( + f"Planning phase contact={contact}: Spheres before - Total: {sphere_counts_before['total']}, Robot:" + f" {sphere_counts_before['robot_links']}, Attached: {sphere_counts_before['attached_objects']}" + ) + + if contact: + # Store current spheres for the attached link so we can restore later + attached_links: list[str] = list(self.attachment_links) + for attached_link in attached_links: + link_spheres[attached_link] = self.motion_gen.kinematics.kinematics_config.get_link_spheres( + attached_link + ).clone() + + self.logger.debug(f"Attached link: {attached_links}") + # Disable all specified links for contact planning + self.logger.debug(f"Disable link names: {disable_link_names}") + self._set_active_links(disable_link_names + attached_links, active=False) + else: + self.logger.debug(f"Disable link names: {disable_link_names}") + + # Count spheres after link disabling + sphere_counts_after_disable = self._count_active_spheres() + self.logger.debug( + f"Planning phase contact={contact}: Spheres after disable - Total:" + f" {sphere_counts_after_disable['total']}, Robot: {sphere_counts_after_disable['robot_links']}," + f" Attached: {sphere_counts_after_disable['attached_objects']}" + ) + + planning_success = False + try: + result: Any = self.motion_gen.plan_single(start_state, goal_pose, self.plan_config) + + if result.success.item(): + if result.optimized_plan is not None and len(result.optimized_plan.position) != 0: + self._current_plan = result.optimized_plan + self.logger.debug(f"Using optimized plan with {len(self._current_plan.position)} waypoints") + else: + self._current_plan = result.get_interpolated_plan() + self.logger.debug(f"Using interpolated plan with {len(self._current_plan.position)} waypoints") + + self._current_plan = self.motion_gen.get_full_js(self._current_plan) + common_js_names: list[str] = [ + x for x in self.robot.data.joint_names if x in self._current_plan.joint_names + ] + self._current_plan = self._current_plan.get_ordered_joint_state(common_js_names) + self._plan_index = 0 + + planning_success = True + self.logger.debug(f"Contact planning succeeded with {len(self._current_plan.position)} waypoints") + else: + self.logger.debug(f"Contact planning failed: {result.status}") + + except Exception as e: + self.logger.debug(f"Error during planning: {e}") + + # Always restore sphere state after planning, regardless of success + if contact: + self._set_active_links(disable_link_names, active=True) + for attached_link, spheres in link_spheres.items(): + self.motion_gen.kinematics.kinematics_config.update_link_spheres(attached_link, spheres) + return planning_success + + def _plan_to_contact( + self, + start_state: JointState, + goal_pose: Pose, + retreat_distance: float, + approach_distance: float, + contact: bool = False, + retime_plan: bool = False, + step_size: float | None = None, + ) -> bool: + """Execute multi-phase contact planning with approach and retreat phases. + + Implements a planning strategy for manipulation tasks that require approach and contact handling. + Plans multiple trajectory segments with different collision checking configurations. + + Args: + start_state: Starting joint state for planning + goal_pose: Target pose to reach + retreat_distance: Distance to retreat before transition to contact + approach_distance: Distance to approach before final pose + contact: Whether to enable contact planning mode + retime_plan: Whether to retime the resulting plan + step_size: Step size for retiming (only used if retime_plan is True) + + Returns: + True if all planning phases succeeded, False if any phase failed + """ + self.logger.debug(f"Multi-phase planning: retreat={retreat_distance}, approach={approach_distance}") + + target_poses: list[Pose] = [] + contacts: list[bool] = [] + + if retreat_distance is not None and retreat_distance > 0: + ee_pose: Pose = self.get_ee_pose(start_state) + retreat_pose: Pose = ee_pose.multiply( + self._make_pose( + position=[0.0, 0.0, -retreat_distance], + ) + ) + target_poses.append(retreat_pose) + contacts.append(True) + contacts.append(contact) + if approach_distance is not None and approach_distance > 0: + approach_pose: Pose = goal_pose.multiply( + self._make_pose( + position=[0.0, 0.0, -approach_distance], + ) + ) + target_poses.append(approach_pose) + contacts.append(True) + + target_poses.append(goal_pose) + + current_state: JointState = start_state + full_plan: JointState | None = None + + for i, (target_pose, contact_flag) in enumerate(zip(target_poses, contacts)): + self.logger.debug( + f"Planning phase {i + 1} of {len(target_poses)}: contact={contact_flag} (collision" + f" {'disabled' if contact_flag else 'enabled'})" + ) + + success: bool = self._plan_to_contact_pose( + start_state=current_state, + goal_pose=target_pose, + contact=contact_flag, + ) + + if not success: + self.logger.debug(f"Phase {i + 1} planning failed") + return False + + if full_plan is None: + full_plan = self._current_plan + else: + full_plan = full_plan.stack(self._current_plan) + + last_waypoint: torch.Tensor = self._current_plan.position[-1] + current_state = JointState( + position=last_waypoint.unsqueeze(0), + velocity=torch.zeros_like(last_waypoint.unsqueeze(0)), + acceleration=torch.zeros_like(last_waypoint.unsqueeze(0)), + joint_names=self._current_plan.joint_names, + ) + current_state = current_state.get_ordered_joint_state(self.motion_gen.kinematics.joint_names) + + self._current_plan = full_plan + self._plan_index = 0 + + if retime_plan and step_size is not None: + original_length: int = len(self._current_plan.position) + self._current_plan = self._linearly_retime_plan(step_size=step_size, plan=self._current_plan) + self.logger.debug( + f"Retimed complete plan from {original_length} to {len(self._current_plan.position)} waypoints" + ) + + self.logger.debug(f"Multi-phase planning succeeded with {len(self._current_plan.position)} total waypoints") + + return True + + def _linearly_retime_plan( + self, + step_size: float = 0.01, + plan: JointState | None = None, + ) -> JointState | None: + """Apply linear retiming to trajectory for consistent execution speed. + + Resamples the trajectory with uniform spacing between waypoints to ensure + consistent motion speed during execution. + + Args: + step_size: Desired spacing between waypoints in joint space + plan: Trajectory to retime, uses current plan if None + + Returns: + Retimed trajectory with uniform waypoint spacing, or None if plan is invalid + """ + if plan is None: + plan = self._current_plan + + if plan is None or len(plan.position) == 0: + return plan + + path = plan.position + + if len(path) <= 1: + return plan + + deltas = path[1:] - path[:-1] + distances = torch.norm(deltas, dim=-1) + + waypoints = [path[0]] + for distance, waypoint in zip(distances, path[1:]): + if distance > 1e-6: + waypoints.append(waypoint) + + if len(waypoints) <= 1: + return plan + + waypoints = torch.stack(waypoints) + + if len(waypoints) > 1: + deltas = waypoints[1:] - waypoints[:-1] + distances = torch.norm(deltas, dim=-1) + cum_distances = torch.cat([torch.zeros(1, device=distances.device), torch.cumsum(distances, dim=0)]) + + if len(waypoints) < 2 or cum_distances[-1] < 1e-6: + return plan + + total_distance = cum_distances[-1] + num_steps = int(torch.ceil(total_distance / step_size).item()) + 1 + + # Create linearly spaced distances + sampled_distances = torch.linspace(cum_distances[0], cum_distances[-1], num_steps, device=cum_distances.device) + + # Linear interpolation + indices = torch.searchsorted(cum_distances, sampled_distances) + indices = torch.clamp(indices, 1, len(cum_distances) - 1) + + # Get interpolation weights + weights = (sampled_distances - cum_distances[indices - 1]) / ( + cum_distances[indices] - cum_distances[indices - 1] + ) + weights = weights.unsqueeze(-1) + + # Interpolate waypoints + sampled_waypoints = (1 - weights) * waypoints[indices - 1] + weights * waypoints[indices] + + self.logger.debug( + f"Retiming: {len(path)} to {len(sampled_waypoints)} waypoints, " + f"Distance: {total_distance:.3f}, Step size: {step_size}" + ) + + retimed_plan = JointState( + position=sampled_waypoints, + velocity=torch.zeros( + (len(sampled_waypoints), plan.velocity.shape[-1]), + device=plan.velocity.device, + dtype=plan.velocity.dtype, + ), + acceleration=torch.zeros( + (len(sampled_waypoints), plan.acceleration.shape[-1]), + device=plan.acceleration.device, + dtype=plan.acceleration.dtype, + ), + joint_names=plan.joint_names, + ) + + return retimed_plan + + def has_next_waypoint(self) -> bool: + """Check if more waypoints remain in the current trajectory. + + Returns: + True if there are unprocessed waypoints, False if trajectory is complete or empty + """ + return self._current_plan is not None and self._plan_index < len(self._current_plan.position) + + def get_next_waypoint_ee_pose(self) -> Pose: + """Get end-effector pose for the next waypoint in the trajectory. + + Advances the trajectory execution index and computes the end-effector pose + for the next waypoint using forward kinematics. + + Returns: + End-effector pose for the next waypoint in world coordinates + + Raises: + IndexError: If no more waypoints remain in the trajectory + """ + if not self.has_next_waypoint(): + raise IndexError("No more waypoints in the plan.") + next_joint_state: JointState = self._current_plan[self._plan_index] + self._plan_index += 1 + eef_state: CudaRobotModelState = self.motion_gen.compute_kinematics(next_joint_state) + return eef_state.ee_pose + + def reset_plan(self) -> None: + """Reset trajectory execution state. + + Clears the current trajectory and resets the execution index to zero. + This prepares the planner for a new planning operation. + """ + self._plan_index = 0 + self._current_plan = None + if self.visualize_plan and hasattr(self, "plan_visualizer"): + self.plan_visualizer.clear_visualization() + self.plan_visualizer.mark_idle() + + def get_planned_poses(self) -> list[torch.Tensor]: + """Extract all end-effector poses from current trajectory. + + Computes end-effector poses for all waypoints in the current trajectory without + affecting the execution state. Optionally repeats the final pose multiple times + if configured for stable goal reaching. + + Returns: + List of end-effector poses as 4x4 transformation matrices, with optional repetition + """ + if self._current_plan is None: + return [] + + # Save current execution state + original_plan_index = self._plan_index + + # Iterate through the plan to get all poses + planned_poses: list[torch.Tensor] = [] + self._plan_index = 0 + while self.has_next_waypoint(): + # Directly use the joint state from the plan to compute pose + # without advancing the main plan index in get_next_waypoint_ee_pose + next_joint_state: JointState = self._current_plan[self._plan_index] + self._plan_index += 1 # Manually advance index for this loop + eef_state: Any = self.motion_gen.compute_kinematics(next_joint_state) + planned_pose: Pose | None = eef_state.ee_pose + + if planned_pose is not None: + # Convert pose to environment device for compatibility + position = ( + self._to_env_device(planned_pose.position) + if isinstance(planned_pose.position, torch.Tensor) + else planned_pose.position + ) + rotation = ( + self._to_env_device(planned_pose.get_rotation()) + if isinstance(planned_pose.get_rotation(), torch.Tensor) + else planned_pose.get_rotation() + ) + planned_poses.append(PoseUtils.make_pose(position, rotation)[0]) + + # Restore the original execution state + self._plan_index = original_plan_index + + if self.n_repeat is not None and self.n_repeat > 0 and len(planned_poses) > 0: + self.logger.info(f"Repeating final pose {self.n_repeat} times") + final_pose: torch.Tensor = planned_poses[-1] + planned_poses.extend([final_pose] * self.n_repeat) + + return planned_poses + + # ===================================================================================== + # VISUALIZATION METHODS + # ===================================================================================== + + def _update_visualization_at_joint_positions(self, joint_positions: torch.Tensor) -> None: + """Update sphere visualization for the robot at specific joint positions. + + Args: + joint_positions: Joint configuration to visualize collision spheres at + """ + if not self.visualize_spheres: + return + + self.frame_counter += 1 + if self.frame_counter % self.sphere_update_freq != 0: + return + + original_joints: torch.Tensor = self.robot.data.joint_pos[self.env_id].clone() + + try: + # Ensure joint positions are on environment device for robot commands + env_joint_positions = ( + self._to_env_device(joint_positions) if joint_positions.device != self.env.device else joint_positions + ) + self.robot.set_joint_position_target(env_joint_positions.view(1, -1), env_ids=[self.env_id]) + self._update_sphere_visualization(force_update=False) + finally: + self.robot.set_joint_position_target(original_joints.unsqueeze(0), env_ids=[self.env_id]) + + def _update_sphere_visualization(self, force_update: bool = True) -> None: + """Update visual representation of robot collision spheres in USD stage. + + Creates or updates sphere primitives in the USD stage to show the robot's + collision model. Different colors are used for robot links (green) and + attached objects (orange) to help distinguish collision boundaries. + + Args: + force_update: True to recreate all spheres, False to update existing positions only + """ + # Get current sphere data + cu_js = self._get_current_joint_state_for_curobo() + sphere_position = self._to_curobo_device( + cu_js.position if isinstance(cu_js.position, torch.Tensor) else torch.tensor(cu_js.position) + ) + sphere_list = self.motion_gen.kinematics.get_robot_as_spheres(sphere_position)[0] + robot_link_count = self._get_robot_link_sphere_count() + + # Remove existing spheres if force update or first time + if (self.spheres is None or force_update) and self.spheres is not None: + self._remove_existing_spheres() + + # Initialize sphere list if needed + if self.spheres is None or force_update: + self.spheres = [] + + # Create or update all spheres + for sphere_idx, sphere in enumerate(sphere_list): + if not self._is_valid_sphere(sphere): + continue + + sphere_config = self._create_sphere_config(sphere_idx, sphere, robot_link_count) + prim_path = f"/curobo/robot_sphere_{sphere_idx}" + + # Remove old sphere if updating + if not (self.spheres is None or force_update): + if sphere_idx < len(self.spheres) and self.usd_helper.stage.GetPrimAtPath(prim_path).IsValid(): + self.usd_helper.stage.RemovePrim(prim_path) + + # Spawn sphere + spawn_mesh_sphere(prim_path=prim_path, translation=sphere_config["position"], cfg=sphere_config["cfg"]) + + # Store reference if creating new + if self.spheres is None or force_update or sphere_idx >= len(self.spheres): + self.spheres.append((prim_path, float(sphere.radius))) + + def _get_robot_link_sphere_count(self) -> int: + """Calculate total number of collision spheres for robot links excluding attached objects. + + Iterates through all robot collision links (excluding the attached object link) and + counts the active collision spheres for each link. This count is used to determine + which spheres in the visualization represent robot links vs attached objects. + + Returns: + Total number of active collision spheres for robot links only + """ + sphere_config = self.motion_gen.kinematics.kinematics_config + robot_links = [ + link + for link in self.robot_cfg["kinematics"]["collision_link_names"] + if link != self.config.attached_object_link_name + ] + return sum( + int(torch.sum(sphere_config.get_link_spheres(link_name)[:, 3] > 0).item()) for link_name in robot_links + ) + + def _remove_existing_spheres(self) -> None: + """Remove all existing sphere visualization primitives from the USD stage. + + Iterates through all stored sphere references and removes their corresponding + USD primitives from the stage. This is used during force updates or when + recreating the sphere visualization from scratch. + """ + stage = self.usd_helper.stage + for prim_path, _ in self.spheres: + if stage.GetPrimAtPath(prim_path).IsValid(): + stage.RemovePrim(prim_path) + + def _is_valid_sphere(self, sphere) -> bool: + """Validate sphere data for visualization rendering. + + Checks if a sphere has valid position coordinates (no NaN values) and a positive + radius. Invalid spheres are skipped during visualization to prevent rendering errors. + + Args: + sphere: Sphere object containing position and radius data + + Returns: + True if sphere has valid position and positive radius, False otherwise + """ + pos_tensor = torch.tensor(sphere.position, dtype=torch.float32) + return not torch.isnan(pos_tensor).any() and sphere.radius > 0 + + def _create_sphere_config(self, sphere_idx: int, sphere, robot_link_count: int) -> dict: + """Create sphere configuration with position and visual properties for USD rendering. + + Determines sphere type (robot link vs attached object), calculates world position, + and creates the appropriate visual configuration including colors and materials. + Robot link spheres are green with lower opacity, while attached object spheres + are orange with higher opacity for better distinction. + + Args: + sphere_idx: Index of the sphere in the sphere list + sphere: Sphere object containing position and radius data + robot_link_count: Total number of robot link spheres (for type determination) + + Returns: + Dictionary containing 'position' (world coordinates) and 'cfg' (MeshSphereCfg) + """ + + is_attached = sphere_idx >= robot_link_count + color = (1.0, 0.5, 0.0) if is_attached else (0.0, 1.0, 0.0) + opacity = 0.9 if is_attached else 0.5 + + # Calculate position in world frame (do not use env_origin) + root_translation = (self.robot.data.root_pos_w[self.env_id, :3]).detach().cpu().numpy() + position = sphere.position.cpu().numpy() if hasattr(sphere.position, "cpu") else sphere.position + if not is_attached: + position = position + root_translation + + return { + "position": position, + "cfg": MeshSphereCfg( + radius=float(sphere.radius), + visual_material=PreviewSurfaceCfg(diffuse_color=color, opacity=opacity, emissive_color=color), + ), + } + + def _is_sphere_attached_object(self, sphere_index: int, sphere_config: Any) -> bool: + """Check if a sphere belongs to attached_object link. + + Args: + sphere_index: Index of the sphere to check + sphere_config: Sphere configuration object + + Returns: + True if sphere belongs to an attached object, False if it's a robot link sphere + """ + # Get total number of robot link spheres (excluding attached_object) + robot_links = [ + link + for link in self.robot_cfg["kinematics"]["collision_link_names"] + if link != self.config.attached_object_link_name + ] + + total_robot_spheres = 0 + for link_name in robot_links: + try: + link_spheres = sphere_config.get_link_spheres(link_name) + active_spheres = torch.sum(link_spheres[:, 3] > 0).item() + total_robot_spheres += int(active_spheres) + except Exception: + continue + + # If sphere_index >= total_robot_spheres, it's an attached object sphere + is_attached = sphere_index >= total_robot_spheres + + if sphere_index < 5: # Debug first few spheres + self.logger.debug( + f"SPHERE {sphere_index}: total_robot_spheres={total_robot_spheres}, is_attached={is_attached}" + ) + + return is_attached + + # ===================================================================================== + # HIGH-LEVEL PLANNING INTERFACE + # ===================================================================================== + + def update_world_and_plan_motion( + self, + target_pose: torch.Tensor, + expected_attached_object: str | None = None, + env_id: int = 0, + step_size: float | None = None, + enable_retiming: bool | None = None, + ) -> bool: + """Complete planning pipeline with world updates and object attachment handling. + + Provides a high-level interface that handles the complete planning workflow: + world synchronization, object attachment/detachment, gripper configuration, + and motion planning. + + Args: + target_pose: Target end-effector pose as 4x4 transformation matrix + expected_attached_object: Name of object that should be attached, None for no attachment + env_id: Environment ID for multi-environment setups + step_size: Step size for linear retiming if retiming is enabled + enable_retiming: Whether to enable linear retiming of trajectory + + Returns: + True if complete planning pipeline succeeded, False if any step failed + """ + # Always reset the plan before starting a new one to ensure a clean state + self.reset_plan() + + self.logger.debug("=== MOTION PLANNING DEBUG ===") + self.logger.debug(f"Expected attached object: {expected_attached_object}") + + self.update_world() + gripper_closed = expected_attached_object is not None + self._set_gripper_state(gripper_closed) + current_attached = self.get_attached_objects() + gripper_pos = self.robot.data.joint_pos[env_id, -2:] + + self.logger.debug(f"Current attached objects: {current_attached}") + + # Attach object if expected but not currently attached + if expected_attached_object and expected_attached_object not in current_attached: + self.logger.debug(f"Need to attach {expected_attached_object}") + + object_mappings = self._get_object_mappings() + + self.logger.debug(f"Object mappings found: {list(object_mappings.keys())}") + + if expected_attached_object in object_mappings: + expected_path = object_mappings[expected_attached_object] + + self.logger.debug(f"Object path: {expected_path}") + + # Debug object poses + rigid_objects = self.env.scene.rigid_objects + if expected_attached_object in rigid_objects: + obj = rigid_objects[expected_attached_object] + origin = self.env.scene.env_origins[env_id] + obj_pos = obj.data.root_pos_w[env_id] - origin + self.logger.debug(f"Isaac Lab object position: {obj_pos}") + + # Debug end-effector position + ee_frame_cfg = SceneEntityCfg("ee_frame") + ee_frame = self.env.scene[ee_frame_cfg.name] + ee_pos = ee_frame.data.target_pos_w[env_id, 0, :] - origin + self.logger.debug(f"End-effector position: {ee_pos}") + + # Debug distance + distance = torch.linalg.vector_norm(obj_pos - ee_pos).item() + self.logger.debug(f"Distance EE to object: {distance:.4f}") + + # Debug gripper state + gripper_open_val = self.config.grasp_gripper_open_val + self.logger.debug(f"Gripper positions: {gripper_pos}") + self.logger.debug(f"Gripper open val: {gripper_open_val}") + + is_grasped = self._check_object_grasped(gripper_pos, expected_attached_object) + + self.logger.debug(f"Is grasped check result: {is_grasped}") + + if is_grasped: + self._attach_object(expected_attached_object, expected_path, env_id) + self.logger.debug(f"Attached {expected_attached_object}") + else: + self.logger.debug( + "Object not detected as grasped - attachment skipped" + ) # This will cause collision with ghost object! + else: + self.logger.debug(f"Object {expected_attached_object} not found in world mappings") + + # Detach objects if no object should be attached (i.e., placing/releasing) + if expected_attached_object is None and current_attached: + self.logger.debug("Detaching all objects as no object expected to be attached") + self._detach_objects() + + self.logger.debug(f"Planning motion with attached objects: {self.get_attached_objects()}") + + plan_success = self.plan_motion(target_pose, step_size, enable_retiming) + + self.logger.debug(f"Planning result: {plan_success}") + self.logger.debug("=== END POST-GRASP DEBUG ===") + + self._detach_objects() + + return plan_success + + # ===================================================================================== + # UTILITY METHODS + # ===================================================================================== + + def _check_object_grasped(self, gripper_pos: torch.Tensor, object_name: str) -> bool: + """Check if a specific object is currently grasped by the robot. + + Uses gripper position to determine if an object is grasped. + + Args: + gripper_pos: Gripper position tensor + object_name: Name of object to check (e.g., "cube_1") + + Returns: + True if object is detected as grasped + """ + gripper_open_val = self.config.grasp_gripper_open_val + object_grasped = gripper_pos[0].item() < gripper_open_val + + self.logger.info( + f"Object {object_name} is grasped: {object_grasped}" + if object_grasped + else f"Object {object_name} is not grasped" + ) + + return object_grasped + + def _set_gripper_state(self, has_attached_objects: bool) -> None: + """Configure gripper joint positions based on object attachment status. + + Sets the gripper to closed position when objects are attached and open position + when no objects are attached. This ensures proper collision checking and planning + with the correct gripper configuration. + + Args: + has_attached_objects: True if robot currently has attached objects requiring closed gripper + """ + if has_attached_objects: + # Closed gripper for grasping + locked_joints = self.config.gripper_closed_positions + else: + # Open gripper for manipulation + locked_joints = self.config.gripper_open_positions + + self.motion_gen.update_locked_joints(locked_joints, self.robot_cfg) + + def _count_active_spheres(self) -> dict[str, int]: + """Count active collision spheres by category for debugging. + + Analyzes the current collision sphere configuration to provide detailed + statistics about robot links vs attached object spheres. This is helpful + for debugging collision checking issues and attachment problems. + + Returns: + Dictionary containing sphere counts by category (total, robot_links, attached_objects) + """ + cu_js = self._get_current_joint_state_for_curobo() + + # Ensure position tensor is on CUDA for cuRobo + if isinstance(cu_js.position, torch.Tensor): + sphere_position = self._to_curobo_device(cu_js.position) + else: + # Convert list to tensor and move to CUDA + sphere_position = self._to_curobo_device(torch.tensor(cu_js.position)) + + sphere_list = self.motion_gen.kinematics.get_robot_as_spheres(sphere_position)[0] + + # Get sphere configuration + sphere_config = self.motion_gen.kinematics.kinematics_config + + # Count robot link spheres (excluding attached_object) + robot_links = [ + link + for link in self.robot_cfg["kinematics"]["collision_link_names"] + if link != self.config.attached_object_link_name + ] + robot_sphere_count = 0 + for link_name in robot_links: + if hasattr(sphere_config, "get_link_spheres"): + link_spheres = sphere_config.get_link_spheres(link_name) + if link_spheres is not None: + active_spheres = torch.sum(link_spheres[:, 3] > 0).item() + robot_sphere_count += int(active_spheres) + + # Count attached object spheres by checking actual sphere list + attached_sphere_count = 0 + + # Handle sphere_list as either a list or single Sphere object + total_spheres = len(list(sphere_list)) + + # Any spheres beyond robot_sphere_count are attached object spheres + attached_sphere_count = max(0, total_spheres - robot_sphere_count) + + self.logger.debug( + f"SPHERE COUNT: Total={total_spheres}, Robot={robot_sphere_count},Attached={attached_sphere_count}" + ) + + return { + "total": total_spheres, + "robot_links": robot_sphere_count, + "attached_objects": attached_sphere_count, + } diff --git a/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner_cfg.py new file mode 100644 index 000000000000..b3b3f7cce827 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner_cfg.py @@ -0,0 +1,466 @@ +# Copyright (c) 2024-2026, 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 os +import tempfile + +import yaml + +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import WorldConfig +from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path +from isaaclab.utils.configclass import configclass + + +@configclass +class CuroboPlannerCfg: + """Configuration for CuRobo motion planner. + + This dataclass provides a flexible configuration system for the CuRobo motion planner. + The base configuration is robot-agnostic, with factory methods providing pre-configured + settings for specific robots and tasks. + + Example Usage: + >>> # Use a pre-configured robot + >>> config = CuroboPlannerCfg.franka_config() + >>> + >>> # Or create from task name + >>> config = CuroboPlannerCfg.from_task_name("Isaac-Stack-Cube-Franka-v0") + >>> + >>> # Initialize planner with config + >>> planner = CuroboPlanner(env, robot, config) + + To add support for a new robot, see the factory methods section below for detailed instructions. + """ + + # Robot configuration + robot_config_file: str | None = None + """cuRobo robot configuration file (path defined by curobo api).""" + + robot_name: str = "" + """Robot name for visualization and identification.""" + + ee_link_name: str | None = None + """End-effector link name (auto-detected from robot config if None).""" + + # Gripper configuration + gripper_joint_names: list[str] = [] + """Names of gripper joints.""" + + gripper_open_positions: dict[str, float] = {} + """Open gripper positions for cuRobo to update spheres""" + + gripper_closed_positions: dict[str, float] = {} + """Closed gripper positions for cuRobo to update spheres""" + + # Hand link configuration (for contact planning) + hand_link_names: list[str] = [] + """Names of hand/finger links to disable during contact planning.""" + + # Attachment configuration + attached_object_link_name: str = "attached_object" + """Name of the link used for attaching objects.""" + + # World configuration + world_config_file: str = "collision_table.yml" + """CuRobo world configuration file (without path).""" + + # Static objects to not update in the world model + static_objects: list[str] = [] + """Names of static objects to not update in the world model.""" + + # Optional prim path configuration + robot_prim_path: str | None = None + """Absolute USD prim path to the robot root for world extraction; None derives it from environment root.""" + + world_ignore_substrings: list[str] | None = None + """List of substring patterns to ignore when extracting world obstacles + (e.g., default ground plane, debug prims). + """ + + # Motion planning parameters + collision_checker_type: CollisionCheckerType = CollisionCheckerType.MESH + """Type of collision checker to use.""" + + num_trajopt_seeds: int = 12 + """Number of seeds for trajectory optimization.""" + + num_graph_seeds: int = 12 + """Number of seeds for graph search.""" + + interpolation_dt: float = 0.05 + """Time step for interpolating waypoints.""" + + collision_cache_size: dict[str, int] = {"obb": 150, "mesh": 150} + """Cache sizes for different collision types.""" + + trajopt_tsteps: int = 32 + """Number of trajectory optimization time steps.""" + + collision_activation_distance: float = 0.0 + """Distance at which collision constraints are activated.""" + + approach_distance: float = 0.05 + """Distance to approach at the end of the plan.""" + + retreat_distance: float = 0.05 + """Distance to retreat at the start of the plan.""" + + grasp_gripper_open_val: float = 0.04 + """Gripper joint value when considered open for grasp detection.""" + + # Planning configuration + enable_graph: bool = True + """Whether to enable graph-based planning.""" + + enable_graph_attempt: int = 5 + """Number of graph planning attempts.""" + + max_planning_attempts: int = 15 + """Maximum number of planning attempts.""" + + enable_finetune_trajopt: bool = True + """Whether to enable trajectory optimization fine-tuning.""" + + time_dilation_factor: float = 1.0 + """Time dilation factor for planning.""" + + surface_sphere_radius: float = 0.005 + """Radius of surface spheres for collision checking.""" + + # Debug and visualization + n_repeat: int | None = None + """Number of times to repeat final waypoint for stabilization. If None, no repetition.""" + + motion_step_size: float | None = None + """Step size (in radians) for retiming motion plans. If None, no retiming.""" + + visualize_spheres: bool = False + """Visualize robot collision spheres. Note: only works for env 0.""" + + visualize_plan: bool = False + """Visualize motion plan in Rerun. Note: only works for env 0.""" + + debug_planner: bool = False + """Enable detailed motion planning debug information.""" + + sphere_update_freq: int = 5 + """Frequency to update sphere visualization, specified in number of frames.""" + + motion_noise_scale: float = 0.0 + """Scale of Gaussian noise to add to the planned waypoints. Defaults to 0.0 (no noise).""" + + # Collision sphere configuration + collision_spheres_file: str | None = None + """Collision spheres configuration file (auto-detected if None).""" + + extra_collision_spheres: dict[str, int] = {"attached_object": 100} + """Extra collision spheres for attached objects.""" + + position_threshold: float = 0.005 + """Position threshold for motion planning.""" + + rotation_threshold: float = 0.05 + """Rotation threshold for motion planning.""" + + cuda_device: int | None = 0 + """Preferred CUDA device index; None uses torch.cuda.current_device() (respects CUDA_VISIBLE_DEVICES).""" + + def get_world_config(self) -> WorldConfig: + """Load and prepare the world configuration. + + This method can be overridden in subclasses or customized per task + to provide different world configuration setups. + + Returns: + WorldConfig: The configured world for collision checking + """ + # Default implementation: just load the world config file + world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), self.world_config_file))) + return world_cfg + + def _get_world_config_with_table_adjustment(self) -> WorldConfig: + """Load world config with standard table adjustments. + + This is a helper method that implements the common pattern of adjusting + table height and combining mesh/cuboid worlds. Used by specific task configs. + + Returns: + WorldConfig: World configuration with adjusted table + """ + # Load the base world config + world_cfg_table = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), self.world_config_file))) + + # Adjust table height if cuboid exists and has a pose + if world_cfg_table.cuboid and len(world_cfg_table.cuboid) > 0 and world_cfg_table.cuboid[0].pose: + world_cfg_table.cuboid[0].pose[2] -= 0.02 + + # Get mesh world for additional collision objects + world_cfg_mesh = WorldConfig.from_dict( + load_yaml(join_path(get_world_configs_path(), self.world_config_file)) + ).get_mesh_world() + + # Adjust mesh configuration if it exists + if world_cfg_mesh.mesh and len(world_cfg_mesh.mesh) > 0: + mesh_obj = world_cfg_mesh.mesh[0] + if mesh_obj.name: + mesh_obj.name += "_mesh" + if mesh_obj.pose: + mesh_obj.pose[2] = -10.5 # Move mesh below scene + + # Combine cuboid and mesh worlds + world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg_mesh.mesh) + return world_cfg + + @classmethod + def _create_temp_robot_yaml(cls, base_yaml: str, urdf_path: str) -> str: + """Create a temporary robot configuration YAML with custom URDF path. + + Args: + base_yaml: Base robot configuration file name + urdf_path: Absolute path to the URDF file + + Returns: + Path to the temporary YAML file + + Raises: + FileNotFoundError: If the URDF file doesn't exist + """ + # Validate URDF path + if not os.path.isabs(urdf_path) or not os.path.isfile(urdf_path): + raise FileNotFoundError(f"URDF must be a local file: {urdf_path}") + + # Load base configuration + robot_cfg_path = get_robot_configs_path() + base_path = join_path(robot_cfg_path, base_yaml) + data = load_yaml(base_path) + print(f"urdf_path: {urdf_path}") + # Update URDF path + data["robot_cfg"]["kinematics"]["urdf_path"] = urdf_path + + # Write to temporary file + tmp_dir = tempfile.mkdtemp(prefix="curobo_robot_cfg_") + out_path = os.path.join(tmp_dir, base_yaml) + with open(out_path, "w") as f: + yaml.safe_dump(data, f, sort_keys=False) + + return out_path + + # ===================================================================================== + # FACTORY METHODS FOR ROBOT CONFIGURATIONS + # ===================================================================================== + """ + Creating Custom Robot Configurations + ===================================== + + To create a configuration for your own robot, follow these steps: + + 1. Create a Factory Method + --------------------------- + Define a classmethod that returns a configured instance: + + .. code-block:: python + + @classmethod + def my_robot_config(cls) -> "CuroboPlannerCfg": + # Option 1: Download from Nucleus (like Franka example) + urdf_path = f"{ISAACLAB_NUCLEUS_DIR}/path/to/my_robot.urdf" + local_urdf = retrieve_file_path(urdf_path, force_download=True) + + # Option 2: Use local file directly + # local_urdf = "/absolute/path/to/my_robot.urdf" + + # Create temporary YAML with custom URDF path + robot_cfg_file = cls._create_temp_robot_yaml("my_robot.yml", local_urdf) + + return cls( + # Required: Specify robot configuration file + robot_config_file=robot_cfg_file, # Use the generated YAML with custom URDF + robot_name="my_robot", + + # Gripper configuration (if robot has grippers) + gripper_joint_names=["gripper_left", "gripper_right"], + gripper_open_positions={"gripper_left": 0.05, "gripper_right": 0.05}, + gripper_closed_positions={"gripper_left": 0.01, "gripper_right": 0.01}, + + # Hand/finger links to disable during contact planning + hand_link_names=["finger_link_1", "finger_link_2", "palm_link"], + + # Optional: Absolute USD prim path to the robot root for world extraction; + # None derives it from environment root. + robot_prim_path=None, + + # Optional: List of substring patterns to ignore when extracting world obstacles + # (e.g., default ground plane, debug prims). + # None derives it from the environment root and adds some default patterns. + # This is useful for environments with a lot of prims. + world_ignore_substrings=None, + + # Optional: Custom collision spheres configuration + # Path relative to curobo (can override with custom spheres file) + collision_spheres_file="spheres/my_robot_spheres.yml", + + # Grasp detection threshold + grasp_gripper_open_val=0.05, + + # Motion planning parameters (tune for your robot) + approach_distance=0.05, # Distance to approach before grasping + retreat_distance=0.05, # Distance to retreat after grasping + time_dilation_factor=0.5, # Speed factor (0.5 = half speed) + + # Visualization options + visualize_spheres=False, + visualize_plan=False, + debug_planner=False, + ) + + 2. Task-Specific Configurations + -------------------------------- + For task-specific variants, create methods that modify the base config: + + .. code-block:: python + + @classmethod + def my_robot_pick_place_config(cls) -> "CuroboPlannerCfg": + config = cls.my_robot_config() # Start from base config + + # Override for pick-and-place tasks + config.approach_distance = 0.08 + config.retreat_distance = 0.10 + config.enable_finetune_trajopt = True + config.collision_activation_distance = 0.02 + + # Custom world configuration if needed + config.get_world_config = lambda: config._get_world_config_with_table_adjustment() + + return config + + 3. Register in from_task_name() + -------------------------------- + Add your robot detection logic to the from_task_name method: + + .. code-block:: python + + @classmethod + def from_task_name(cls, task_name: str) -> "CuroboPlannerCfg": + task_lower = task_name.lower() + + # Add your robot detection + if "my-robot" in task_lower: + if "pick-place" in task_lower: + return cls.my_robot_pick_place_config() + else: + return cls.my_robot_config() + + # ... existing robot checks ... + + Important Notes + --------------- + - The _create_temp_robot_yaml() helper creates a temporary YAML with your custom URDF + - If using Nucleus assets, retrieve_file_path() downloads them to a local temp directory + - The base robot YAML (e.g., "my_robot.yml") should exist in cuRobo's robot configs + + Best Practices + -------------- + 1. Start with conservative parameters (slow speed, large distances) + 2. Test with visualization enabled (visualize_plan=True) for debugging + 3. Tune collision_activation_distance based on controller precision to follow collision-free motion + 4. Adjust sphere counts in extra_collision_spheres for attached objects + 5. Use debug_planner=True when developing new configurations + """ + + @classmethod + def franka_config(cls) -> "CuroboPlannerCfg": + """Create configuration for Franka Panda robot. + + This method uses a custom URDF from Nucleus for the Franka robot. + + Returns: + CuroboPlannerCfg: Configuration for Franka robot + """ + urdf_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/SkillGenAssets/FrankaPanda/franka_panda.urdf" + local_urdf = retrieve_file_path(urdf_path, force_download=True) + + robot_cfg_file = cls._create_temp_robot_yaml("franka.yml", local_urdf) + + return cls( + robot_config_file=robot_cfg_file, + robot_name="franka", + gripper_joint_names=["panda_finger_joint1", "panda_finger_joint2"], + gripper_open_positions={"panda_finger_joint1": 0.04, "panda_finger_joint2": 0.04}, + gripper_closed_positions={"panda_finger_joint1": 0.023, "panda_finger_joint2": 0.023}, + hand_link_names=["panda_leftfinger", "panda_rightfinger", "panda_hand"], + collision_spheres_file="spheres/franka_mesh.yml", + grasp_gripper_open_val=0.04, + approach_distance=0.0, + retreat_distance=0.0, + max_planning_attempts=1, + time_dilation_factor=0.6, + enable_finetune_trajopt=True, + n_repeat=None, + motion_step_size=None, + visualize_spheres=False, + visualize_plan=False, + debug_planner=False, + sphere_update_freq=5, + motion_noise_scale=0.02, + # World extraction tuning for Franka envs + world_ignore_substrings=["/World/defaultGroundPlane", "/curobo"], + ) + + @classmethod + def franka_stack_cube_bin_config(cls) -> "CuroboPlannerCfg": + """Create configuration for Franka stacking cube in a bin.""" + config = cls.franka_config() + config.static_objects = ["bin", "table"] + config.gripper_closed_positions = {"panda_finger_joint1": 0.024, "panda_finger_joint2": 0.024} + config.approach_distance = 0.05 + config.retreat_distance = 0.07 + config.surface_sphere_radius = 0.01 + config.debug_planner = False + config.collision_activation_distance = 0.02 + config.visualize_plan = False + config.enable_finetune_trajopt = True + config.motion_noise_scale = 0.02 + config.get_world_config = lambda: config._get_world_config_with_table_adjustment() + return config + + @classmethod + def franka_stack_cube_config(cls) -> "CuroboPlannerCfg": + """Create configuration for Franka stacking a normal cube.""" + config = cls.franka_config() + config.static_objects = ["table"] + config.visualize_plan = False + config.debug_planner = False + config.motion_noise_scale = 0.02 + config.collision_activation_distance = 0.01 + config.approach_distance = 0.05 + config.retreat_distance = 0.05 + config.surface_sphere_radius = 0.01 + config.get_world_config = lambda: config._get_world_config_with_table_adjustment() + return config + + @classmethod + def from_task_name(cls, task_name: str) -> "CuroboPlannerCfg": + """Create configuration from task name. + + Args: + task_name: Task name (e.g., "Isaac-Stack-Cube-Bin-Franka-v0") + + Returns: + CuroboPlannerCfg: Configuration for the specified task + """ + task_lower = task_name.lower() + + if "stack-cube-bin" in task_lower: + return cls.franka_stack_cube_bin_config() + elif "stack-cube" in task_lower: + return cls.franka_stack_cube_config() + else: + # Default to Franka configuration + print(f"Warning: Unknown robot in task '{task_name}', using Franka configuration") + return cls.franka_config() diff --git a/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/plan_visualizer.py b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/plan_visualizer.py new file mode 100644 index 000000000000..0f5252e208c9 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/plan_visualizer.py @@ -0,0 +1,937 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Utility for visualizing motion plans using Rerun. + +This module provides tools to visualize motion plans, robot poses, and collision spheres +using Rerun's visualization capabilities. It helps in debugging and validating collision-free paths. +""" + +import atexit +import os +import signal +import subprocess +import threading +import time +import weakref +from typing import TYPE_CHECKING, Any, Optional + +import numpy as np +import torch + +# Check if rerun is installed +try: + import rerun as rr +except ImportError: + raise ImportError("Rerun is not installed!") + +from curobo.types.state import JointState + +import isaaclab.utils.math as PoseUtils + +# Import psutil for process management +try: + import psutil + + PSUTIL_AVAILABLE = True +except ImportError: + PSUTIL_AVAILABLE = False + print("Warning: psutil not available. Process monitoring will be limited.") + +if TYPE_CHECKING: # For type hints only + import trimesh + + +# Global registry to track all PlanVisualizer instances for cleanup +_GLOBAL_PLAN_VISUALIZERS: list["PlanVisualizer"] = [] + + +def _cleanup_all_plan_visualizers(): + """Enhanced global cleanup function with better process killing.""" + global _GLOBAL_PLAN_VISUALIZERS + + if PSUTIL_AVAILABLE: + killed_count = 0 + for proc in psutil.process_iter(["pid", "name", "cmdline"]): + # Check if it's a rerun process + if (proc.info["name"] and "rerun" in proc.info["name"].lower()) or ( + proc.info["cmdline"] and any("rerun" in str(arg).lower() for arg in proc.info["cmdline"]) + ): + proc.kill() + killed_count += 1 + + print(f"Killed {killed_count} Rerun viewer processes on script exit") + else: + # Fallback to pkill + subprocess.run(["pkill", "-f", "rerun"], stderr=subprocess.DEVNULL, check=False) + print("Used pkill fallback to close Rerun processes") + + # Also clean up individual instances + for visualizer in _GLOBAL_PLAN_VISUALIZERS[:]: + if not visualizer._closed: + visualizer.close() + + _GLOBAL_PLAN_VISUALIZERS.clear() + + +# Register global cleanup on module import +atexit.register(_cleanup_all_plan_visualizers) + + +class PlanVisualizer: + """Visualizes motion plans using Rerun. + + This class provides methods to visualize: + 1. Robot poses along a planned trajectory + 2. Attached objects and their collision spheres + 3. Robot collision spheres + 4. Target poses and waypoints + """ + + def __init__( + self, + robot_name: str = "panda", + recording_id: str | None = None, + debug: bool = False, + save_path: str | None = None, + base_translation: np.ndarray | None = None, + ): + """Initialize the plan visualizer. + + Args: + robot_name: Name of the robot for visualization + recording_id: Optional ID for the Rerun recording + debug: Whether to print debug information + save_path: Optional path to save the recording + base_translation: Optional base translation to apply to all visualized entities + """ + self.robot_name = robot_name + self.debug = debug + self.recording_id = recording_id or f"motion_plan_{robot_name}" + self.save_path = save_path + self._closed = False + # Translation offset applied to all visualized entities (for multi-env setups) + self._base_translation = ( + np.array(base_translation, dtype=float) if base_translation is not None else np.zeros(3) + ) + + # Enhanced process management + self._parent_pid = os.getpid() + self._monitor_thread = None + self._monitor_active = False + + # Motion generator reference for sphere animation (set by CuroboPlanner) + self._motion_gen_ref = None + + # Register this instance globally for cleanup + global _GLOBAL_PLAN_VISUALIZERS + _GLOBAL_PLAN_VISUALIZERS.append(self) + + # Initialize Rerun + rr.init(self.recording_id, spawn=False) + + # Spawn viewer and keep handle if provided so we can terminate it later + try: + self._rerun_process = rr.spawn() + except Exception: + # Older versions of Rerun may not return a process handle + self._rerun_process = None + + # Set up coordinate system + rr.log("world", rr.ViewCoordinates.RIGHT_HAND_Y_UP) + + # Store visualization state + self._current_frame = 0 + self._sphere_entities: dict[str, list[str]] = {"robot": [], "attached": [], "target": []} + + # Start enhanced parent process monitoring + self._start_parent_process_monitoring() + + # Use weakref.finalize for cleanup when object is garbage collected + self._finalizer = weakref.finalize( + self, self._cleanup_class_resources, self.recording_id, self.save_path, debug + ) + + # Also register atexit handler as backup for normal script termination + # Store values locally to avoid capturing self in the closure + recording_id = self.recording_id + save_path = self.save_path + debug_flag = debug + atexit.register(self._cleanup_class_resources, recording_id, save_path, debug_flag) + + # Store original signal handlers so we can restore them after cleanup + self._original_sigint_handler = signal.signal(signal.SIGINT, signal.SIG_DFL) + self._original_sigterm_handler = signal.signal(signal.SIGTERM, signal.SIG_DFL) + + # Handle Ctrl+C (SIGINT) and termination (SIGTERM) signals + def signal_handler(signum, frame): + if self.debug: + print(f"Received signal {signum}, closing Rerun viewer...") + self._cleanup_on_exit() + + # Restore original signal handler and re-raise the signal + if signum == signal.SIGINT: + signal.signal(signal.SIGINT, self._original_sigint_handler) + elif signum == signal.SIGTERM: + signal.signal(signal.SIGTERM, self._original_sigterm_handler) + os.kill(os.getpid(), signum) + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + if self.debug: + print(f"Initialized Rerun visualization with recording ID: {self.recording_id}") + if np.linalg.norm(self._base_translation) > 0: + print(f"Applying translation offset: {self._base_translation}") + if PSUTIL_AVAILABLE: + print("Enhanced process monitoring enabled") + + def _start_parent_process_monitoring(self) -> None: + """Start monitoring the parent process and cleanup when it dies.""" + if not PSUTIL_AVAILABLE: + if self.debug: + print("psutil not available, skipping parent process monitoring") + return + + self._monitor_active = True + + def monitor_parent_process() -> None: + """Monitor thread function that watches the parent process.""" + if self.debug: + print(f"Starting parent process monitor for PID {self._parent_pid}") + + # Get parent process handle + parent_process = psutil.Process(self._parent_pid) + + # Monitor parent process + while self._monitor_active: + try: + if not parent_process.is_running(): + if self.debug: + print(f"Parent process {self._parent_pid} died, cleaning up Rerun...") + self._kill_rerun_processes() + break + + # Check every 2 seconds + time.sleep(2) + + except (psutil.NoSuchProcess, psutil.AccessDenied): + if self.debug: + print(f"Parent process {self._parent_pid} no longer accessible, cleaning up...") + self._kill_rerun_processes() + break + except Exception as e: + if self.debug: + print(f"Error in parent process monitor: {e}") + break + + # Start monitor thread + self._monitor_thread = threading.Thread(target=monitor_parent_process, daemon=True) + self._monitor_thread.start() + + def _kill_rerun_processes(self) -> None: + """Enhanced method to kill Rerun viewer processes using psutil.""" + try: + if PSUTIL_AVAILABLE: + killed_count = 0 + for proc in psutil.process_iter(["pid", "name", "cmdline"]): + try: + # Check if it's a rerun process + is_rerun = False + + # Check process name + if proc.info["name"] and "rerun" in proc.info["name"].lower(): + is_rerun = True + + # Check command line arguments + if proc.info["cmdline"] and any("rerun" in str(arg).lower() for arg in proc.info["cmdline"]): + is_rerun = True + + if is_rerun: + proc.kill() + killed_count += 1 + if self.debug: + print(f"Killed Rerun process {proc.info['pid']} ({proc.info['name']})") + + except (psutil.NoSuchProcess, psutil.AccessDenied, psutil.ZombieProcess): + # Process already dead or inaccessible + pass + except Exception as e: + if self.debug: + print(f"Error killing process: {e}") + + if self.debug: + print(f"Killed {killed_count} Rerun processes using psutil") + + else: + # Fallback to pkill if psutil not available + result = subprocess.run(["pkill", "-f", "rerun"], stderr=subprocess.DEVNULL, check=False) + if self.debug: + print(f"Used pkill fallback (return code: {result.returncode})") + + except Exception as e: + if self.debug: + print(f"Error killing rerun processes: {e}") + + @staticmethod + def _cleanup_class_resources(recording_id: str, save_path: str | None, debug: bool) -> None: + """Static method for cleanup that doesn't hold references to the instance. + + This is called by weakref.finalize when the object is garbage collected. + """ + if debug: + print(f"Cleaning up Rerun visualization for {recording_id}") + + # Disconnect from Rerun + rr.disconnect() + + # Save to file if requested + if save_path is not None: + rr.save(save_path) + if debug: + print(f"Saved Rerun recording to {save_path}") + + # Enhanced process killing + if PSUTIL_AVAILABLE: + killed_count = 0 + for proc in psutil.process_iter(["pid", "name", "cmdline"]): + if (proc.info["name"] and "rerun" in proc.info["name"].lower()) or ( + proc.info["cmdline"] and any("rerun" in str(arg).lower() for arg in proc.info["cmdline"]) + ): + proc.kill() + killed_count += 1 + + if debug: + print(f"Killed {killed_count} Rerun processes during cleanup") + else: + subprocess.run(["pkill", "-f", "rerun"], stderr=subprocess.DEVNULL, check=False) + + if debug: + print("Cleanup completed") + + def _cleanup_on_exit(self) -> None: + """Manual cleanup method for signal handlers.""" + if not self._closed: + # Stop monitoring thread + self._monitor_active = False + + self.close() + self._kill_rerun_processes() + + def close(self) -> None: + """Close the Rerun visualization with enhanced cleanup.""" + if self._closed: + return + + # Stop parent process monitoring + self._monitor_active = False + if self._monitor_thread and self._monitor_thread.is_alive(): + # Give the thread a moment to stop gracefully + time.sleep(0.1) + + # Disconnect from Rerun (closes connections, servers, and files) + rr.disconnect() + + # Save to file if requested + if self.save_path is not None: + rr.save(self.save_path) + if self.debug: + print(f"Saved Rerun recording to {self.save_path}") + + self._closed = True + + # Terminate viewer process if we have a handle + try: + process = getattr(self, "_rerun_process", None) + if process is not None and process.poll() is None: + process.terminate() + try: + process.wait(timeout=5) + except Exception: + process.kill() + except Exception: + pass + + # Enhanced process killing + self._kill_rerun_processes() + + # Remove from global registry + global _GLOBAL_PLAN_VISUALIZERS + if self in _GLOBAL_PLAN_VISUALIZERS: + _GLOBAL_PLAN_VISUALIZERS.remove(self) + + if self.debug: + print("Closed Rerun visualization with enhanced cleanup") + + def visualize_plan( + self, + plan: JointState, + target_pose: torch.Tensor, + robot_spheres: list[Any] | None = None, + attached_spheres: list[Any] | None = None, + ee_positions: np.ndarray | None = None, + world_scene: Optional["trimesh.Scene"] = None, + ) -> None: + """Visualize a complete motion plan with all components. + + Args: + plan: Joint state trajectory to visualize + target_pose: Target end-effector pose + robot_spheres: Optional list of robot collision spheres + attached_spheres: Optional list of attached object spheres + ee_positions: Optional end-effector positions + world_scene: Optional world scene to visualize + """ + if self.debug: + robot_count = len(robot_spheres) if robot_spheres else 0 + attached_count = len(attached_spheres) if attached_spheres else 0 + offset_info = ( + f"offset={self._base_translation}" if np.linalg.norm(self._base_translation) > 0 else "no offset" + ) + print( + f"Visualizing plan: {len(plan.position)} waypoints, {robot_count} robot spheres (with offset)," + f" {attached_count} attached spheres (no offset), {offset_info}" + ) + + # Set timeline for static visualization (separate from animation) + rr.set_time("static_plan", sequence=self._current_frame) + self._current_frame += 1 + + # Clear previous visualization of dynamic entities (keep static meshes) + self._clear_visualization() + + # If a scene is supplied and not yet logged, draw it once + if world_scene is not None: + self._visualize_world_scene(world_scene) + + # Visualize target pose + self._visualize_target_pose(target_pose) + + # Visualize trajectory (end-effector positions if provided) + self._visualize_trajectory(plan, ee_positions) + + # Visualize spheres if provided + if robot_spheres: + self._visualize_robot_spheres(robot_spheres) + if attached_spheres: + self._visualize_attached_spheres(attached_spheres) + else: + # Clear any existing attached sphere visualization when no objects are attached + self._clear_attached_spheres() + + def _clear_visualization(self) -> None: + """Clear all visualization entities.""" + # Clear dynamic trajectory, target, and finger logs to avoid artifacts between visualizations + dynamic_paths = [ + "trajectory", + "target", + "anim", + ] + + for path in dynamic_paths: + rr.log(f"world/{path}", rr.Clear(recursive=True)) + + for entity_type, entities in self._sphere_entities.items(): + for entity in entities: + rr.log(f"world/{entity_type}/{entity}", rr.Clear(recursive=True)) + self._sphere_entities[entity_type] = [] + self._current_frame = 0 + + def clear_visualization(self) -> None: + """Public method to clear the visualization.""" + self._clear_visualization() + + def _visualize_target_pose(self, target_pose: torch.Tensor) -> None: + """Visualize the target end-effector pose. + + Args: + target_pose: Target pose as 4x4 transformation matrix + """ + pos, rot = PoseUtils.unmake_pose(target_pose) + + # Convert to numpy arrays + pos_np = pos.detach().cpu().numpy() if torch.is_tensor(pos) else np.array(pos) + rot_np = rot.detach().cpu().numpy() if torch.is_tensor(rot) else np.array(rot) + + # Ensure arrays are the right shape + pos_np = pos_np.reshape(-1) + rot_np = rot_np.reshape(3, 3) + + # Apply translation offset + pos_np += self._base_translation + + # Log target position + rr.log( + "world/target/position", + rr.Points3D( + positions=np.array([pos_np]), + colors=[[255, 0, 0]], # Red + radii=[0.02], + ), + ) + + # Log target orientation as coordinate frame + rr.log( + "world/target/frame", + rr.Transform3D( + translation=pos_np, + mat3x3=rot_np, + ), + ) + + def _visualize_trajectory( + self, + plan: JointState, + ee_positions: np.ndarray | None = None, + ) -> None: + """Visualize the robot trajectory. + + Args: + plan: Joint state trajectory + ee_positions: Optional end-effector positions + """ + if ee_positions is None: + raw = plan.position.detach().cpu().numpy() if torch.is_tensor(plan.position) else np.array(plan.position) + if raw.shape[1] >= 3: + positions = raw[:, :3] + else: + raise ValueError("ee_positions not provided and joint positions are not 3-D") + else: + positions = ee_positions + + # Apply translation offset + positions = positions + self._base_translation + + # Log trajectory points + rr.log( + "world/trajectory", + rr.LineStrips3D( + [positions], # single strip consisting of all waypoints + colors=[[0, 100, 255]], # Blue + radii=[0.005], + ), + static=True, + ) + + # Log keyframes + for i, pos in enumerate(positions): + rr.log( + f"world/trajectory/keyframe_{i}", + rr.Points3D( + positions=np.array([pos]), + colors=[[0, 100, 255]], # Blue + radii=[0.01], + ), + static=True, + ) + + def _visualize_robot_spheres(self, spheres: list[Any]) -> None: + """Visualize robot collision spheres. + + Args: + spheres: List of robot collision spheres + """ + self._log_spheres( + spheres=spheres, + entity_type="robot", + color=[0, 255, 100, 128], # Semi-transparent green + apply_offset=True, + ) + + def _visualize_attached_spheres(self, spheres: list[Any]) -> None: + """Visualize attached object collision spheres. + + Args: + spheres: List of attached object spheres + """ + self._log_spheres( + spheres=spheres, + entity_type="attached", + color=[255, 0, 0, 128], # Semi-transparent red + apply_offset=False, + ) + + def _clear_attached_spheres(self) -> None: + """Clear all attached object spheres.""" + for entity_id in self._sphere_entities.get("attached", []): + rr.log(f"world/attached/{entity_id}", rr.Clear(recursive=True)) + self._sphere_entities["attached"] = [] + + # --------------------------------------------------------------------- + # PRIVATE UTILITIES + # --------------------------------------------------------------------- + + def _log_spheres( + self, + spheres: list[Any], + entity_type: str, + color: list[int], + apply_offset: bool = False, + ) -> None: + """Generic helper for sphere visualization. + + Args: + spheres: List of CuRobo ``Sphere`` objects. + entity_type: Log path prefix (``robot`` or ``attached``). + color: RGBA color for the spheres. + apply_offset: Whether to add ``self._base_translation`` to sphere positions. + """ + for i, sphere in enumerate(spheres): + entity_id = f"sphere_{i}" + # Track entities so we can clear them later + self._sphere_entities.setdefault(entity_type, []).append(entity_id) + + # Convert position to numpy and optionally apply offset + pos = ( + sphere.position.detach().cpu().numpy() + if torch.is_tensor(sphere.position) + else np.array(sphere.position) + ) + if apply_offset: + pos = pos + self._base_translation + pos = pos.reshape(-1) # Ensure 1-D + + # Log sphere via Rerun + rr.log( + f"world/{entity_type}/{entity_id}", + rr.Points3D(positions=np.array([pos]), colors=[color], radii=[float(sphere.radius)]), + ) + + def _visualize_world_scene(self, scene: "trimesh.Scene") -> None: + """Log world geometry and dynamic transforms each call. + + Geometry is sent once (cached), but transforms are updated every invocation + so objects that move (cubes after randomization) appear at the correct + pose per episode/frame. + """ + import trimesh # local import to avoid hard dependency at top + + def _to_rerun_mesh(mesh: "trimesh.Trimesh") -> "rr.Mesh3D": + # Basic conversion without materials + return rr.Mesh3D( + vertex_positions=mesh.vertices, + triangle_indices=mesh.faces, + vertex_normals=mesh.vertex_normals if mesh.vertex_normals is not None else None, + ) + + if not hasattr(self, "_logged_geometry"): + self._logged_geometry = set() + + for node in scene.graph.nodes_geometry: + tform, geom_key = scene.graph.get(node) + mesh = scene.geometry.get(geom_key) + if mesh is None: + continue + rr_path = f"world/scene/{node.replace('/', '_')}" + + # Always update transform (objects may move between calls) + # NOTE: World scene objects are already in correct world coordinates, no offset needed + rr.log( + rr_path, + rr.Transform3D( + translation=tform[:3, 3], + mat3x3=tform[:3, :3], + axis_length=0.25, + ), + static=False, + ) + + # Geometry: send only once per node to avoid duplicates + if rr_path not in self._logged_geometry: + if isinstance(mesh, trimesh.Trimesh): + rr_mesh = _to_rerun_mesh(mesh) + elif isinstance(mesh, trimesh.PointCloud): + rr_mesh = rr.Points3D(positions=mesh.vertices, colors=mesh.colors) + else: + continue + + rr.log(rr_path, rr_mesh, static=True) + self._logged_geometry.add(rr_path) + + if self.debug: + print(f"Logged/updated {len(scene.graph.nodes_geometry)} world nodes in Rerun") + + def animate_plan( + self, + ee_positions: np.ndarray, + object_positions: dict[str, np.ndarray] | None = None, + timeline: str = "plan", + point_radius: float = 0.01, + ) -> None: + """Animate robot end-effector and (optionally) attached object positions over time using Rerun. + + This helper logs a single 3-D point per timestep so that Rerun can play back the + trajectory on the provided ``timeline``. It is intentionally lightweight and does + not attempt to render the full robot geometryโ€”only key pointsโ€”keeping the data + transfer to the viewer minimal. + + Args: + ee_positions: Array of shape (T, 3) with end-effector world positions. + object_positions: Mapping from object name to an array (T, 3) with that + object's world positions. Each trajectory must be at least as long as + ``ee_positions``; extra entries are ignored. + timeline: Name of the Rerun timeline used for the animation frames. + point_radius: Visual radius (in metres) of the rendered points. + """ + if ee_positions is None or len(ee_positions) == 0: + return + + # Iterate over timesteps and log a frame on the chosen timeline + for idx, pos in enumerate(ee_positions): + # Set time on the chosen timeline (creates it if needed) + rr.set_time(timeline, sequence=idx) + + # Log end-effector marker (needs offset for multi-env) + rr.log( + "world/anim/ee", + rr.Points3D( + positions=np.array([pos + self._base_translation]), + colors=[[0, 100, 255]], # Blue + radii=[point_radius], + ), + ) + + # Optionally log attached object markers + # NOTE: Object positions are already in world coordinates, no offset needed + if object_positions is not None: + for name, traj in object_positions.items(): + if idx >= len(traj): + continue + rr.log( + f"world/anim/{name}", + rr.Points3D( + positions=np.array([traj[idx]]), + colors=[[255, 128, 0]], # Orange + radii=[point_radius], + ), + ) + + def animate_spheres_along_path( + self, + plan: JointState, + robot_spheres_at_start: list[Any] | None = None, + attached_spheres_at_start: list[Any] | None = None, + timeline: str = "sphere_animation", + interpolation_steps: int = 10, + ) -> None: + """Animate robot and attached object spheres along the planned trajectory with smooth interpolation. + + This method creates a dense, interpolated trajectory and computes sphere positions + at many intermediate points to create smooth animation of the robot moving along the path. + + Args: + plan: Joint state trajectory to animate spheres along + robot_spheres_at_start: Initial robot collision spheres (for reference) + attached_spheres_at_start: Initial attached object spheres (for reference) + timeline: Name of the Rerun timeline for the animation + interpolation_steps: Number of interpolated steps between each waypoint pair + """ + if plan is None or len(plan.position) == 0: + if self.debug: + print("No plan available for sphere animation") + return + + if self.debug: + robot_count = len(robot_spheres_at_start) if robot_spheres_at_start else 0 + attached_count = len(attached_spheres_at_start) if attached_spheres_at_start else 0 + print(f"Creating smooth animation for {robot_count} robot spheres and {attached_count} attached spheres") + print( + f"Original plan: {len(plan.position)} waypoints, interpolating with {interpolation_steps} steps between" + " waypoints" + ) + + # We need access to the motion generator to compute spheres at each waypoint + if not hasattr(self, "_motion_gen_ref") or self._motion_gen_ref is None: + if self.debug: + print("Motion generator reference not available for sphere animation") + return + + motion_gen = self._motion_gen_ref + + # Validate motion generator has required attributes + if not hasattr(motion_gen, "kinematics") or motion_gen.kinematics is None: + if self.debug: + print("Motion generator kinematics not available for sphere animation") + return + + # Clear static spheres to avoid visual clutter during animation + self._hide_static_spheres_for_animation() + + # Count robot link spheres (excluding attached objects) for consistent splitting + robot_link_count = 0 + if robot_spheres_at_start: + robot_link_count = len(robot_spheres_at_start) + + # Create interpolated trajectory for smooth animation + interpolated_positions = self._create_interpolated_trajectory(plan, interpolation_steps) + + if self.debug: + print(f"Created interpolated trajectory with {len(interpolated_positions)} total frames") + + # Animate spheres along the interpolated trajectory + for frame_idx, joint_positions in enumerate(interpolated_positions): + # Set time on the animation timeline with consistent timing + rr.set_time(timeline, sequence=frame_idx) + + # Create joint state for this interpolated position + if isinstance(joint_positions, torch.Tensor): + sphere_position = joint_positions + else: + sphere_position = torch.tensor(joint_positions) + + # Ensure tensor is on the right device for CuRobo + if hasattr(motion_gen, "tensor_args") and motion_gen.tensor_args is not None: + sphere_position = motion_gen.tensor_args.to_device(sphere_position) + + # Get spheres at this configuration + try: + sphere_list = motion_gen.kinematics.get_robot_as_spheres(sphere_position)[0] + except Exception as e: + if self.debug: + print(f"Failed to compute spheres for frame {frame_idx}: {e}") + continue + + # Handle sphere_list as either a list or single Sphere object + if hasattr(sphere_list, "__iter__") and not hasattr(sphere_list, "position"): + sphere_items = list(sphere_list) + else: + sphere_items = [sphere_list] + + # Separate robot and attached object spheres with different colors + robot_sphere_positions = [] + robot_sphere_radii = [] + attached_sphere_positions = [] + attached_sphere_radii = [] + + for i, sphere in enumerate(sphere_items): + # Convert position to numpy + pos = ( + sphere.position.detach().cpu().numpy() + if torch.is_tensor(sphere.position) + else np.array(sphere.position) + ) + pos = pos.reshape(-1) + radius = float(sphere.radius) + + if i < robot_link_count: + # Robot sphere - needs base translation offset + robot_sphere_positions.append(pos + self._base_translation) + robot_sphere_radii.append(radius) + else: + # Attached object sphere - already in world coordinates + attached_sphere_positions.append(pos) + attached_sphere_radii.append(radius) + + # Log robot spheres with green color + if robot_sphere_positions: + rr.log( + "world/robot_animation", + rr.Points3D( + positions=np.array(robot_sphere_positions), + colors=[[0, 255, 100, 220]] * len(robot_sphere_positions), # Bright green + radii=robot_sphere_radii, + ), + ) + + # Log attached object spheres with orange color (or clear if no attached objects) + if attached_sphere_positions: + rr.log( + "world/attached_animation", + rr.Points3D( + positions=np.array(attached_sphere_positions), + colors=[[255, 150, 0, 220]] * len(attached_sphere_positions), # Bright orange + radii=attached_sphere_radii, + ), + ) + else: + # Clear attached object spheres when no objects are attached + rr.log("world/attached_animation", rr.Clear(recursive=True)) + + if self.debug: + print( + f"Completed smooth sphere animation with {len(interpolated_positions)} frames on timeline '{timeline}'" + ) + + def _hide_static_spheres_for_animation(self) -> None: + """Hide static sphere visualization during animation to reduce visual clutter.""" + # Clear static robot spheres + for entity_id in self._sphere_entities.get("robot", []): + rr.log(f"world/robot/{entity_id}", rr.Clear(recursive=True)) + + # Clear static attached spheres + for entity_id in self._sphere_entities.get("attached", []): + rr.log(f"world/attached/{entity_id}", rr.Clear(recursive=True)) + + if self.debug: + print("Hidden static spheres for cleaner animation view") + + def _create_interpolated_trajectory(self, plan: JointState, interpolation_steps: int) -> list[torch.Tensor]: + """Create a smooth interpolated trajectory between waypoints. + + Args: + plan: Original joint state trajectory + interpolation_steps: Number of interpolation steps between each waypoint pair + + Returns: + List of interpolated joint positions + """ + if len(plan.position) < 2: + # If only one waypoint, just return it + return [plan.position[0] if isinstance(plan.position[0], torch.Tensor) else torch.tensor(plan.position[0])] # type: ignore + + interpolated_positions = [] + + # Convert plan positions to tensors if needed + waypoints = [] + for i in range(len(plan.position)): + pos = plan.position[i] + if isinstance(pos, torch.Tensor): + waypoints.append(pos) + else: + waypoints.append(torch.tensor(pos)) + + # Interpolate between each pair of consecutive waypoints + for i in range(len(waypoints) - 1): + start_pos = waypoints[i] + end_pos = waypoints[i + 1] + + # Create interpolation steps between start and end + for step in range(interpolation_steps): + alpha = step / interpolation_steps + interpolated_pos = start_pos * (1 - alpha) + end_pos * alpha + interpolated_positions.append(interpolated_pos) + + # Add the final waypoint + interpolated_positions.append(waypoints[-1]) + + return interpolated_positions + + def set_motion_generator_reference(self, motion_gen: Any) -> None: + """Set the motion generator reference for sphere animation. + + Args: + motion_gen: CuRobo motion generator instance + """ + self._motion_gen_ref = motion_gen + + def mark_idle(self) -> None: + """Signal that the planner is idle, clearing animations. + + This method advances the animation timelines and logs empty data to ensure that + no leftover visualizations from the previous plan are shown. It's useful for + creating a clean state between planning episodes. + """ + # Advance plan timeline and emit empty anim so latest frame is blank + rr.set_time("plan", sequence=self._current_frame) + self._current_frame += 1 + empty = np.empty((0, 3), dtype=float) + rr.log("world/anim/ee", rr.Points3D(positions=empty)) + rr.log("world/robot_animation", rr.Points3D(positions=empty)) + rr.log("world/attached_animation", rr.Points3D(positions=empty)) + + # Also advance sphere animation timeline + rr.set_time("sphere_animation", sequence=self._current_frame) + rr.log("world/robot_animation", rr.Points3D(positions=empty)) + rr.log("world/attached_animation", rr.Points3D(positions=empty)) diff --git a/source/isaaclab_mimic/isaaclab_mimic/motion_planners/motion_planner_base.py b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/motion_planner_base.py new file mode 100644 index 000000000000..43bf6b140515 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/motion_planner_base.py @@ -0,0 +1,134 @@ +# Copyright (c) 2024-2026, 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 abc import ABC, abstractmethod +from typing import Any + +import torch + +from isaaclab.assets import Articulation +from isaaclab.envs.manager_based_env import ManagerBasedEnv + + +class MotionPlannerBase(ABC): + """Abstract base class for motion planners. + + This class defines the public interface that all motion planners must implement. + It focuses on the essential functionality that users interact with, while leaving + implementation details to specific planner backends. + + The core workflow is: + 1. Initialize planner with environment and robot + 2. Call update_world_and_plan_motion() to plan to a target + 3. Execute plan using has_next_waypoint() and get_next_waypoint_ee_pose() + + Example: + >>> from isaaclab_mimic.motion_planners.curobo.curobo_planner import CuroboPlanner + >>> from isaaclab_mimic.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg + >>> config = CuroboPlannerCfg.franka_config() + >>> planner = CuroboPlanner(env, robot, config) + >>> success = planner.update_world_and_plan_motion(target_pose) + >>> if success: + >>> while planner.has_next_waypoint(): + >>> action = planner.get_next_waypoint_ee_pose() + >>> obs, info = env.step(action) + """ + + def __init__( + self, env: ManagerBasedEnv, robot: Articulation, env_id: int = 0, debug: bool = False, **kwargs + ) -> None: + """Initialize the motion planner. + + Args: + env: The environment instance + robot: Robot articulation to plan motions for + env_id: Environment ID (0 to num_envs-1) + debug: Whether to print detailed debugging information + **kwargs: Additional planner-specific arguments + """ + self.env = env + self.robot = robot + self.env_id = env_id + self.debug = debug + + @abstractmethod + def update_world_and_plan_motion(self, target_pose: torch.Tensor, **kwargs: Any) -> bool: + """Update collision world and plan motion to target pose. + + This is the main entry point for motion planning. It should: + 1. Update the planner's internal world representation + 2. Plan a collision-free path to the target pose + 3. Store the plan internally for execution + + Args: + target_pose: Target pose to plan motion to (4x4 transformation matrix) + **kwargs: Planner-specific arguments (e.g., retiming, contact planning) + + Returns: + bool: True if planning succeeded, False otherwise + """ + raise NotImplementedError + + @abstractmethod + def has_next_waypoint(self) -> bool: + """Check if there are more waypoints in current plan. + + Returns: + bool: True if there are more waypoints, False otherwise + """ + raise NotImplementedError + + @abstractmethod + def get_next_waypoint_ee_pose(self) -> Any: + """Get next waypoint's end-effector pose from current plan. + + This method should only be called after checking has_next_waypoint(). + + Returns: + Any: End-effector pose for the next waypoint in the plan. + """ + raise NotImplementedError + + def get_planned_poses(self) -> list[Any]: + """Get all planned poses from current plan. + + Returns: + list[Any]: List of planned poses. + + Note: + Default implementation iterates through waypoints. + Child classes can override for a more efficient implementation. + """ + planned_poses = [] + # Create a copy of the planner state to not affect the original plan execution + # This is a placeholder and may need to be implemented by child classes + # if they manage complex internal state. + # For now, we assume the planner can be reset and we can iterate through the plan. + # A more robust solution might involve a dedicated method to get the full plan. + self.reset_plan() + while self.has_next_waypoint(): + pose = self.get_next_waypoint_ee_pose() + planned_poses.append(pose) + return planned_poses + + @abstractmethod + def reset_plan(self) -> None: + """Reset the current plan and execution state. + + This should clear any stored plan and reset the execution index or iterator. + """ + raise NotImplementedError + + def get_planner_info(self) -> dict[str, Any]: + """Get information about the planner. + + Returns: + dict: Information about the planner (name, version, capabilities, etc.) + """ + return { + "name": self.__class__.__name__, + "env_id": self.env_id, + "debug": self.debug, + } diff --git a/source/isaaclab_mimic/isaaclab_mimic/ui/instruction_display.py b/source/isaaclab_mimic/isaaclab_mimic/ui/instruction_display.py new file mode 100644 index 000000000000..274038d3d9eb --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/ui/instruction_display.py @@ -0,0 +1,99 @@ +# Copyright (c) 2024-2026, 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 (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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, xr: bool): + self.xr = xr + + if self.xr: + 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/source/isaaclab_mimic/setup.py b/source/isaaclab_mimic/setup.py index 8f03274b39fa..c43d268fe260 100644 --- a/source/isaaclab_mimic/setup.py +++ b/source/isaaclab_mimic/setup.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -8,8 +8,8 @@ import itertools import os import platform -import toml +import toml from setuptools import setup # Obtain the extension data from the extension.toml file @@ -29,7 +29,7 @@ # Check if the platform is Linux and add the dependency if platform.system() == "Linux": - EXTRAS_REQUIRE["robomimic"].append("robomimic@git+https://github.com/ARISE-Initiative/robomimic.git") + EXTRAS_REQUIRE["robomimic"].append("robomimic@git+https://github.com/ARISE-Initiative/robomimic.git@v0.4.0") # Cumulation of all extra-requires EXTRAS_REQUIRE["all"] = list(itertools.chain.from_iterable(EXTRAS_REQUIRE.values())) @@ -48,14 +48,16 @@ keywords=EXTENSION_TOML_DATA["package"]["keywords"], install_requires=INSTALL_REQUIRES, extras_require=EXTRAS_REQUIRE, - license="MIT", + license="Apache-2.0", include_package_data=True, python_requires=">=3.10", classifiers=[ "Natural Language :: English", "Programming Language :: Python :: 3.10", - "Isaac Sim :: 2023.1.1", - "Isaac Sim :: 4.0.0", + "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/test/test_curobo_planner_cube_stack.py b/source/isaaclab_mimic/test/test_curobo_planner_cube_stack.py new file mode 100644 index 000000000000..4c532f62ef62 --- /dev/null +++ b/source/isaaclab_mimic/test/test_curobo_planner_cube_stack.py @@ -0,0 +1,249 @@ +# Copyright (c) 2024-2026, 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 __future__ import annotations + +import random +from typing import Any + +import pytest + +SEED: int = 42 +random.seed(SEED) + +from isaaclab.app import AppLauncher + +headless = True +app_launcher = AppLauncher(headless=headless) +simulation_app: Any = app_launcher.app + +from collections.abc import Generator + +import gymnasium as gym +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.envs.manager_based_env import ManagerBasedEnv +from isaaclab.markers import FRAME_MARKER_CFG, VisualizationMarkers + +from isaaclab_mimic.envs.franka_stack_ik_rel_mimic_env_cfg import FrankaCubeStackIKRelMimicEnvCfg +from isaaclab_mimic.motion_planners.curobo.curobo_planner import CuroboPlanner +from isaaclab_mimic.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg + +GRIPPER_OPEN_CMD: float = 1.0 +GRIPPER_CLOSE_CMD: float = -1.0 + + +def _eef_name(env: ManagerBasedEnv) -> str: + return list(env.cfg.subtask_configs.keys())[0] + + +def _action_from_pose( + env: ManagerBasedEnv, target_pose: torch.Tensor, gripper_binary_action: float, env_id: int = 0 +) -> torch.Tensor: + eef = _eef_name(env) + play_action = env.target_eef_pose_to_action( + target_eef_pose_dict={eef: target_pose}, + gripper_action_dict={eef: torch.tensor([gripper_binary_action], device=env.device, dtype=torch.float32)}, + env_id=env_id, + ) + if play_action.dim() == 1: + play_action = play_action.unsqueeze(0) + return play_action + + +def _env_step_with_action(env: ManagerBasedEnv, action: torch.Tensor) -> None: + env.step(action) + + +def _execute_plan(env: ManagerBasedEnv, planner: CuroboPlanner, gripper_binary_action: float, env_id: int = 0) -> None: + """Execute planner's EEF planned poses using env.step with IK-relative controller actions.""" + planned_poses = planner.get_planned_poses() + if not planned_poses: + return + for pose in planned_poses: + action = _action_from_pose(env, pose, gripper_binary_action, env_id=env_id) + _env_step_with_action(env, action) + + +def _execute_gripper_action( + env: ManagerBasedEnv, robot: Articulation, gripper_binary_action: float, steps: int = 12, env_id: int = 0 +) -> None: + """Hold current EEF pose and toggle gripper for a few steps.""" + eef = _eef_name(env) + curr_pose = env.get_robot_eef_pose(eef_name=eef, env_ids=[env_id])[0] + for _ in range(steps): + action = _action_from_pose(env, curr_pose, gripper_binary_action, env_id=env_id) + _env_step_with_action(env, action) + + +DOWN_FACING_QUAT = torch.tensor([0.0, 1.0, 0.0, 0.0], dtype=torch.float32) + + +@pytest.fixture(scope="class") +def cube_stack_test_env() -> Generator[dict[str, Any], None, None]: + """Create the environment and motion planner once for the test suite and yield them.""" + random.seed(SEED) + torch.manual_seed(SEED) + + env_cfg = FrankaCubeStackIKRelMimicEnvCfg() + env_cfg.scene.num_envs = 1 + for frame in env_cfg.scene.ee_frame.target_frames: + if frame.name == "end_effector": + print(f"Setting end effector offset from {frame.offset.pos} to (0.0, 0.0, 0.0) for SkillGen parity") + frame.offset.pos = (0.0, 0.0, 0.0) + + env: ManagerBasedEnv = gym.make( + "Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0", + cfg=env_cfg, + headless=headless, + ).unwrapped + env.reset() + + robot: Articulation = env.scene["robot"] + planner_cfg = CuroboPlannerCfg.franka_stack_cube_config() + planner_cfg.visualize_plan = False + planner_cfg.visualize_spheres = False + planner_cfg.debug_planner = True + planner_cfg.retreat_distance = 0.05 + planner_cfg.approach_distance = 0.05 + planner_cfg.time_dilation_factor = 1.0 + + planner = CuroboPlanner( + env=env, + robot=robot, + config=planner_cfg, + env_id=0, + ) + + goal_pose_visualizer = None + if not headless: + marker_cfg = FRAME_MARKER_CFG.replace(prim_path="/World/Visuals/goal_pose") + marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + goal_pose_visualizer = VisualizationMarkers(marker_cfg) + + yield { + "env": env, + "robot": robot, + "planner": planner, + "goal_pose_visualizer": goal_pose_visualizer, + } + + env.close() + + +class TestCubeStackPlanner: + @pytest.fixture(autouse=True) + def setup(self, cube_stack_test_env) -> None: + self.env: ManagerBasedEnv = cube_stack_test_env["env"] + self.robot: Articulation = cube_stack_test_env["robot"] + self.planner: CuroboPlanner = cube_stack_test_env["planner"] + self.goal_pose_visualizer: VisualizationMarkers | None = cube_stack_test_env["goal_pose_visualizer"] + + def _visualize_goal_pose(self, pos: torch.Tensor, quat: torch.Tensor) -> None: + """Visualize the goal frame markers at pos, quat (xyzw).""" + if headless or self.goal_pose_visualizer is None: + return + self.goal_pose_visualizer.visualize(translations=pos.unsqueeze(0), orientations=quat.unsqueeze(0)) + + def _pose_from_xy_quat(self, xy: torch.Tensor, z: float, quat: torch.Tensor) -> torch.Tensor: + """Build a 4ร—4 pose given xy (Tensor[2]), z, and quaternion.""" + device = xy.device + dtype = xy.dtype + pos = torch.cat([xy, torch.tensor([z], dtype=dtype, device=device)]) + rot = math_utils.matrix_from_quat(quat.to(device).unsqueeze(0))[0] + return math_utils.make_pose(pos, rot) + + def _get_cube_pos(self, cube_name: str) -> torch.Tensor: + """Return the current world position of a cube's root (x, y, z).""" + obj: RigidObject = self.env.scene[cube_name] + return obj.data.root_pos_w[0, :3].clone().detach() + + def _place_pose_over_cube(self, cube_name: str, height_offset: float) -> torch.Tensor: + """Compute a goal pose directly above the named cube using the latest pose.""" + base_pos = self._get_cube_pos(cube_name) + return self._pose_from_xy_quat(base_pos[:2], base_pos[2].item() + height_offset, DOWN_FACING_QUAT) + + def test_pick_and_stack(self) -> None: + """Plan and execute pick-and-place to stack cube_1 on cube_2, then cube_3 on the stack.""" + cube_1_pos = self._get_cube_pos("cube_1") + cube_2_pos = self._get_cube_pos("cube_2") + cube_3_pos = self._get_cube_pos("cube_3") + print(f"Cube 1 position: {cube_1_pos}") + print(f"Cube 2 position: {cube_2_pos}") + print(f"Cube 3 position: {cube_3_pos}") + + # Approach above cube_1 + pre_grasp_height = 0.1 + pre_grasp_pose = self._pose_from_xy_quat(cube_1_pos[:2], pre_grasp_height, DOWN_FACING_QUAT) + print(f"Pre-grasp pose: {pre_grasp_pose}") + if not headless: + pos_pg = pre_grasp_pose[:3, 3].detach().cpu() + quat_pg = math_utils.quat_from_matrix(pre_grasp_pose[:3, :3].unsqueeze(0))[0].detach().cpu() + self._visualize_goal_pose(pos_pg, quat_pg) + + # Plan to pre-grasp + assert self.planner.update_world_and_plan_motion(pre_grasp_pose), "Failed to plan to pre-grasp pose" + _execute_plan(self.env, self.planner, gripper_binary_action=GRIPPER_OPEN_CMD) + + # Close gripper to grasp cube_1 (hold pose while closing) + _execute_gripper_action(self.env, self.robot, GRIPPER_CLOSE_CMD, steps=16) + + # Plan placement with cube_1 attached (above latest cube_2) + place_pose = self._place_pose_over_cube("cube_2", 0.15) + + if not headless: + pos_place = place_pose[:3, 3].detach().cpu() + quat_place = math_utils.quat_from_matrix(place_pose[:3, :3].unsqueeze(0))[0].detach().cpu() + self._visualize_goal_pose(pos_place, quat_place) + + # Plan with attached object + assert self.planner.update_world_and_plan_motion(place_pose, expected_attached_object="cube_1"), ( + "Failed to plan placement trajectory with attached cube" + ) + _execute_plan(self.env, self.planner, gripper_binary_action=GRIPPER_CLOSE_CMD) + + # Release cube 1 + _execute_gripper_action(self.env, self.robot, GRIPPER_OPEN_CMD, steps=16) + + # Go to cube 3 + cube_3_pos_now = self._get_cube_pos("cube_3") + pre_grasp_pose = self._pose_from_xy_quat(cube_3_pos_now[:2], pre_grasp_height, DOWN_FACING_QUAT) + print(f"Pre-grasp pose: {pre_grasp_pose}") + if not headless: + pos_pg = pre_grasp_pose[:3, 3].detach().cpu() + quat_pg = math_utils.quat_from_matrix(pre_grasp_pose[:3, :3].unsqueeze(0))[0].detach().cpu() + self._visualize_goal_pose(pos_pg, quat_pg) + + assert self.planner.update_world_and_plan_motion(pre_grasp_pose, expected_attached_object=None), ( + "Failed to plan retract motion" + ) + _execute_plan(self.env, self.planner, gripper_binary_action=GRIPPER_OPEN_CMD) + + # Grasp cube 3 + _execute_gripper_action(self.env, self.robot, GRIPPER_CLOSE_CMD) + + # Plan placement with cube_3 attached, to cube 2 (use latest cube_2 pose) + place_pose = self._place_pose_over_cube("cube_2", 0.18) + + if not headless: + pos_place = place_pose[:3, 3].detach().cpu() + quat_place = math_utils.quat_from_matrix(place_pose[:3, :3].unsqueeze(0))[0].detach().cpu() + self._visualize_goal_pose(pos_place, quat_place) + + assert self.planner.update_world_and_plan_motion(place_pose, expected_attached_object="cube_3"), ( + "Failed to plan placement trajectory with attached cube" + ) + _execute_plan(self.env, self.planner, gripper_binary_action=GRIPPER_CLOSE_CMD) + + # Release cube 3 + _execute_gripper_action(self.env, self.robot, GRIPPER_OPEN_CMD) + + print("Pick-and-place stacking test completed successfully!") diff --git a/source/isaaclab_mimic/test/test_curobo_planner_franka.py b/source/isaaclab_mimic/test/test_curobo_planner_franka.py new file mode 100644 index 000000000000..9e5adf724c2f --- /dev/null +++ b/source/isaaclab_mimic/test/test_curobo_planner_franka.py @@ -0,0 +1,173 @@ +# Copyright (c) 2024-2026, 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 random +from collections.abc import Generator +from typing import Any + +import pytest + +SEED: int = 42 +random.seed(SEED) + +from isaaclab.app import AppLauncher + +headless = True +app_launcher = AppLauncher(headless=headless) +simulation_app: Any = app_launcher.app + +import gymnasium as gym +import torch + +import isaaclab.utils.assets as _al_assets +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObjectCfg +from isaaclab.envs.manager_based_env import ManagerBasedEnv +from isaaclab.markers import FRAME_MARKER_CFG, VisualizationMarkers +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg + +ISAAC_NUCLEUS_DIR: str = getattr(_al_assets, "ISAAC_NUCLEUS_DIR", "/Isaac") + +from isaaclab_mimic.motion_planners.curobo.curobo_planner import CuroboPlanner +from isaaclab_mimic.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg + +from isaaclab_tasks.manager_based.manipulation.stack.config.franka.stack_joint_pos_env_cfg import FrankaCubeStackEnvCfg + +# Predefined EE goals for the test +# Each entry is a tuple of: (goal specification, goal ID) +predefined_ee_goals_and_ids = [ + ({"pos": [0.70, -0.25, 0.25], "quat": [0.0, 0.707, 0.0, 0.707]}, "Behind wall, left"), + ({"pos": [0.70, 0.25, 0.25], "quat": [0.0, 0.707, 0.0, 0.707]}, "Behind wall, right"), + ({"pos": [0.65, 0.0, 0.45], "quat": [0.0, 1.0, 0.0, 0.0]}, "Behind wall, center, high"), + ({"pos": [0.80, -0.15, 0.35], "quat": [0.0, 0.5, 0.0, 0.866]}, "Behind wall, far left"), + ({"pos": [0.80, 0.15, 0.35], "quat": [0.0, 0.5, 0.0, 0.866]}, "Behind wall, far right"), +] + + +@pytest.fixture(scope="class") +def curobo_test_env() -> Generator[dict[str, Any], None, None]: + """Set up the environment for the Curobo test and yield test-critical data.""" + random.seed(SEED) + torch.manual_seed(SEED) + + env_cfg = FrankaCubeStackEnvCfg() + env_cfg.scene.num_envs = 1 + + # Add a static wall for the robot to avoid + wall_props = RigidBodyPropertiesCfg(kinematic_enabled=True, disable_gravity=True) + wall_cfg = RigidObjectCfg( + prim_path="/World/envs/env_0/moving_wall", + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/red_block.usd", + scale=(0.5, 4.5, 7.0), + rigid_props=wall_props, + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.55, 0.0, 0.80)), + ) + setattr(env_cfg.scene, "moving_wall", wall_cfg) + + env: ManagerBasedEnv = gym.make("Isaac-Stack-Cube-Franka-v0", cfg=env_cfg, headless=headless).unwrapped + env.reset() + + robot = env.scene["robot"] + planner = CuroboPlanner(env=env, robot=robot, config=CuroboPlannerCfg.franka_config()) + + goal_pose_visualizer = None + if not headless: + goal_marker_cfg = FRAME_MARKER_CFG.replace(prim_path="/World/Visuals/goal_poses") + goal_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + goal_pose_visualizer = VisualizationMarkers(goal_marker_cfg) + + # Allow the simulation to settle + for _ in range(3): + env.sim.step(render=False) + + planner.update_world() + + # Default joint positions for the Franka arm (7-DOF) + home_j = torch.tensor([0.0, -0.4, 0.0, -2.1, 0.0, 2.1, 0.7]) + + # Yield the necessary objects for the test + yield { + "env": env, + "robot": robot, + "planner": planner, + "goal_pose_visualizer": goal_pose_visualizer, + "home_j": home_j, + } + + # Teardown: close the environment and simulation app + env.close() + + +class TestCuroboPlanner: + """Test suite for the Curobo motion planner, focusing on obstacle avoidance.""" + + @pytest.fixture(autouse=True) + def setup(self, curobo_test_env) -> None: + """Inject the test environment into the test class instance.""" + self.env: ManagerBasedEnv = curobo_test_env["env"] + self.robot: Articulation = curobo_test_env["robot"] + self.planner: CuroboPlanner = curobo_test_env["planner"] + self.goal_pose_visualizer: VisualizationMarkers | None = curobo_test_env["goal_pose_visualizer"] + self.home_j: torch.Tensor = curobo_test_env["home_j"] + + def _visualize_goal_pose(self, pos: torch.Tensor, quat: torch.Tensor) -> None: + """Visualize the goal pose using frame markers if not in headless mode.""" + if headless or self.goal_pose_visualizer is None: + return + pos_vis = pos.unsqueeze(0) + quat_vis = quat.unsqueeze(0) + self.goal_pose_visualizer.visualize(translations=pos_vis, orientations=quat_vis) + + def _execute_current_plan(self) -> None: + """Replay the waypoints of the current plan in the simulator for visualization.""" + if headless or self.planner.current_plan is None: + return + for q in self.planner.current_plan.position: + q_tensor = q if isinstance(q, torch.Tensor) else torch.as_tensor(q, dtype=torch.float32) + self._set_arm_positions(q_tensor) + self.env.sim.step(render=True) + + def _set_arm_positions(self, q: torch.Tensor) -> None: + """Set the joint positions of the robot's arm, appending default gripper values if necessary.""" + if q.dim() == 1: + q = q.unsqueeze(0) + if q.shape[-1] == 7: # Arm only + fingers = torch.tensor([0.04, 0.04], device=q.device, dtype=q.dtype).repeat(q.shape[0], 1) + q_full = torch.cat([q, fingers], dim=-1) + else: + q_full = q + self.robot.write_joint_position_to_sim(q_full) + + @pytest.mark.parametrize("goal_spec, goal_id", predefined_ee_goals_and_ids) + def test_plan_to_predefined_goal(self, goal_spec, goal_id) -> None: + """Test planning to a predefined goal, ensuring the planner can find a path around an obstacle.""" + print(f"Planning for goal: {goal_id}") + + # Reset robot to a known home position before each test + self._set_arm_positions(self.home_j) + self.env.sim.step() + + pos = torch.tensor(goal_spec["pos"], dtype=torch.float32) + quat = torch.tensor(goal_spec["quat"], dtype=torch.float32) + + if not headless: + self._visualize_goal_pose(pos, quat) + + # Ensure the goal is actually behind the wall + assert pos[0] > 0.55, f"Goal '{goal_id}' is not behind the wall (x={pos[0]:.3f})" + + rot_matrix = math_utils.matrix_from_quat(quat.unsqueeze(0))[0] + ee_goal = math_utils.make_pose(pos, rot_matrix) + + result = self.planner.plan_motion(ee_goal) + print(f"Planning result for '{goal_id}': {'Success' if result else 'Failure'}") + + assert result, f"Failed to find a motion plan for the goal: '{goal_id}'" + + if result and not headless: + self._execute_current_plan() diff --git a/source/isaaclab_mimic/test/test_generate_dataset.py b/source/isaaclab_mimic/test/test_generate_dataset.py index e6dc82c8e5a0..8568ab4ec01d 100644 --- a/source/isaaclab_mimic/test/test_generate_dataset.py +++ b/source/isaaclab_mimic/test/test_generate_dataset.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -13,114 +13,130 @@ import os import subprocess import tempfile -import unittest + +import pytest from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path DATASETS_DOWNLOAD_DIR = tempfile.mkdtemp(suffix="_Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0") NUCLEUS_DATASET_PATH = os.path.join(ISAACLAB_NUCLEUS_DIR, "Tests", "Mimic", "dataset.hdf5") - - -class TestGenerateDataset(unittest.TestCase): - """Test the dataset generation behavior of the Isaac Lab Mimic workflow.""" - - def setUp(self): - """Set up the environment for testing.""" - # Create the datasets directory if it does not exist - if not os.path.exists(DATASETS_DOWNLOAD_DIR): - print("Creating directory : ", DATASETS_DOWNLOAD_DIR) - os.makedirs(DATASETS_DOWNLOAD_DIR) - # Try to download the dataset from Nucleus - try: - retrieve_file_path(NUCLEUS_DATASET_PATH, DATASETS_DOWNLOAD_DIR) - except Exception as e: - print(e) - print("Could not download dataset from Nucleus") - self.fail( - "The dataset required for this test is currently unavailable. Dataset path: " + NUCLEUS_DATASET_PATH - ) - - # Set the environment variable PYTHONUNBUFFERED to 1 to get all text outputs in result.stdout - self.pythonunbuffered_env_var_ = os.environ.get("PYTHONUNBUFFERED") - os.environ["PYTHONUNBUFFERED"] = "1" - - # Automatically detect the workflow root (backtrack from current file location) - current_dir = os.path.dirname(os.path.abspath(__file__)) - workflow_root = os.path.abspath(os.path.join(current_dir, "../../..")) - - # Run the command to generate core configs - config_command = [ - workflow_root + "/isaaclab.sh", - "-p", - os.path.join(workflow_root, "scripts/imitation_learning/isaaclab_mimic/annotate_demos.py"), - "--task", - "Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0", - "--input_file", - DATASETS_DOWNLOAD_DIR + "/dataset.hdf5", - "--output_file", - DATASETS_DOWNLOAD_DIR + "/annotated_dataset.hdf5", - "--signals", - "grasp_1", - "stack_1", - "grasp_2", - "--auto", - "--headless", - ] - print(config_command) - - # Execute the command and capture the result - result = subprocess.run(config_command, capture_output=True, text=True) - - # Print the result for debugging purposes - print("Config generation result:") - print(result.stdout) # Print standard output from the command - print(result.stderr) # Print standard error from the command - - # Check if the config generation was successful - self.assertEqual(result.returncode, 0, msg=result.stderr) - - def tearDown(self): - """Clean up after tests.""" - if self.pythonunbuffered_env_var_: - os.environ["PYTHONUNBUFFERED"] = self.pythonunbuffered_env_var_ - else: - del os.environ["PYTHONUNBUFFERED"] - - def test_generate_dataset(self): - """Test the dataset generation script.""" - # Automatically detect the workflow root (backtrack from current file location) - current_dir = os.path.dirname(os.path.abspath(__file__)) - workflow_root = os.path.abspath(os.path.join(current_dir, "../../..")) - - # Define the command to run the dataset generation script - command = [ - workflow_root + "/isaaclab.sh", - "-p", - os.path.join(workflow_root, "scripts/imitation_learning/isaaclab_mimic/generate_dataset.py"), - "--input_file", - DATASETS_DOWNLOAD_DIR + "/annotated_dataset.hdf5", - "--output_file", - DATASETS_DOWNLOAD_DIR + "/generated_dataset.hdf5", - "--generation_num_trials", - "1", - "--headless", - ] - - # Call the script and capture output - result = subprocess.run(command, capture_output=True, text=True) - - # Print the result for debugging purposes - print("Dataset generation result:") - print(result.stdout) # Print standard output from the command - print(result.stderr) # Print standard error from the command - - # Check if the script executed successfully - self.assertEqual(result.returncode, 0, msg=result.stderr) - - # Check for specific output - expected_output = "successes/attempts. Exiting" - self.assertIn(expected_output, result.stdout) - - -if __name__ == "__main__": - unittest.main() +EXPECTED_SUCCESSFUL_ANNOTATIONS = 10 + + +@pytest.fixture +def setup_test_environment(): + """Set up the environment for testing.""" + # Create the datasets directory if it does not exist + if not os.path.exists(DATASETS_DOWNLOAD_DIR): + print("Creating directory : ", DATASETS_DOWNLOAD_DIR) + os.makedirs(DATASETS_DOWNLOAD_DIR) + + # Try to download the dataset from Nucleus + try: + retrieve_file_path(NUCLEUS_DATASET_PATH, DATASETS_DOWNLOAD_DIR) + except Exception as e: + print(e) + print("Could not download dataset from Nucleus") + pytest.fail( + "The dataset required for this test is currently unavailable. Dataset path: " + NUCLEUS_DATASET_PATH + ) + + # Set the environment variable PYTHONUNBUFFERED to 1 to get all text outputs in result.stdout + pythonunbuffered_env_var_ = os.environ.get("PYTHONUNBUFFERED") + os.environ["PYTHONUNBUFFERED"] = "1" + + # Automatically detect the workflow root (backtrack from current file location) + current_dir = os.path.dirname(os.path.abspath(__file__)) + workflow_root = os.path.abspath(os.path.join(current_dir, "../../..")) + + # Run the command to generate core configs + config_command = [ + workflow_root + "/isaaclab.sh", + "-p", + os.path.join(workflow_root, "scripts/imitation_learning/isaaclab_mimic/annotate_demos.py"), + "--task", + "Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0", + "--input_file", + DATASETS_DOWNLOAD_DIR + "/dataset.hdf5", + "--output_file", + DATASETS_DOWNLOAD_DIR + "/annotated_dataset.hdf5", + "--auto", + "--headless", + ] + print(config_command) + + # Execute the command and capture the result + result = subprocess.run(config_command, capture_output=True, text=True) + + print(f"Annotate demos result: {result.returncode}\n\n\n\n\n\n\n\n\n\n\n\n") + + # Print the result for debugging purposes + print("Config generation result:") + print(result.stdout) # Print standard output from the command + print(result.stderr) # Print standard error from the command + + # Check if the config generation was successful + assert result.returncode == 0, result.stderr + + # Check that at least one task was completed successfully by parsing stdout + # Look for the line that reports successful task completions + success_line = None + for line in result.stdout.split("\n"): + if "Successful task completions:" in line: + success_line = line + break + + assert success_line is not None, "Could not find 'Successful task completions:' in output" + + # Extract the number from the line + try: + successful_count = int(success_line.split(":")[-1].strip()) + assert successful_count == EXPECTED_SUCCESSFUL_ANNOTATIONS, ( + f"Expected 10 successful annotations but got {successful_count}" + ) + except (ValueError, IndexError) as e: + pytest.fail(f"Could not parse successful task count from line: '{success_line}'. Error: {e}") + + # Yield the workflow root for use in tests + yield workflow_root + + # Cleanup: restore the original environment variable + if pythonunbuffered_env_var_: + os.environ["PYTHONUNBUFFERED"] = pythonunbuffered_env_var_ + else: + del os.environ["PYTHONUNBUFFERED"] + + +@pytest.mark.isaacsim_ci +def test_generate_dataset(setup_test_environment): + """Test the dataset generation script.""" + workflow_root = setup_test_environment + + # Define the command to run the dataset generation script + command = [ + workflow_root + "/isaaclab.sh", + "-p", + os.path.join(workflow_root, "scripts/imitation_learning/isaaclab_mimic/generate_dataset.py"), + "--input_file", + DATASETS_DOWNLOAD_DIR + "/annotated_dataset.hdf5", + "--output_file", + DATASETS_DOWNLOAD_DIR + "/generated_dataset.hdf5", + "--generation_num_trials", + "1", + "--headless", + ] + + # Call the script and capture output + result = subprocess.run(command, capture_output=True, text=True) + + # Print the result for debugging purposes + print("Dataset generation result:") + print(result.stdout) # Print standard output from the command + print(result.stderr) # Print standard error from the command + + # Check if the script executed successfully + assert result.returncode == 0, result.stderr + + # Check for specific output + expected_output = "successes/attempts. Exiting" + assert expected_output in result.stdout diff --git a/source/isaaclab_mimic/test/test_generate_dataset_skillgen.py b/source/isaaclab_mimic/test/test_generate_dataset_skillgen.py new file mode 100644 index 000000000000..7f5afc7d664c --- /dev/null +++ b/source/isaaclab_mimic/test/test_generate_dataset_skillgen.py @@ -0,0 +1,91 @@ +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test dataset generation with SkillGen for Isaac Lab Mimic workflow.""" + +from isaaclab.app import AppLauncher + +# Launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import os +import subprocess +import tempfile + +import pytest + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +DATASETS_DOWNLOAD_DIR = tempfile.mkdtemp(suffix="_Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0") +NUCLEUS_SKILLGEN_ANNOTATED_DATASET_PATH = os.path.join( + ISAACLAB_NUCLEUS_DIR, "Mimic", "franka_stack_datasets", "annotated_dataset_skillgen.hdf5" +) + + +@pytest.fixture +def setup_skillgen_test_environment(): + """Prepare environment for SkillGen dataset generation test.""" + # Create the datasets directory if it does not exist + if not os.path.exists(DATASETS_DOWNLOAD_DIR): + print("Creating directory : ", DATASETS_DOWNLOAD_DIR) + os.makedirs(DATASETS_DOWNLOAD_DIR) + + # Download the SkillGen annotated dataset from Nucleus into DATASETS_DOWNLOAD_DIR + retrieve_file_path(NUCLEUS_SKILLGEN_ANNOTATED_DATASET_PATH, DATASETS_DOWNLOAD_DIR) + + # Set the environment variable PYTHONUNBUFFERED to 1 to get all text outputs in result.stdout + pythonunbuffered_env_var_ = os.environ.get("PYTHONUNBUFFERED") + os.environ["PYTHONUNBUFFERED"] = "1" + + # Automatically detect the workflow root (backtrack from current file location) + current_dir = os.path.dirname(os.path.abspath(__file__)) + workflow_root = os.path.abspath(os.path.join(current_dir, "../../..")) + + # Yield the workflow root for use in tests + yield workflow_root + + # Cleanup: restore the original environment variable + if pythonunbuffered_env_var_: + os.environ["PYTHONUNBUFFERED"] = pythonunbuffered_env_var_ + else: + del os.environ["PYTHONUNBUFFERED"] + + +def test_generate_dataset_skillgen(setup_skillgen_test_environment): + """Test dataset generation with SkillGen enabled.""" + workflow_root = setup_skillgen_test_environment + + input_file = os.path.join(DATASETS_DOWNLOAD_DIR, "annotated_dataset_skillgen.hdf5") + output_file = os.path.join(DATASETS_DOWNLOAD_DIR, "generated_dataset_skillgen.hdf5") + + command = [ + workflow_root + "/isaaclab.sh", + "-p", + os.path.join(workflow_root, "scripts/imitation_learning/isaaclab_mimic/generate_dataset.py"), + "--device", + "cpu", + "--input_file", + input_file, + "--output_file", + output_file, + "--num_envs", + "1", + "--generation_num_trials", + "1", + "--use_skillgen", + "--headless", + "--task", + "Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0", + ] + + result = subprocess.run(command, capture_output=True, text=True) + + print("SkillGen dataset generation result:") + print(result.stdout) + print(result.stderr) + + assert result.returncode == 0, result.stderr + expected_output = "successes/attempts. Exiting" + assert expected_output in result.stdout diff --git a/source/isaaclab_mimic/test/test_selection_strategy.py b/source/isaaclab_mimic/test/test_selection_strategy.py index d3fbf6906df0..ac58be34db0f 100644 --- a/source/isaaclab_mimic/test/test_selection_strategy.py +++ b/source/isaaclab_mimic/test/test_selection_strategy.py @@ -1,4 +1,4 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# Copyright (c) 2024-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: Apache-2.0 @@ -9,8 +9,8 @@ simulation_app = AppLauncher(headless=True).app import numpy as np +import pytest import torch -import unittest import isaaclab.utils.math as PoseUtils @@ -26,272 +26,253 @@ NUM_ITERS = 1000 -class TestNearestNeighborObjectStrategy(unittest.TestCase): - """Test the NearestNeighborObjectStrategy class.""" - - def setUp(self): - """Set up test cases for the NearestNeighborObjectStrategy.""" - # Initialize the strategy object for selecting nearest neighbors - self.strategy = NearestNeighborObjectStrategy() - - def test_select_source_demo_identity_orientations(self): - """Test the selection of source demonstrations using two distinct object_pose clusters. - - This method generates two clusters of object poses and randomly adjusts the current object pose within - specified deviations. It then simulates multiple selections to verify that when the current pose is close - to cluster 1, all selected indices correspond to that cluster, and that the same holds true for cluster 2. - """ - - # Define ranges for two clusters of object poses - cluster_1_range_min = 0 - cluster_1_range_max = 4 - cluster_2_range_min = 25 - cluster_2_range_max = 35 - - # Generate object poses for cluster 1 with varying translations - src_object_poses_in_world_cluster_1 = [ - torch.eye(4) - + torch.tensor([[0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, -1.0]]) - for i in range(cluster_1_range_min, cluster_1_range_max) - ] - - # Generate object poses for cluster 2 similarly - src_object_poses_in_world_cluster_2 = [ - torch.eye(4) - + torch.tensor([[0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, -1.0]]) - for i in range(cluster_2_range_min, cluster_2_range_max) - ] - - # Combine the poses from both clusters into a single list - src_object_poses_in_world = src_object_poses_in_world_cluster_1 + src_object_poses_in_world_cluster_2 - - # Create DatagenInfo instances for these positions - src_subtask_datagen_infos = [ - DatagenInfo(object_poses={0: object_pose.unsqueeze(0)}) for object_pose in src_object_poses_in_world - ] - - # Define the end-effector pose (not used in the nearest neighbor selection) - eef_pose = torch.eye(4) - - # Test 1: - # Set the current object pose to the first value of cluster 1 and add some noise - # Check that the nearest neighbor is always part of cluster 1 - max_deviation = 3 # Define a maximum deviation for the current pose - # Randomly select an index from cluster 1 - random_index_cluster_1 = np.random.randint(0, len(src_object_poses_in_world_cluster_1)) - cluster_1_curr_object_pose = src_object_poses_in_world_cluster_1[ - random_index_cluster_1 - ].clone() # Use clone to avoid reference issues - # Randomly adjust the current pose within the maximum deviation - cluster_1_curr_object_pose[0, 3] += torch.rand(1).item() * max_deviation - cluster_1_curr_object_pose[1, 3] += torch.rand(1).item() * max_deviation - cluster_1_curr_object_pose[2, 3] += torch.rand(1).item() * max_deviation - - # Select source demonstrations multiple times to check randomness - selected_indices = [ - self.strategy.select_source_demo( - eef_pose, - cluster_1_curr_object_pose, - src_subtask_datagen_infos, - pos_weight=1.0, - rot_weight=1.0, - nn_k=3, # Check among the top 3 nearest neighbors - ) - for _ in range(NUM_ITERS) - ] - - # Assert that all selected indices are valid indices within cluster 1 - self.assertTrue( - np.all(np.array(selected_indices) < len(src_object_poses_in_world_cluster_1)), - "Some selected indices are not part of cluster 1.", - ) - - # Test 2: - # Set the current object pose to the first value of cluster 2 and add some noise - # Check that the nearest neighbor is always part of cluster 2 - max_deviation = 5 # Define a maximum deviation for the current pose in cluster 2 - # Randomly select an index from cluster 2 - random_index_cluster_2 = np.random.randint(0, len(src_object_poses_in_world_cluster_2)) - cluster_2_curr_object_pose = src_object_poses_in_world_cluster_2[ - random_index_cluster_2 - ].clone() # Use clone to avoid reference issues - # Randomly adjust the current pose within the maximum deviation - cluster_2_curr_object_pose[0, 3] += torch.rand(1).item() * max_deviation - cluster_2_curr_object_pose[1, 3] += torch.rand(1).item() * max_deviation - cluster_2_curr_object_pose[2, 3] += torch.rand(1).item() * max_deviation - - # Select source demonstrations multiple times to check randomness - selected_indices = [ - self.strategy.select_source_demo( - eef_pose, - cluster_2_curr_object_pose, - src_subtask_datagen_infos, - pos_weight=1.0, - rot_weight=1.0, - nn_k=6, # Check among the top 6 nearest neighbors - ) - for _ in range(20) - ] - - # Assert that all selected indices are valid indices within cluster 2 - self.assertTrue( - np.all(np.array(selected_indices) < len(src_object_poses_in_world)), - "Some selected indices are not part of cluster 2.", +@pytest.fixture +def nearest_neighbor_object_strategy(): + """Fixture for NearestNeighborObjectStrategy.""" + return NearestNeighborObjectStrategy() + + +@pytest.fixture +def nearest_neighbor_robot_distance_strategy(): + """Fixture for NearestNeighborRobotDistanceStrategy.""" + return NearestNeighborRobotDistanceStrategy() + + +def test_select_source_demo_identity_orientations_object_strategy(nearest_neighbor_object_strategy): + """Test the selection of source demonstrations using two distinct object_pose clusters. + + This method generates two clusters of object poses and randomly adjusts the current object pose within + specified deviations. It then simulates multiple selections to verify that when the current pose is close + to cluster 1, all selected indices correspond to that cluster, and that the same holds true for cluster 2. + """ + + # Define ranges for two clusters of object poses + cluster_1_range_min = 0 + cluster_1_range_max = 4 + cluster_2_range_min = 25 + cluster_2_range_max = 35 + + # Generate object poses for cluster 1 with varying translations + src_object_poses_in_world_cluster_1 = [ + torch.eye(4) + torch.tensor([[0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, -1.0]]) + for i in range(cluster_1_range_min, cluster_1_range_max) + ] + + # Generate object poses for cluster 2 similarly + src_object_poses_in_world_cluster_2 = [ + torch.eye(4) + torch.tensor([[0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, i], [0.0, 0.0, 0.0, -1.0]]) + for i in range(cluster_2_range_min, cluster_2_range_max) + ] + + # Combine the poses from both clusters into a single list + src_object_poses_in_world = src_object_poses_in_world_cluster_1 + src_object_poses_in_world_cluster_2 + + # Create DatagenInfo instances for these positions + src_subtask_datagen_infos = [ + DatagenInfo(object_poses={0: object_pose.unsqueeze(0)}) for object_pose in src_object_poses_in_world + ] + + # Define the end-effector pose (not used in the nearest neighbor selection) + eef_pose = torch.eye(4) + + # Test 1: + # Set the current object pose to the first value of cluster 1 and add some noise + # Check that the nearest neighbor is always part of cluster 1 + max_deviation = 3 # Define a maximum deviation for the current pose + # Randomly select an index from cluster 1 + random_index_cluster_1 = np.random.randint(0, len(src_object_poses_in_world_cluster_1)) + cluster_1_curr_object_pose = src_object_poses_in_world_cluster_1[ + random_index_cluster_1 + ].clone() # Use clone to avoid reference issues + # Randomly adjust the current pose within the maximum deviation + cluster_1_curr_object_pose[0, 3] += torch.rand(1).item() * max_deviation + cluster_1_curr_object_pose[1, 3] += torch.rand(1).item() * max_deviation + cluster_1_curr_object_pose[2, 3] += torch.rand(1).item() * max_deviation + + # Select source demonstrations multiple times to check randomness + selected_indices = [ + nearest_neighbor_object_strategy.select_source_demo( + eef_pose, + cluster_1_curr_object_pose, + src_subtask_datagen_infos, + pos_weight=1.0, + rot_weight=1.0, + nn_k=3, # Check among the top 3 nearest neighbors ) - self.assertTrue( - np.all(np.array(selected_indices) > (len(src_object_poses_in_world_cluster_1) - 1)), - "Some selected indices are not part of cluster 2.", + for _ in range(NUM_ITERS) + ] + + # Assert that all selected indices are valid indices within cluster 1 + assert np.all(np.array(selected_indices) < len(src_object_poses_in_world_cluster_1)), ( + "Some selected indices are not part of cluster 1." + ) + + # Test 2: + # Set the current object pose to the first value of cluster 2 and add some noise + # Check that the nearest neighbor is always part of cluster 2 + max_deviation = 5 # Define a maximum deviation for the current pose in cluster 2 + # Randomly select an index from cluster 2 + random_index_cluster_2 = np.random.randint(0, len(src_object_poses_in_world_cluster_2)) + cluster_2_curr_object_pose = src_object_poses_in_world_cluster_2[ + random_index_cluster_2 + ].clone() # Use clone to avoid reference issues + # Randomly adjust the current pose within the maximum deviation + cluster_2_curr_object_pose[0, 3] += torch.rand(1).item() * max_deviation + cluster_2_curr_object_pose[1, 3] += torch.rand(1).item() * max_deviation + cluster_2_curr_object_pose[2, 3] += torch.rand(1).item() * max_deviation + + # Select source demonstrations multiple times to check randomness + selected_indices = [ + nearest_neighbor_object_strategy.select_source_demo( + eef_pose, + cluster_2_curr_object_pose, + src_subtask_datagen_infos, + pos_weight=1.0, + rot_weight=1.0, + nn_k=6, # Check among the top 6 nearest neighbors ) - - -class TestNearestNeighborRobotDistanceStrategy(unittest.TestCase): - """Test the NearestNeighborRobotDistanceStrategy class.""" - - def setUp(self): - """Set up test cases for the NearestNeighborRobotDistanceStrategy.""" - # Initialize the strategy object for selecting nearest neighbors - self.strategy = NearestNeighborRobotDistanceStrategy() - - def test_select_source_demo_identity_orientations(self): - """Test the selection of source demonstrations based on identity-oriented poses with varying positions. - - This method generates two clusters of object poses and randomly adjusts the current object pose within - specified deviations. It then simulates multiple selections to verify that when the current pose is close - to cluster 1, all selected indices correspond to that cluster, and that the same holds true for cluster 2. - """ - - # Define ranges for two clusters of object poses - cluster_1_range_min = 0 - cluster_1_range_max = 4 - cluster_2_range_min = 25 - cluster_2_range_max = 35 - - # Generate random transformed object poses for cluster 1 with varying translations - # This represents the first object pose for the transformed subtask segment for each source demo - transformed_eef_pose_cluster_1 = [ - torch.eye(4) + torch.tensor([[0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, -1]]) - for i in range(cluster_1_range_min, cluster_1_range_max) - ] - - # Generate object poses for cluster 2 similarly - transformed_eef_pose_cluster_2 = [ - torch.eye(4) + torch.tensor([[0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, -1]]) - for i in range(cluster_2_range_min, cluster_2_range_max) - ] - - # Combine the poses from both clusters into a single list - # This represents the first end effector pose for the transformed subtask segment for each source demo - transformed_eef_in_world_poses_tensor = torch.stack( - transformed_eef_pose_cluster_1 + transformed_eef_pose_cluster_2 - ) - - # Create transformation matrices corresponding to each source object pose - src_obj_in_world_poses = torch.stack([ + for _ in range(20) + ] + + # Assert that all selected indices are valid indices within cluster 2 + assert np.all(np.array(selected_indices) < len(src_object_poses_in_world)), ( + "Some selected indices are not part of cluster 2." + ) + assert np.all(np.array(selected_indices) > (len(src_object_poses_in_world_cluster_1) - 1)), ( + "Some selected indices are not part of cluster 2." + ) + + +def test_select_source_demo_identity_orientations_robot_distance_strategy(nearest_neighbor_robot_distance_strategy): + """Test the selection of source demonstrations based on identity-oriented poses with varying positions. + + This method generates two clusters of object poses and randomly adjusts the current object pose within + specified deviations. It then simulates multiple selections to verify that when the current pose is close + to cluster 1, all selected indices correspond to that cluster, and that the same holds true for cluster 2. + """ + + # Define ranges for two clusters of object poses + cluster_1_range_min = 0 + cluster_1_range_max = 4 + cluster_2_range_min = 25 + cluster_2_range_max = 35 + + # Generate random transformed object poses for cluster 1 with varying translations + # This represents the first object pose for the transformed subtask segment for each source demo + transformed_eef_pose_cluster_1 = [ + torch.eye(4) + torch.tensor([[0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, -1]]) + for i in range(cluster_1_range_min, cluster_1_range_max) + ] + + # Generate object poses for cluster 2 similarly + transformed_eef_pose_cluster_2 = [ + torch.eye(4) + torch.tensor([[0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, i], [0, 0, 0, -1]]) + for i in range(cluster_2_range_min, cluster_2_range_max) + ] + + # Combine the poses from both clusters into a single list + # This represents the first end effector pose for the transformed subtask segment for each source demo + transformed_eef_in_world_poses_tensor = torch.stack(transformed_eef_pose_cluster_1 + transformed_eef_pose_cluster_2) + + # Create transformation matrices corresponding to each source object pose + src_obj_in_world_poses = torch.stack( + [ PoseUtils.generate_random_transformation_matrix(pos_boundary=10, rot_boundary=(2 * np.pi)) for _ in range(transformed_eef_in_world_poses_tensor.shape[0]) - ]) - - # Calculate the src_eef poses from the transformed eef poses, src_obj_in_world and curr_obj_pose_in_world - # This is the inverse of the transformation of the eef pose done in NearestNeighborRobotDistanceStrategy - # Refer to NearestNeighborRobotDistanceStrategy.select_source_demo for more details - curr_object_in_world_pose = PoseUtils.generate_random_transformation_matrix( - pos_boundary=10, rot_boundary=(2 * np.pi) - ) - world_in_curr_obj_pose = PoseUtils.pose_inv(curr_object_in_world_pose) - - src_eef_in_src_obj_poses = PoseUtils.pose_in_A_to_pose_in_B( - pose_in_A=transformed_eef_in_world_poses_tensor, - pose_A_in_B=world_in_curr_obj_pose, - ) - - src_eef_in_world_poses = PoseUtils.pose_in_A_to_pose_in_B( - pose_in_A=src_eef_in_src_obj_poses, - pose_A_in_B=src_obj_in_world_poses, - ) - - # Check that both lists have the same length - self.assertTrue( - src_obj_in_world_poses.shape[0] == src_eef_in_world_poses.shape[0], - "Source object poses and end effector poses does not have the same length." - "This is a bug in the test code and not the source code.", - ) - - # Create DatagenInfo instances for these positions - src_subtask_datagen_infos = [ - DatagenInfo( - eef_pose=src_eef_in_world_pose.unsqueeze(0), object_poses={0: src_obj_in_world_pose.unsqueeze(0)} - ) - for src_obj_in_world_pose, src_eef_in_world_pose in zip(src_obj_in_world_poses, src_eef_in_world_poses) - ] - - # Test 1: Ensure the nearest neighbor is always part of cluster 1 - max_deviation = 3 # Define a maximum deviation for the current pose - # Define the end-effector pose - # Set the current object pose to the first value of cluster 1 and add some noise - random_index_cluster_1 = np.random.randint(0, len(transformed_eef_pose_cluster_1)) - curr_eef_in_world_pose = transformed_eef_pose_cluster_1[ - random_index_cluster_1 - ].clone() # Use clone to avoid reference issues - # Randomly adjust the current pose within the maximum deviation - curr_eef_in_world_pose[0, 3] += torch.rand(1).item() * max_deviation - curr_eef_in_world_pose[1, 3] += torch.rand(1).item() * max_deviation - curr_eef_in_world_pose[2, 3] += torch.rand(1).item() * max_deviation - - # Select source demonstrations multiple times to check randomness - selected_indices = [ - self.strategy.select_source_demo( - curr_eef_in_world_pose, - curr_object_in_world_pose, - src_subtask_datagen_infos, - pos_weight=1.0, - rot_weight=1.0, - nn_k=3, # Check among the top 3 nearest neighbors - ) - for _ in range(20) - ] - - # Assert that all selected indices are valid indices within cluster 1 - self.assertTrue( - np.all(np.array(selected_indices) < len(transformed_eef_pose_cluster_1)), - "Some selected indices are not part of cluster 1.", - ) - - # Test 2: Ensure the nearest neighbor is always part of cluster 2 - max_deviation = 3 # Define a maximum deviation for the current pose - # Define the end-effector pose - # Set the current object pose to the first value of cluster 2 and add some noise - random_index_cluster_2 = np.random.randint(0, len(transformed_eef_pose_cluster_2)) - curr_eef_in_world_pose = transformed_eef_pose_cluster_2[ - random_index_cluster_2 - ].clone() # Use clone to avoid reference issues - # Randomly adjust the current pose within the maximum deviation - curr_eef_in_world_pose[0, 3] += torch.rand(1).item() * max_deviation - curr_eef_in_world_pose[1, 3] += torch.rand(1).item() * max_deviation - curr_eef_in_world_pose[2, 3] += torch.rand(1).item() * max_deviation - - # Select source demonstrations multiple times to check randomness - selected_indices = [ - self.strategy.select_source_demo( - curr_eef_in_world_pose, - curr_object_in_world_pose, - src_subtask_datagen_infos, - pos_weight=1.0, - rot_weight=1.0, - nn_k=3, # Check among the top 3 nearest neighbors - ) - for _ in range(20) ] - - # Assert that all selected indices are valid indices within cluster 2 - self.assertTrue( - np.all(np.array(selected_indices) < transformed_eef_in_world_poses_tensor.shape[0]), - "Some selected indices are not part of cluster 2.", + ) + + # Calculate the src_eef poses from the transformed eef poses, src_obj_in_world and curr_obj_pose_in_world + # This is the inverse of the transformation of the eef pose done in NearestNeighborRobotDistanceStrategy + # Refer to NearestNeighborRobotDistanceStrategy.select_source_demo for more details + curr_object_in_world_pose = PoseUtils.generate_random_transformation_matrix( + pos_boundary=10, rot_boundary=(2 * np.pi) + ) + world_in_curr_obj_pose = PoseUtils.pose_inv(curr_object_in_world_pose) + + src_eef_in_src_obj_poses = PoseUtils.pose_in_A_to_pose_in_B( + pose_in_A=transformed_eef_in_world_poses_tensor, + pose_A_in_B=world_in_curr_obj_pose, + ) + + src_eef_in_world_poses = PoseUtils.pose_in_A_to_pose_in_B( + pose_in_A=src_eef_in_src_obj_poses, + pose_A_in_B=src_obj_in_world_poses, + ) + + # Check that both lists have the same length + assert src_obj_in_world_poses.shape[0] == src_eef_in_world_poses.shape[0], ( + "Source object poses and end effector poses does not have the same length. " + "This is a bug in the test code and not the source code." + ) + + # Create DatagenInfo instances for these positions + src_subtask_datagen_infos = [ + DatagenInfo(eef_pose=src_eef_in_world_pose.unsqueeze(0), object_poses={0: src_obj_in_world_pose.unsqueeze(0)}) + for src_obj_in_world_pose, src_eef_in_world_pose in zip(src_obj_in_world_poses, src_eef_in_world_poses) + ] + + # Test 1: Ensure the nearest neighbor is always part of cluster 1 + max_deviation = 3 # Define a maximum deviation for the current pose + # Define the end-effector pose + # Set the current object pose to the first value of cluster 1 and add some noise + random_index_cluster_1 = np.random.randint(0, len(transformed_eef_pose_cluster_1)) + curr_eef_in_world_pose = transformed_eef_pose_cluster_1[ + random_index_cluster_1 + ].clone() # Use clone to avoid reference issues + # Randomly adjust the current pose within the maximum deviation + curr_eef_in_world_pose[0, 3] += torch.rand(1).item() * max_deviation + curr_eef_in_world_pose[1, 3] += torch.rand(1).item() * max_deviation + curr_eef_in_world_pose[2, 3] += torch.rand(1).item() * max_deviation + + # Select source demonstrations multiple times to check randomness + selected_indices = [ + nearest_neighbor_robot_distance_strategy.select_source_demo( + curr_eef_in_world_pose, + curr_object_in_world_pose, + src_subtask_datagen_infos, + pos_weight=1.0, + rot_weight=1.0, + nn_k=3, # Check among the top 3 nearest neighbors ) - self.assertTrue( - np.all(np.array(selected_indices) > (len(transformed_eef_pose_cluster_1) - 1)), - "Some selected indices are not part of cluster 2.", + for _ in range(20) + ] + + # Assert that all selected indices are valid indices within cluster 1 + assert np.all(np.array(selected_indices) < len(transformed_eef_pose_cluster_1)), ( + "Some selected indices are not part of cluster 1." + ) + + # Test 2: Ensure the nearest neighbor is always part of cluster 2 + max_deviation = 3 # Define a maximum deviation for the current pose + # Define the end-effector pose + # Set the current object pose to the first value of cluster 2 and add some noise + random_index_cluster_2 = np.random.randint(0, len(transformed_eef_pose_cluster_2)) + curr_eef_in_world_pose = transformed_eef_pose_cluster_2[ + random_index_cluster_2 + ].clone() # Use clone to avoid reference issues + # Randomly adjust the current pose within the maximum deviation + curr_eef_in_world_pose[0, 3] += torch.rand(1).item() * max_deviation + curr_eef_in_world_pose[1, 3] += torch.rand(1).item() * max_deviation + curr_eef_in_world_pose[2, 3] += torch.rand(1).item() * max_deviation + + # Select source demonstrations multiple times to check randomness + selected_indices = [ + nearest_neighbor_robot_distance_strategy.select_source_demo( + curr_eef_in_world_pose, + curr_object_in_world_pose, + src_subtask_datagen_infos, + pos_weight=1.0, + rot_weight=1.0, + nn_k=3, # Check among the top 3 nearest neighbors ) - - -if __name__ == "__main__": - unittest.main() + for _ in range(20) + ] + + # Assert that all selected indices are valid indices within cluster 2 + assert np.all(np.array(selected_indices) < transformed_eef_in_world_poses_tensor.shape[0]), ( + "Some selected indices are not part of cluster 2." + ) + assert np.all(np.array(selected_indices) > (len(transformed_eef_pose_cluster_1) - 1)), ( + "Some selected indices are not part of cluster 2." + ) diff --git a/source/isaaclab_rl/config/extension.toml b/source/isaaclab_rl/config/extension.toml index ebc77a2310b3..79f171d3398c 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.1.1" +version = "0.4.7" # Description title = "Isaac Lab RL" diff --git a/source/isaaclab_rl/docs/CHANGELOG.rst b/source/isaaclab_rl/docs/CHANGELOG.rst index c838232364ae..6fe0be78d0a5 100644 --- a/source/isaaclab_rl/docs/CHANGELOG.rst +++ b/source/isaaclab_rl/docs/CHANGELOG.rst @@ -1,6 +1,180 @@ Changelog --------- +0.4.7 (2025-12-29) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Added :mod:`isaaclab_rl.utils.pretrained_checkpoint` sub-module to handle various pre-trained checkpoint tasks. + This module was previously located in the :mod:`isaaclab.utils` module. + + +0.4.6 (2025-11-10) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Added support for decoupling RL device from simulation device in for RL games wrapper. + This allows users to run simulation on one device (e.g., CPU) while running RL training/inference on another device. + + +0.4.5 (2025-12-01) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added state_dependent_std rsl_rl param to RSL-RL wrapper. + + +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) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Enhanced rl-games wrapper to allow dict observation. + + +0.2.4 (2025-08-07) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Disallowed string values in ``sb3_ppo_cfg.yaml`` from being passed to ``eval()`` in + :meth:`~isaaclab_rl.sb3.process_sb3_cfg`. This change prevents accidental or malicious + code execution when loading configuration files, improving overall security and reliability. + + +0.2.3 (2025-06-29) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Support SB3 VecEnv wrapper to configure with composite observation spaces properly so that the cnn creation pipelines + natively supported by sb3 can be automatically triggered + + +0.2.2 (2025-06-30) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Call :meth:`eval` during :meth:`forward`` RSL-RL OnnxPolicyExporter + + +0.2.1 (2025-06-26) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Relaxed upper range pin for protobuf python dependency for more permissive installation. + + +0.2.0 (2025-04-24) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Switched to a 3.11 compatible branch for rl-games as Isaac Sim 5.0 is now using Python 3.11. + + +0.1.5 (2025-04-11) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Optimized Stable-Baselines3 wrapper ``Sb3VecEnvWrapper`` (now 4x faster) by using Numpy buffers and only logging episode and truncation information by default. +* Upgraded minimum SB3 version to 2.6.0 and added optional dependencies for progress bar + + +0.1.4 (2025-04-10) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added configurations for distillation implementation in RSL-RL. +* Added configuration for recurrent actor-critic in RSL-RL. + + +0.1.3 (2025-03-31) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed the location of :meth:`isaaclab_rl.rsl_rl.RslRlOnPolicyRunnerCfg._modify_action_space` + to be called only after retrieving the dimensions of the environment, preventing errors + related to accessing uninitialized attributes. + + +0.1.2 (2025-03-28) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added symmetry and curiosity-based exploration configurations for RSL-RL wrapper. + + 0.1.1 (2025-03-10) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_rl/isaaclab_rl/__init__.py b/source/isaaclab_rl/isaaclab_rl/__init__.py index a0c300361a66..f9e67f543a74 100644 --- a/source/isaaclab_rl/isaaclab_rl/__init__.py +++ b/source/isaaclab_rl/isaaclab_rl/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -17,7 +17,3 @@ 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. """ - -from . import rl_games, rsl_rl, sb3, skrl - -__all__ = ["sb3", "skrl", "rsl_rl", "rl_games"] 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 000000000000..f1d006ae6cc9 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrappers and utilities to configure an environment for rl-games library.""" + +from .pbt import * +from .rl_games import * diff --git a/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/__init__.py b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/__init__.py new file mode 100644 index 000000000000..c56bf4f40e51 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/__init__.py @@ -0,0 +1,7 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .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 000000000000..ad942de8eec9 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/mutation.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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 000000000000..aeec36055eb1 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt.py @@ -0,0 +1,268 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import random +import sys + +import numpy as np +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 000000000000..b494dd1fdefe --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_cfg.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 000000000000..959c24e41031 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_utils.py @@ -0,0 +1,297 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import datetime +import os +import random +import socket +from collections import OrderedDict +from pathlib import Path + +import yaml +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 58% rename from source/isaaclab_rl/isaaclab_rl/rl_games.py rename to source/isaaclab_rl/isaaclab_rl/rl_games/rl_games.py index 9bc47bddedd5..d5c786c7c9e7 100644 --- a/source/isaaclab_rl/isaaclab_rl/rl_games.py +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/rl_games.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -34,10 +34,11 @@ # needed to import for allowing type-hinting:gym.spaces.Box | None from __future__ import annotations +from collections.abc import Callable + 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 @@ -60,12 +61,15 @@ class RlGamesVecEnvWrapper(IVecEnv): 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. + To use asymmetric actor-critic, map privileged observation groups under ``"states"`` (e.g. ``["critic"]``). + + The wrapper supports **either** concatenated tensors (default) **or** Dict inputs: + when wrapper is concate mode, rl-games sees {"obs": Tensor, (optional)"states": Tensor} + when wrapper is not concate mode, rl-games sees {"obs": dict[str, Tensor], (optional)"states": dict[str, Tensor]} + + - Concatenated mode (``concate_obs_group=True``): ``observation_space``/``state_space`` are ``gym.spaces.Box``. + - Dict mode (``concate_obs_group=False``): ``observation_space``/``state_space`` are ``gym.spaces.Dict`` keyed by + the requested groups. When no ``"states"`` groups are provided, the states Dict is omitted at runtime. .. caution:: @@ -79,7 +83,15 @@ class RlGamesVecEnvWrapper(IVecEnv): https://github.com/NVIDIA-Omniverse/IsaacGymEnvs """ - def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, rl_device: str, clip_obs: float, clip_actions: float): + def __init__( + self, + env: ManagerBasedRLEnv | DirectRLEnv, + rl_device: str, + clip_obs: float, + clip_actions: float, + obs_groups: dict[str, list[str]] | None = None, + concate_obs_group: bool = True, + ): """Initializes the wrapper instance. Args: @@ -87,6 +99,9 @@ def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, rl_device: str, clip_ob rl_device: The device on which agent computations are performed. clip_obs: The clipping value for observations. clip_actions: The clipping value for actions. + obs_groups: The remapping from isaaclab observation to rl-games, default to None for backward compatible. + concate_obs_group: The boolean value indicates if input to rl-games network is dict or tensor. Default to + True for backward compatible. Raises: ValueError: The environment is not inherited from :class:`ManagerBasedRLEnv` or :class:`DirectRLEnv`. @@ -105,11 +120,36 @@ def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, rl_device: str, clip_ob 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: + + # resolve the observation group + self._concate_obs_groups = concate_obs_group + self._obs_groups = obs_groups + if obs_groups is None: + self._obs_groups = {"obs": ["policy"], "states": []} + if not self.unwrapped.single_observation_space.get("policy"): + raise KeyError("Policy observation group is expected if no explicit groups is defined") + if self.unwrapped.single_observation_space.get("critic"): + self._obs_groups["states"] = ["critic"] + + if ( + self._concate_obs_groups + and isinstance(self.state_space, gym.spaces.Box) + and isinstance(self.observation_space, gym.spaces.Box) + ): self.rlg_num_states = self.state_space.shape[0] + elif ( + not self._concate_obs_groups + and isinstance(self.state_space, gym.spaces.Dict) + and isinstance(self.observation_space, gym.spaces.Dict) + ): + space = [space.shape[0] for space in self.state_space.values()] + self.rlg_num_states = sum(space) + else: + raise TypeError( + "only valid combination for state space is gym.space.Box when concate_obs_groups is True, " + " and gym.space.Dict when concate_obs_groups is False. You have concate_obs_groups: " + f" {self._concate_obs_groups}, and state_space: {self.state_space.__class__}" + ) def __str__(self): """Returns the wrapper name and the :attr:`env` representation string.""" @@ -135,19 +175,18 @@ def render_mode(self) -> str | None: return self.env.render_mode @property - def observation_space(self) -> gym.spaces.Box: - """Returns the :attr:`Env` :attr:`observation_space`.""" + def observation_space(self) -> gym.spaces.Box | gym.spaces.Dict: + """Returns the :attr:`Env` :attr:`observation_space` (``Box`` if concatenated, otherwise ``Dict``).""" # 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) + space = self.unwrapped.single_observation_space + clip = self._clip_obs + if not self._concate_obs_groups: + policy_space = {grp: gym.spaces.Box(-clip, clip, space.get(grp).shape) for grp in self._obs_groups["obs"]} + return gym.spaces.Dict(policy_space) + else: + shapes = [space.get(group).shape for group in self._obs_groups["obs"]] + cat_shape, self._obs_concat_fn = make_concat_plan(shapes) + return gym.spaces.Box(-clip, clip, cat_shape) @property def action_space(self) -> gym.Space: @@ -193,23 +232,18 @@ def device(self) -> str: 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 state_space(self) -> gym.spaces.Box | gym.spaces.Dict | None: + """Returns the privileged observation space for the critic (``Box`` if concatenated, otherwise ``Dict``).""" + # # note: rl-games only wants single observation space + space = self.unwrapped.single_observation_space + clip = self._clip_obs + if not self._concate_obs_groups: + state_space = {grp: gym.spaces.Box(-clip, clip, space.get(grp).shape) for grp in self._obs_groups["states"]} + return gym.spaces.Dict(state_space) + else: + shapes = [space.get(group).shape for group in self._obs_groups["states"]] + cat_shape, self._states_concat_fn = make_concat_plan(shapes) + return gym.spaces.Box(-self._clip_obs, self._clip_obs, cat_shape) def get_number_of_agents(self) -> int: """Returns number of actors in the environment.""" @@ -270,7 +304,7 @@ def close(self): # noqa: D102 Helper functions """ - def _process_obs(self, obs_dict: VecEnvObs) -> torch.Tensor | dict[str, torch.Tensor]: + def _process_obs(self, obs_dict: VecEnvObs) -> dict[str, torch.Tensor] | dict[str, dict[str, torch.Tensor]]: """Processing of the observations and states from the environment. Note: @@ -280,32 +314,65 @@ def _process_obs(self, obs_dict: VecEnvObs) -> torch.Tensor | dict[str, torch.Te 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. + Returns: + A dictionary for RL-Games with keys: + - ``"obs"``: either a concatenated tensor (``concate_obs_group=True``) or a Dict of group tensors. + - ``"states"`` (optional): same structure as above when state groups are configured; omitted otherwise. """ - # process policy obs - obs = obs_dict["policy"] + # move observations to RL device if different from sim device + if self._rl_device != self._sim_device: + obs_dict = {key: obs.to(device=self._rl_device) for key, obs in obs_dict.items()} + # 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} + for key, obs in obs_dict.items(): + obs_dict[key] = torch.clamp(obs, -self._clip_obs, self._clip_obs) + + # process input obs dict + rl_games_obs = {"obs": {group: obs_dict[group] for group in self._obs_groups["obs"]}} + if len(self._obs_groups["states"]) > 0: + rl_games_obs["states"] = {group: obs_dict[group] for group in self._obs_groups["states"]} + + if self._concate_obs_groups: + rl_games_obs["obs"] = self._obs_concat_fn(list(rl_games_obs["obs"].values())) + if "states" in rl_games_obs: + rl_games_obs["states"] = self._states_concat_fn(list(rl_games_obs["states"].values())) + + return rl_games_obs + + +def make_concat_plan(shapes: list[tuple[int, ...]]) -> tuple[tuple[int, ...], Callable]: + """ + Given per-sample shapes (no batch dim), return: + - the concatenated per-sample shape + - a function that concatenates a list of batch tensors accordingly. + + Rules: + 0) Empty -> (0,), No-op + 1) All 1D -> concat features (dim=1). + 2) Same rank > 1: + 2a) If all s[:-1] equal -> concat along last dim (channels-last, dim=-1). + 2b) If all s[1:] equal -> concat along first dim (channels-first, dim=1). + """ + if len(shapes) == 0: + return (0,), lambda x: x + # case 1: all vectors + if all(len(s) == 1 for s in shapes): + return (sum(s[0] for s in shapes),), lambda x: torch.cat(x, dim=1) + # case 2: same rank > 1 + rank = len(shapes[0]) + if all(len(s) == rank for s in shapes) and rank > 1: + # 2a: concat along last axis (โ€ฆC) + if all(s[:-1] == shapes[0][:-1] for s in shapes): + out_shape = shapes[0][:-1] + (sum(s[-1] for s in shapes),) + return out_shape, lambda x: torch.cat(x, dim=-1) + # 2b: concat along first axis (Cโ€ฆ) + if all(s[1:] == shapes[0][1:] for s in shapes): + out_shape = (sum(s[0] for s in shapes),) + shapes[0][1:] + return out_shape, lambda x: torch.cat(x, dim=1) else: - return obs + raise ValueError(f"Could not find a valid concatenation plan for rank {[(len(s),) for s in shapes]}") + else: + raise ValueError("Could not find a valid concatenation plan, please make sure all value share the same size") """ diff --git a/source/isaaclab_rl/isaaclab_rl/rsl_rl/__init__.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/__init__.py index 19314893a62c..3d8c62968994 100644 --- a/source/isaaclab_rl/isaaclab_rl/rsl_rl/__init__.py +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -15,6 +15,9 @@ """ +from .distillation_cfg import * from .exporter import export_policy_as_jit, export_policy_as_onnx -from .rl_cfg import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from .rl_cfg import * +from .rnd_cfg import RslRlRndCfg +from .symmetry_cfg import RslRlSymmetryCfg from .vecenv_wrapper import RslRlVecEnvWrapper diff --git a/source/isaaclab_rl/isaaclab_rl/rsl_rl/distillation_cfg.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/distillation_cfg.py new file mode 100644 index 000000000000..1a631eeffdaa --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/distillation_cfg.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from .rl_cfg import RslRlBaseRunnerCfg + +######################### +# 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_obs_normalization: bool = MISSING + """Whether to normalize the observation for the student network.""" + + teacher_obs_normalization: bool = MISSING + """Whether to normalize the observation for the teacher network.""" + + 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.""" + + optimizer: Literal["adam", "adamw", "sgd", "rmsprop"] = "adam" + """The optimizer to use for the student policy.""" + + loss_type: Literal["mse", "huber"] = "mse" + """The loss type to use for the student policy.""" + + +######################### +# Runner configurations # +######################### + + +@configclass +class RslRlDistillationRunnerCfg(RslRlBaseRunnerCfg): + """Configuration of the runner for distillation algorithms.""" + + class_name: str = "DistillationRunner" + """The runner class name. Default is DistillationRunner.""" + + policy: RslRlDistillationStudentTeacherCfg = MISSING + """The policy configuration.""" + + algorithm: RslRlDistillationAlgorithmCfg = MISSING + """The algorithm configuration.""" diff --git a/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py index 94144888719a..d5b8a248adb8 100644 --- a/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py @@ -1,33 +1,34 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import copy import os + import torch -def export_policy_as_jit(actor_critic: object, normalizer: object | None, path: str, filename="policy.pt"): +def export_policy_as_jit(policy: object, normalizer: object | None, path: str, filename="policy.pt"): """Export policy into a Torch JIT file. Args: - actor_critic: The actor-critic torch module. + 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(actor_critic, normalizer) + policy_exporter = _TorchPolicyExporter(policy, normalizer) policy_exporter.export(path, filename) def export_policy_as_onnx( - actor_critic: object, path: str, normalizer: object | None = None, filename="policy.onnx", verbose=False + policy: object, path: str, normalizer: object | None = None, filename="policy.onnx", verbose=False ): """Export policy into a Torch ONNX file. Args: - actor_critic: The actor-critic torch module. + 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". @@ -35,7 +36,7 @@ def export_policy_as_onnx( """ if not os.path.exists(path): os.makedirs(path, exist_ok=True) - policy_exporter = _OnnxPolicyExporter(actor_critic, normalizer, verbose) + policy_exporter = _OnnxPolicyExporter(policy, normalizer, verbose) policy_exporter.export(path, filename) @@ -47,17 +48,34 @@ def export_policy_as_onnx( class _TorchPolicyExporter(torch.nn.Module): """Exporter of actor-critic into JIT file.""" - def __init__(self, actor_critic, normalizer=None): + def __init__(self, policy, normalizer=None): super().__init__() - self.actor = copy.deepcopy(actor_critic.actor) - self.is_recurrent = actor_critic.is_recurrent + 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 = copy.deepcopy(actor_critic.memory_a.rnn) self.rnn.cpu() + self.rnn_type = type(self.rnn).__name__.lower() # 'lstm' or 'gru' 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 + if self.rnn_type == "lstm": + 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 + elif self.rnn_type == "gru": + self.forward = self.forward_gru + self.reset = self.reset_memory + else: + raise NotImplementedError(f"Unsupported RNN type: {self.rnn_type}") # copy normalizer if exists if normalizer: self.normalizer = copy.deepcopy(normalizer) @@ -72,6 +90,13 @@ def forward_lstm(self, x): x = x.squeeze(0) return self.actor(x) + def forward_gru(self, x): + x = self.normalizer(x) + x, h = self.rnn(x.unsqueeze(0), self.hidden_state) + self.hidden_state[:] = h + x = x.squeeze(0) + return self.actor(x) + def forward(self, x): return self.actor(self.normalizer(x)) @@ -81,7 +106,8 @@ def reset(self): def reset_memory(self): self.hidden_state[:] = 0.0 - self.cell_state[:] = 0.0 + if hasattr(self, "cell_state"): + self.cell_state[:] = 0.0 def export(self, path, filename): os.makedirs(path, exist_ok=True) @@ -94,15 +120,31 @@ def export(self, path, filename): class _OnnxPolicyExporter(torch.nn.Module): """Exporter of actor-critic into ONNX file.""" - def __init__(self, actor_critic, normalizer=None, verbose=False): + def __init__(self, policy, normalizer=None, verbose=False): super().__init__() self.verbose = verbose - self.actor = copy.deepcopy(actor_critic.actor) - self.is_recurrent = actor_critic.is_recurrent + 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 = copy.deepcopy(actor_critic.memory_a.rnn) self.rnn.cpu() - self.forward = self.forward_lstm + self.rnn_type = type(self.rnn).__name__.lower() # 'lstm' or 'gru' + if self.rnn_type == "lstm": + self.forward = self.forward_lstm + elif self.rnn_type == "gru": + self.forward = self.forward_gru + else: + raise NotImplementedError(f"Unsupported RNN type: {self.rnn_type}") # copy normalizer if exists if normalizer: self.normalizer = copy.deepcopy(normalizer) @@ -115,27 +157,50 @@ def forward_lstm(self, x_in, h_in, c_in): x = x.squeeze(0) return self.actor(x), h, c + def forward_gru(self, x_in, h_in): + x_in = self.normalizer(x_in) + x, h = self.rnn(x_in.unsqueeze(0), h_in) + x = x.squeeze(0) + return self.actor(x), h + def forward(self, x): return self.actor(self.normalizer(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) - 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={}, - ) + + if self.rnn_type == "lstm": + c_in = torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size) + torch.onnx.export( + self, + (obs, h_in, c_in), + os.path.join(path, filename), + export_params=True, + opset_version=opset_version, + verbose=self.verbose, + input_names=["obs", "h_in", "c_in"], + output_names=["actions", "h_out", "c_out"], + dynamic_axes={}, + ) + elif self.rnn_type == "gru": + torch.onnx.export( + self, + (obs, h_in), + os.path.join(path, filename), + export_params=True, + opset_version=opset_version, + verbose=self.verbose, + input_names=["obs", "h_in"], + output_names=["actions", "h_out"], + dynamic_axes={}, + ) + else: + raise NotImplementedError(f"Unsupported RNN type: {self.rnn_type}") else: obs = torch.zeros(1, self.actor[0].in_features) torch.onnx.export( @@ -143,7 +208,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/isaaclab_rl/rsl_rl/rl_cfg.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/rl_cfg.py index c2a498998d6f..7be991174dec 100644 --- a/source/isaaclab_rl/isaaclab_rl/rsl_rl/rl_cfg.py +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/rl_cfg.py @@ -1,13 +1,22 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + from dataclasses import MISSING from typing import Literal from isaaclab.utils import configclass +from .rnd_cfg import RslRlRndCfg +from .symmetry_cfg import RslRlSymmetryCfg + +######################### +# Policy configurations # +######################### + @configclass class RslRlPpoActorCriticCfg: @@ -19,6 +28,18 @@ class RslRlPpoActorCriticCfg: 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.""" + + state_dependent_std: bool = False + """Whether to use state-dependent standard deviation for the policy. Default is False.""" + + actor_obs_normalization: bool = MISSING + """Whether to normalize the observation for the actor network.""" + + critic_obs_normalization: bool = MISSING + """Whether to normalize the observation for the critic network.""" + actor_hidden_dims: list[int] = MISSING """The hidden dimensions of the actor network.""" @@ -30,23 +51,33 @@ class RslRlPpoActorCriticCfg: @configclass -class RslRlPpoAlgorithmCfg: - """Configuration for the PPO algorithm.""" +class RslRlPpoActorCriticRecurrentCfg(RslRlPpoActorCriticCfg): + """Configuration for the PPO actor-critic networks with recurrent layers.""" - class_name: str = "PPO" - """The algorithm class name. Default is PPO.""" + class_name: str = "ActorCriticRecurrent" + """The policy class name. Default is ActorCriticRecurrent.""" - value_loss_coef: float = MISSING - """The coefficient for the value loss.""" + rnn_type: str = MISSING + """The type of RNN to use. Either "lstm" or "gru".""" - use_clipped_value_loss: bool = MISSING - """Whether to use clipped value loss.""" + rnn_hidden_dim: int = MISSING + """The dimension of the RNN layers.""" - clip_param: float = MISSING - """The clipping parameter for the policy.""" + rnn_num_layers: int = MISSING + """The number of RNN layers.""" - entropy_coef: float = MISSING - """The coefficient for the entropy loss.""" + +############################ +# 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.""" @@ -66,16 +97,46 @@ class RslRlPpoAlgorithmCfg: 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. + """ + + rnd_cfg: RslRlRndCfg | None = None + """The RND configuration. Default is None, in which case RND is not used.""" + + symmetry_cfg: RslRlSymmetryCfg | None = None + """The symmetry configuration. Default is None, in which case symmetry is not used.""" + + +######################### +# Runner configurations # +######################### + @configclass -class RslRlOnPolicyRunnerCfg: - """Configuration of the runner for on-policy algorithms.""" +class RslRlBaseRunnerCfg: + """Base configuration of the runner.""" seed: int = 42 """The seed for the experiment. Default is 42.""" @@ -89,21 +150,40 @@ class RslRlOnPolicyRunnerCfg: max_iterations: int = MISSING """The maximum number of iterations.""" - empirical_normalization: bool = MISSING - """Whether to use empirical normalization.""" + empirical_normalization: bool | None = None + """This parameter is deprecated and will be removed in the future. - policy: RslRlPpoActorCriticCfg = MISSING - """The policy configuration.""" + Use `actor_obs_normalization` and `critic_obs_normalization` instead. + """ - algorithm: RslRlPpoAlgorithmCfg = MISSING - """The algorithm configuration.""" + obs_groups: dict[str, list[str]] = MISSING + """A mapping from observation groups to observation sets. + + The keys of the dictionary are predefined observation sets used by the underlying algorithm + and values are lists of observation groups provided by the environment. + + For instance, if the environment provides a dictionary of observations with groups "policy", "images", + and "privileged", these can be mapped to algorithmic observation sets as follows: + + .. code-block:: python + + obs_groups = { + "policy": ["policy", "images"], + "critic": ["policy", "privileged"], + } + + This way, the policy will receive the "policy" and "images" observations, and the critic will + receive the "policy" and "privileged" observations. + + For more details, please check ``vec_env.py`` in the rsl_rl library. + """ clip_actions: float | None = None - """The clipping value for actions. If ``None``, then no clipping is done.""" + """The clipping value for actions. If None, then no clipping is done. Defaults to None. - ## - # Checkpointing parameters - ## + .. note:: + This clipping is performed inside the :class:`RslRlVecEnvWrapper` wrapper. + """ save_interval: int = MISSING """The number of iterations between saves.""" @@ -119,10 +199,6 @@ class RslRlOnPolicyRunnerCfg: ``{time-stamp}_{run_name}``. """ - ## - # Logging parameters - ## - logger: Literal["tensorboard", "neptune", "wandb"] = "tensorboard" """The logger to use. Default is tensorboard.""" @@ -132,12 +208,11 @@ class RslRlOnPolicyRunnerCfg: wandb_project: str = "isaaclab" """The wandb project name. Default is "isaaclab".""" - ## - # Loading parameters - ## - resume: bool = False - """Whether to resume. Default is False.""" + """Whether to resume a previous training. Default is False. + + This flag will be ignored for distillation. + """ load_run: str = ".*" """The run directory to load. Default is ".*" (all). @@ -150,3 +225,17 @@ class RslRlOnPolicyRunnerCfg: If regex expression, the latest (alphabetical order) matching file will be loaded. """ + + +@configclass +class RslRlOnPolicyRunnerCfg(RslRlBaseRunnerCfg): + """Configuration of the runner for on-policy algorithms.""" + + class_name: str = "OnPolicyRunner" + """The runner class name. Default is OnPolicyRunner.""" + + policy: RslRlPpoActorCriticCfg = MISSING + """The policy configuration.""" + + algorithm: RslRlPpoAlgorithmCfg = MISSING + """The algorithm configuration.""" diff --git a/source/isaaclab_rl/isaaclab_rl/rsl_rl/rnd_cfg.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/rnd_cfg.py new file mode 100644 index 000000000000..0cc698dc8813 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/rnd_cfg.py @@ -0,0 +1,99 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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/source/isaaclab_rl/isaaclab_rl/rsl_rl/symmetry_cfg.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/symmetry_cfg.py new file mode 100644 index 000000000000..8f60c7430686 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/symmetry_cfg.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 (tensordict.TensorDict | None): The observation tensor dictionary. If None, the observation is not used. + action (torch.Tensor | None): The action tensor. If None, the action is not used. + + Returns: + A tuple containing the augmented observation dictionary 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/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py index 5f351a9f0dea..dde20f2bb165 100644 --- a/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py @@ -1,27 +1,20 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import gymnasium as gym import torch - from rsl_rl.env import VecEnv +from tensordict import TensorDict 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. + """Wraps around Isaac Lab environment for the RSL-RL library .. 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. @@ -43,12 +36,14 @@ def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, clip_actions: float | N 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 @@ -58,28 +53,14 @@ def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, clip_actions: float | N self.device = self.unwrapped.device self.max_episode_length = self.unwrapped.max_episode_length - # modify the action space to the clip range - self._modify_action_space() - # 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() @@ -133,14 +114,6 @@ def unwrapped(self) -> ManagerBasedRLEnv | DirectRLEnv: 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.""" @@ -162,13 +135,20 @@ def episode_length_buf(self, value: torch.Tensor): def seed(self, seed: int = -1) -> int: # noqa: D102 return self.unwrapped.seed(seed) - def reset(self) -> tuple[torch.Tensor, dict]: # noqa: D102 + def reset(self) -> tuple[TensorDict, dict]: # noqa: D102 # reset the environment - obs_dict, _ = self.env.reset() - # return observations - return obs_dict["policy"], {"observations": obs_dict} + obs_dict, extras = self.env.reset() + return TensorDict(obs_dict, batch_size=[self.num_envs]), extras - def step(self, actions: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, dict]: + def get_observations(self) -> TensorDict: + """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 TensorDict(obs_dict, batch_size=[self.num_envs]) + + def step(self, actions: torch.Tensor) -> tuple[TensorDict, torch.Tensor, torch.Tensor, dict]: # clip actions if self.clip_actions is not None: actions = torch.clamp(actions, -self.clip_actions, self.clip_actions) @@ -176,16 +156,12 @@ def step(self, actions: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, torch 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 + return TensorDict(obs_dict, batch_size=[self.num_envs]), rew, dones, extras def close(self): # noqa: D102 return self.env.close() @@ -200,7 +176,8 @@ def _modify_action_space(self): 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. + # 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,) ) diff --git a/source/isaaclab_rl/isaaclab_rl/sb3.py b/source/isaaclab_rl/isaaclab_rl/sb3.py index 66e122f2860a..2177bc6252c4 100644 --- a/source/isaaclab_rl/isaaclab_rl/sb3.py +++ b/source/isaaclab_rl/isaaclab_rl/sb3.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -18,27 +18,33 @@ # needed to import for allowing type-hinting: torch.Tensor | dict[str, torch.Tensor] from __future__ import annotations +import warnings +from typing import Any + 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.preprocessing import is_image_space, is_image_space_channels_first 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 +# remove SB3 warnings because PPO with bigger net actually benefits from GPU +warnings.filterwarnings("ignore", message="You are trying to run PPO on the GPU") + """ Configuration Parser. """ -def process_sb3_cfg(cfg: dict) -> dict: +def process_sb3_cfg(cfg: dict, num_envs: int) -> dict: """Convert simple YAML types to Stable-Baselines classes/components. Args: cfg: A configuration dictionary. + num_envs: the number of parallel environments (used to compute `batch_size` for a desired number of minibatches) Returns: A dictionary containing the converted configuration. @@ -47,30 +53,36 @@ def process_sb3_cfg(cfg: dict) -> dict: 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]: + def update_dict(hyperparams: dict[str, Any], depth: int) -> 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"]: + update_dict(value, depth + 1) + if isinstance(value, str): + if value.startswith("nn."): + hyperparams[key] = getattr(nn, value[3:]) + if depth == 0: + if key in ["learning_rate", "clip_range", "clip_range_vf"]: 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) + # 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]}") + # Convert to a desired batch_size (n_steps=2048 by default for SB3 PPO) + if "n_minibatches" in hyperparams: + hyperparams["batch_size"] = (hyperparams.get("n_steps", 2048) * num_envs) // hyperparams["n_minibatches"] + del hyperparams["n_minibatches"] + return hyperparams # parse agent configuration and convert to classes - return update_dict(cfg) + return update_dict(cfg, depth=0) """ @@ -89,8 +101,8 @@ class Sb3VecEnvWrapper(VecEnv): 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. + uses their own API (i.e. it is closer to Gym 0.21). Thus, we implement + the 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`. @@ -123,12 +135,13 @@ class Sb3VecEnvWrapper(VecEnv): """ - def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv): + def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, fast_variant: bool = True): """Initialize the wrapper. Args: env: The environment to wrap around. - + fast_variant: Use fast variant for processing info + (Only episodic reward, lengths and truncation info are included) Raises: ValueError: When the environment is not an instance of :class:`ManagerBasedRLEnv` or :class:`DirectRLEnv`. """ @@ -140,24 +153,16 @@ def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv): ) # initialize the wrapper self.env = env + self.fast_variant = fast_variant # 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) + self.observation_processors = {} + self._process_spaces() # 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) + self._ep_rew_buf = np.zeros(self.num_envs) + self._ep_len_buf = np.zeros(self.num_envs) def __str__(self): """Returns the wrapper name and the :attr:`env` representation string.""" @@ -190,11 +195,11 @@ def unwrapped(self) -> ManagerBasedRLEnv | DirectRLEnv: def get_episode_rewards(self) -> list[float]: """Returns the rewards of all the episodes.""" - return self._ep_rew_buf.cpu().tolist() + return self._ep_rew_buf.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() + return self._ep_len_buf.tolist() """ Operations - MDP @@ -206,8 +211,8 @@ def seed(self, seed: int | None = None) -> list[int | None]: # noqa: D102 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_() + self._ep_rew_buf = np.zeros(self.num_envs) + self._ep_len_buf = np.zeros(self.num_envs) # convert data types to numpy depending on backend return self._process_obs(obs_dict) @@ -224,28 +229,30 @@ def step_async(self, actions): # noqa: D102 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() + rewards = rew.detach().cpu().numpy() terminated = terminated.detach().cpu().numpy() truncated = truncated.detach().cpu().numpy() dones = dones.detach().cpu().numpy() + + reset_ids = dones.nonzero()[0] + + # update episode un-discounted return and length + self._ep_rew_buf += rewards + self._ep_len_buf += 1 # 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_rew_buf[reset_ids] = 0.0 self._ep_len_buf[reset_ids] = 0 - return obs, rew, dones, infos + return obs, rewards, dones, infos def close(self): # noqa: D102 self.env.close() @@ -279,7 +286,8 @@ def env_method(self, method_name: str, *method_args, indices=None, **method_kwar 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.") + # fake implementation to be able to use `evaluate_policy()` helper + return [False] def get_images(self): # noqa: D102 raise NotImplementedError("Getting images is not supported.") @@ -288,6 +296,58 @@ def get_images(self): # noqa: D102 Helper functions. """ + def _process_spaces(self): + # process observation space + observation_space = self.unwrapped.single_observation_space["policy"] + if isinstance(observation_space, gym.spaces.Dict): + for obs_key, obs_space in observation_space.spaces.items(): + processors: list[callable[[torch.Tensor], Any]] = [] + # assume normalized, if not, it won't pass is_image_space, which check [0-255]. + # for scale like image space that has right shape but not scaled, we will scale it later + if is_image_space(obs_space, check_channels=True, normalized_image=True): + actually_normalized = np.all(obs_space.low == -1.0) and np.all(obs_space.high == 1.0) + if not actually_normalized: + if np.any(obs_space.low != 0) or np.any(obs_space.high != 255): + raise ValueError( + "Your image observation is not normalized in environment, and will not be" + "normalized by sb3 if its min is not 0 and max is not 255." + ) + # sb3 will handle normalization and transpose, but sb3 expects uint8 images + if obs_space.dtype != np.uint8: + processors.append(lambda obs: obs.to(torch.uint8)) + observation_space.spaces[obs_key] = gym.spaces.Box(0, 255, obs_space.shape, np.uint8) + else: + # sb3 will NOT handle the normalization, while sb3 will transpose, its transpose applies to all + # image terms and maybe non-ideal, more, if we can do it in torch on gpu, it will be faster then + # sb3 transpose it in numpy with cpu. + if not is_image_space_channels_first(obs_space): + + def tranp(img: torch.Tensor) -> torch.Tensor: + return img.permute(2, 0, 1) if len(img.shape) == 3 else img.permute(0, 3, 1, 2) + + processors.append(tranp) + h, w, c = obs_space.shape + observation_space.spaces[obs_key] = gym.spaces.Box(-1.0, 1.0, (c, h, w), obs_space.dtype) + + def chained_processor(obs: torch.Tensor, procs=processors) -> Any: + for proc in procs: + obs = proc(obs) + return obs + + # add processor to the dictionary + if len(processors) > 0: + self.observation_processors[obs_key] = chained_processor + + # 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. + 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) + 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" @@ -295,7 +355,9 @@ def _process_obs(self, obs_dict: torch.Tensor | dict[str, torch.Tensor]) -> np.n # note: ManagerBasedRLEnv uses torch backend (by default). if isinstance(obs, dict): for key, value in obs.items(): - obs[key] = value.detach().cpu().numpy() + if key in self.observation_processors: + obs[key] = self.observation_processors[key](value) + obs[key] = obs[key].detach().cpu().numpy() elif isinstance(obs, torch.Tensor): obs = obs.detach().cpu().numpy() else: @@ -306,6 +368,29 @@ 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.""" + # faster version: only process env that terminated and add bootstrapping info + if self.fast_variant: + infos = [{} for _ in range(self.num_envs)] + + for idx in reset_ids: + # fill-in episode monitoring info + infos[idx]["episode"] = { + "r": self._ep_rew_buf[idx], + "l": self._ep_len_buf[idx], + } + + # fill-in bootstrap information + infos[idx]["TimeLimit.truncated"] = truncated[idx] and not terminated[idx] + + # add information about terminal observation separately + if isinstance(obs, dict): + terminal_obs = {key: value[idx] for key, value in obs.items()} + else: + terminal_obs = obs[idx] + infos[idx]["terminal_observation"] = terminal_obs + + return infos + # 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 diff --git a/source/isaaclab_rl/isaaclab_rl/skrl.py b/source/isaaclab_rl/isaaclab_rl/skrl.py index 9ef5fc15fc82..5aba121523f2 100644 --- a/source/isaaclab_rl/isaaclab_rl/skrl.py +++ b/source/isaaclab_rl/isaaclab_rl/skrl.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -18,7 +18,7 @@ .. code-block:: python from skrl.envs.torch.wrappers import wrap_env # for PyTorch, or... - from skrl.envs.jax.wrappers import wrap_env # for JAX + from skrl.envs.jax.wrappers import wrap_env # for JAX env = wrap_env(env, wrapper="isaaclab") diff --git a/source/isaaclab_rl/isaaclab_rl/utils/__init__.py b/source/isaaclab_rl/isaaclab_rl/utils/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/utils/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/isaaclab/utils/pretrained_checkpoint.py b/source/isaaclab_rl/isaaclab_rl/utils/pretrained_checkpoint.py similarity index 95% rename from source/isaaclab/isaaclab/utils/pretrained_checkpoint.py rename to source/isaaclab_rl/isaaclab_rl/utils/pretrained_checkpoint.py index d9351fbd77b1..c2ada0e9b5e8 100644 --- a/source/isaaclab/isaaclab/utils/pretrained_checkpoint.py +++ b/source/isaaclab_rl/isaaclab_rl/utils/pretrained_checkpoint.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,17 +11,18 @@ import carb.settings -from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path 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.""" +PRETRAINED_CHECKPOINT_PATH = str(PRETRAINED_CHECKPOINTS_ASSET_ROOT_DIR) + "/Isaac/IsaacLab/PretrainedCheckpoints" +"""URL for where we store all the pre-trained checkpoints""" + WORKFLOWS = ["rl_games", "rsl_rl", "sb3", "skrl"] """The supported workflows for pre-trained checkpoints""" @@ -31,24 +32,21 @@ 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", } +"""The filename for checkpoints used by the different workflows""" -"""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", } +"""Maps workflow to the agent variable name that determines the logging directory logs/{workflow}/{variable}""" def has_pretrained_checkpoints_asset_root_dir() -> bool: @@ -61,7 +59,7 @@ def get_log_root_path(workflow: str, task_name: str) -> str: return os.path.abspath(os.path.join("logs", workflow, task_name)) -def get_latest_job_run_path(workflow: str, task_name: str) -> str: +def get_latest_job_run_path(workflow: str, task_name: str) -> str | None: """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) @@ -77,7 +75,7 @@ def get_pretrained_checkpoint_path(workflow: str, task_name: str) -> str: 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") + return _get_latest_file_or_directory(path, "*.pt") # type: ignore elif workflow == "sb3": return os.path.join(path, "model.zip") elif workflow == "skrl": diff --git a/source/isaaclab_rl/setup.py b/source/isaaclab_rl/setup.py index 26a8dcf9b6c1..998fba147a2a 100644 --- a/source/isaaclab_rl/setup.py +++ b/source/isaaclab_rl/setup.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,8 +7,8 @@ import itertools import os -import toml +import toml from setuptools import setup # Obtain the extension data from the extension.toml file @@ -19,12 +19,10 @@ # Minimum dependencies required prior to installation INSTALL_REQUIRES = [ # generic - "numpy", - "torch==2.5.1", + "numpy<2", + "torch>=2.7", "torchvision>=0.14.1", # ensure compatibility with torch 1.13.1 - # 5.26.0 introduced a breaking change, so we restricted it for now. - # See issue https://github.com/tensorflow/tensorboard/issues/6808 for details. - "protobuf>=3.20.2, < 5.0.0", + "protobuf>=4.25.8,!=5.26.0", # configuration management "hydra-core", # data collection @@ -34,17 +32,21 @@ # video recording "moviepy", # make sure this is consistent with isaac sim version - "pillow==11.0.0", + "pillow==11.3.0", + "packaging<24", ] -PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu118"] +PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu128"] # Extra dependencies for RL agents EXTRAS_REQUIRE = { - "sb3": ["stable-baselines3>=2.1"], - "skrl": ["skrl>=1.4.2"], - "rl-games": ["rl-games==1.6.1", "gym"], # rl-games still needs gym :( - "rsl-rl": ["rsl-rl-lib>=2.1.1"], + "sb3": ["stable-baselines3>=2.6", "tqdm", "rich"], # tqdm/rich for progress bar + "skrl": ["skrl>=1.4.3"], + "rl-games": [ + "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.1.2", "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"] @@ -73,7 +75,10 @@ classifiers=[ "Natural Language :: English", "Programming Language :: Python :: 3.10", + "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/test/test_rl_games_wrapper.py b/source/isaaclab_rl/test/test_rl_games_wrapper.py index 6b79870a2766..e7f01a561b99 100644 --- a/source/isaaclab_rl/test/test_rl_games_wrapper.py +++ b/source/isaaclab_rl/test/test_rl_games_wrapper.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -14,9 +14,11 @@ """Rest everything follows.""" +import os + import gymnasium as gym +import pytest import torch -import unittest import carb import omni.usd @@ -29,112 +31,112 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestRlGamesVecEnvWrapper(unittest.TestCase): - """Test that RL-Games VecEnv wrapper works as expected.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id: - cfg_entry_point = gym.spec(task_spec.id).kwargs.get("rl_games_cfg_entry_point") - if cfg_entry_point is not None: - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - cls.registered_tasks = cls.registered_tasks[:5] - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - # print all existing task names - print(">>> All registered environments:", cls.registered_tasks) - - def setUp(self) -> None: - # common parameters - self.num_envs = 64 - self.device = "cuda" - - def test_random_actions(self): - """Run random actions and check environments return valid signals.""" - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # create a new stage - omni.usd.get_context().new_stage() - # reset the rtx sensors carb setting to False - carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) - try: - # parse configuration - env_cfg = parse_env_cfg(task_name, device=self.device, num_envs=self.num_envs) - # create environment - env = gym.make(task_name, cfg=env_cfg) - # convert to single-agent instance if required by the RL algorithm - if isinstance(env.unwrapped, DirectMARLEnv): - env = multi_agent_to_single_agent(env) - # wrap environment - env = RlGamesVecEnvWrapper(env, "cuda:0", 100, 100) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # avoid shutdown of process on simulation stop - env.unwrapped.sim._app_control_on_stop_handle = None - - # reset environment - obs = env.reset() - # check signal - self.assertTrue(self._check_valid_tensor(obs)) - - # simulate environment for 100 steps - with torch.inference_mode(): - for _ in range(100): - # sample actions from -1 to 1 - actions = 2 * torch.rand(env.num_envs, *env.action_space.shape, device=env.device) - 1 - # apply actions - transition = env.step(actions) - # check signals - for data in transition: - self.assertTrue(self._check_valid_tensor(data), msg=f"Invalid data: {data}") - - # close the environment - print(f">>> Closing environment: {task_name}") +@pytest.fixture(scope="module") +def registered_tasks(): + # disable interactive mode for wandb for automate environments + os.environ["WANDB_DISABLED"] = "true" + # acquire all Isaac environments names + registered_tasks = list() + for task_spec in gym.registry.values(): + if "Isaac" in task_spec.id: + cfg_entry_point = gym.spec(task_spec.id).kwargs.get("rl_games_cfg_entry_point") + if cfg_entry_point is not None: + # skip automate environments as they require cuda installation + if "assembly" in task_spec.id.lower(): + continue + registered_tasks.append(task_spec.id) + # sort environments by name + registered_tasks.sort() + registered_tasks = registered_tasks[:5] + + # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + # print all existing task names + print(">>> All registered environments:", registered_tasks) + return registered_tasks + + +def test_random_actions(registered_tasks): + """Run random actions and check environments return valid signals.""" + # common parameters + num_envs = 64 + device = "cuda" + for task_name in registered_tasks: + # Use pytest's subtests + print(f">>> Running test for environment: {task_name}") + # create a new stage + omni.usd.get_context().new_stage() + # reset the rtx sensors carb setting to False + carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + try: + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # create environment + env = gym.make(task_name, cfg=env_cfg) + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + # wrap environment + env = RlGamesVecEnvWrapper(env, "cuda:0", 100, 100) + except Exception as e: + if "env" in locals() and hasattr(env, "_is_closed"): env.close() + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - """ - Helper functions. - """ + # avoid shutdown of process on simulation stop + env.unwrapped.sim._app_control_on_stop_handle = None + + # reset environment + obs = env.reset() + # check signal + assert _check_valid_tensor(obs) + + # simulate environment for 100 steps + with torch.inference_mode(): + for _ in range(100): + # sample actions from -1 to 1 + actions = 2 * torch.rand(env.num_envs, *env.action_space.shape, device=env.device) - 1 + # apply actions + transition = env.step(actions) + # check signals + for data in transition: + assert _check_valid_tensor(data), f"Invalid data: {data}" + + # close the environment + print(f">>> Closing environment: {task_name}") + env.close() - @staticmethod - def _check_valid_tensor(data: torch.Tensor | dict) -> bool: - """Checks if given data does not have corrupted values. - - Args: - data: Data buffer. - - Returns: - True if the data is valid. - """ - if isinstance(data, torch.Tensor): - return not torch.any(torch.isnan(data)) - elif isinstance(data, dict): - valid_tensor = True - for value in data.values(): - if isinstance(value, dict): - valid_tensor &= TestRlGamesVecEnvWrapper._check_valid_tensor(value) - elif isinstance(value, torch.Tensor): - valid_tensor &= not torch.any(torch.isnan(value)) - return valid_tensor - else: - raise ValueError(f"Input data of invalid type: {type(data)}.") - - -if __name__ == "__main__": - run_tests() + +""" +Helper functions. +""" + + +@staticmethod +def _check_valid_tensor(data: torch.Tensor | dict) -> bool: + """Checks if given data does not have corrupted values. + + Args: + data: Data buffer. + + Returns: + True if the data is valid. + """ + if isinstance(data, torch.Tensor): + return not torch.any(torch.isnan(data)) + elif isinstance(data, dict): + valid_tensor = True + for value in data.values(): + if isinstance(value, dict): + valid_tensor &= _check_valid_tensor(value) + elif isinstance(value, torch.Tensor): + valid_tensor &= not torch.any(torch.isnan(value)) + return valid_tensor + else: + raise ValueError(f"Input data of invalid type: {type(data)}.") diff --git a/source/isaaclab_rl/test/test_rsl_rl_wrapper.py b/source/isaaclab_rl/test/test_rsl_rl_wrapper.py index a9d741816250..2ae427df2797 100644 --- a/source/isaaclab_rl/test/test_rsl_rl_wrapper.py +++ b/source/isaaclab_rl/test/test_rsl_rl_wrapper.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -15,8 +15,9 @@ """Rest everything follows.""" import gymnasium as gym +import pytest import torch -import unittest +from tensordict import TensorDict import carb import omni.usd @@ -29,148 +30,147 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestRslRlVecEnvWrapper(unittest.TestCase): - """Test that RSL-RL VecEnv wrapper works as expected.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id: - cfg_entry_point = gym.spec(task_spec.id).kwargs.get("rsl_rl_cfg_entry_point") - if cfg_entry_point is not None: - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - cls.registered_tasks = cls.registered_tasks[:5] - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - # print all existing task names - print(">>> All registered environments:", cls.registered_tasks) - - def setUp(self) -> None: - # common parameters - self.num_envs = 64 - self.device = "cuda" - - def test_random_actions(self): - """Run random actions and check environments return valid signals.""" - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # create a new stage - omni.usd.get_context().new_stage() - # reset the rtx sensors carb setting to False - carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) - try: - # parse configuration - env_cfg = parse_env_cfg(task_name, device=self.device, num_envs=self.num_envs) - # create environment - env = gym.make(task_name, cfg=env_cfg) - # convert to single-agent instance if required by the RL algorithm - if isinstance(env.unwrapped, DirectMARLEnv): - env = multi_agent_to_single_agent(env) - # wrap environment - env = RslRlVecEnvWrapper(env) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # reset environment - obs, extras = env.reset() - # check signal - self.assertTrue(self._check_valid_tensor(obs)) - self.assertTrue(self._check_valid_tensor(extras)) - - # simulate environment for 100 steps - with torch.inference_mode(): - for _ in range(100): - # sample actions from -1 to 1 - actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 - # apply actions - transition = env.step(actions) - # check signals - for data in transition: - self.assertTrue(self._check_valid_tensor(data), msg=f"Invalid data: {data}") - - # close the environment - print(f">>> Closing environment: {task_name}") +@pytest.fixture(scope="module") +def registered_tasks(): + # acquire all Isaac environments names + registered_tasks = list() + for task_spec in gym.registry.values(): + if "Isaac" in task_spec.id: + cfg_entry_point = gym.spec(task_spec.id).kwargs.get("rsl_rl_cfg_entry_point") + if cfg_entry_point is not None: + registered_tasks.append(task_spec.id) + # sort environments by name + registered_tasks.sort() + registered_tasks = registered_tasks[:5] + + # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + # print all existing task names + print(">>> All registered environments:", registered_tasks) + return registered_tasks + + +def test_random_actions(registered_tasks): + """Run random actions and check environments return valid signals.""" + # common parameters + num_envs = 64 + device = "cuda" + for task_name in registered_tasks: + # Use pytest's subtests + print(f">>> Running test for environment: {task_name}") + # create a new stage + omni.usd.get_context().new_stage() + # reset the rtx sensors carb setting to False + carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + try: + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # create environment + env = gym.make(task_name, cfg=env_cfg) + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + # wrap environment + env = RslRlVecEnvWrapper(env) + except Exception as e: + if "env" in locals() and hasattr(env, "_is_closed"): env.close() - - def test_no_time_outs(self): - """Check that environments with finite horizon do not send time-out signals.""" - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # create a new stage - omni.usd.get_context().new_stage() - # parse configuration - env_cfg = parse_env_cfg(task_name, device=self.device, num_envs=self.num_envs) - # change to finite horizon - env_cfg.is_finite_horizon = True - - # create environment - env = gym.make(task_name, cfg=env_cfg) - # wrap environment - env = RslRlVecEnvWrapper(env) - - # reset environment - _, extras = env.reset() - # check signal - self.assertNotIn("time_outs", extras, msg="Time-out signal found in finite horizon environment.") - - # simulate environment for 10 steps - with torch.inference_mode(): - for _ in range(10): - # sample actions from -1 to 1 - actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 - # apply actions - extras = env.step(actions)[-1] - # check signals - self.assertNotIn( - "time_outs", extras, msg="Time-out signal found in finite horizon environment." - ) - - # close the environment - print(f">>> Closing environment: {task_name}") - env.close() - + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") + + # reset environment + obs, extras = env.reset() + # check signal + assert _check_valid_tensor(obs) + assert _check_valid_tensor(extras) + + # simulate environment for 100 steps + with torch.inference_mode(): + for _ in range(100): + # sample actions from -1 to 1 + actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 + # apply actions + transition = env.step(actions) + # check signals + for data in transition: + assert _check_valid_tensor(data), f"Invalid data: {data}" + + # close the environment + print(f">>> Closing environment: {task_name}") + env.close() + + +def test_no_time_outs(registered_tasks): + """Check that environments with finite horizon do not send time-out signals.""" + # common parameters + num_envs = 64 + device = "cuda" + for task_name in registered_tasks: + # Use pytest's subtests + print(f">>> Running test for environment: {task_name}") + # create a new stage + omni.usd.get_context().new_stage() + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # change to finite horizon + env_cfg.is_finite_horizon = True + + # create environment + env = gym.make(task_name, cfg=env_cfg) + # wrap environment + env = RslRlVecEnvWrapper(env) + + # reset environment + _, extras = env.reset() + # check signal + assert "time_outs" not in extras, "Time-out signal found in finite horizon environment." + + # simulate environment for 10 steps + with torch.inference_mode(): + for _ in range(10): + # sample actions from -1 to 1 + actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 + # apply actions + extras = env.step(actions)[-1] + # check signals + assert "time_outs" not in extras, "Time-out signal found in finite horizon environment." + + # close the environment + print(f">>> Closing environment: {task_name}") + env.close() + + +""" +Helper functions. +""" + + +@staticmethod +def _check_valid_tensor(data: torch.Tensor | dict) -> bool: + """Checks if given data does not have corrupted values. + + Args: + data: Data buffer. + + Returns: + True if the data is valid. """ - Helper functions. - """ - - @staticmethod - def _check_valid_tensor(data: torch.Tensor | dict) -> bool: - """Checks if given data does not have corrupted values. - - Args: - data: Data buffer. - - Returns: - True if the data is valid. - """ - if isinstance(data, torch.Tensor): - return not torch.any(torch.isnan(data)) - elif isinstance(data, dict): - valid_tensor = True - for value in data.values(): - if isinstance(value, dict): - valid_tensor &= TestRslRlVecEnvWrapper._check_valid_tensor(value) - elif isinstance(value, torch.Tensor): - valid_tensor &= not torch.any(torch.isnan(value)) - return valid_tensor - else: - raise ValueError(f"Input data of invalid type: {type(data)}.") - - -if __name__ == "__main__": - run_tests() + if isinstance(data, torch.Tensor): + return not torch.any(torch.isnan(data)) + elif isinstance(data, TensorDict): + return not data.isnan().any() + elif isinstance(data, dict): + valid_tensor = True + for value in data.values(): + if isinstance(value, dict): + valid_tensor &= _check_valid_tensor(value) + elif isinstance(value, torch.Tensor): + valid_tensor &= not torch.any(torch.isnan(value)) + return valid_tensor + else: + raise ValueError(f"Input data of invalid type: {type(data)}.") diff --git a/source/isaaclab_rl/test/test_sb3_wrapper.py b/source/isaaclab_rl/test/test_sb3_wrapper.py index 5bb64d99686f..be5dc46741e7 100644 --- a/source/isaaclab_rl/test/test_sb3_wrapper.py +++ b/source/isaaclab_rl/test/test_sb3_wrapper.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -16,8 +16,8 @@ import gymnasium as gym import numpy as np +import pytest import torch -import unittest import carb import omni.usd @@ -30,114 +30,109 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestStableBaselines3VecEnvWrapper(unittest.TestCase): - """Test that SB3 VecEnv wrapper works as expected.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id: - cfg_entry_point = gym.spec(task_spec.id).kwargs.get("sb3_cfg_entry_point") - if cfg_entry_point is not None: - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - cls.registered_tasks = cls.registered_tasks[:5] - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - # print all existing task names - print(">>> All registered environments:", cls.registered_tasks) - - def setUp(self) -> None: - # common parameters - self.num_envs = 64 - self.device = "cuda" - - def test_random_actions(self): - """Run random actions and check environments return valid signals.""" - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # create a new stage - omni.usd.get_context().new_stage() - # reset the rtx sensors carb setting to False - carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) - try: - # parse configuration - env_cfg = parse_env_cfg(task_name, device=self.device, num_envs=self.num_envs) - # create environment - env = gym.make(task_name, cfg=env_cfg) - # convert to single-agent instance if required by the RL algorithm - if isinstance(env.unwrapped, DirectMARLEnv): - env = multi_agent_to_single_agent(env) - # wrap environment - env = Sb3VecEnvWrapper(env) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # reset environment - obs = env.reset() - # check signal - self.assertTrue(self._check_valid_array(obs)) - - # simulate environment for 100 steps - with torch.inference_mode(): - for _ in range(100): - # sample actions from -1 to 1 - actions = 2 * np.random.rand(env.num_envs, *env.action_space.shape) - 1 - # apply actions - transition = env.step(actions) - # check signals - for data in transition: - self.assertTrue(self._check_valid_array(data), msg=f"Invalid data: {data}") - - # close the environment - print(f">>> Closing environment: {task_name}") +@pytest.fixture(scope="module") +def registered_tasks(): + # acquire all Isaac environments names + registered_tasks = list() + for task_spec in gym.registry.values(): + if "Isaac" in task_spec.id: + cfg_entry_point = gym.spec(task_spec.id).kwargs.get("sb3_cfg_entry_point") + if cfg_entry_point is not None: + registered_tasks.append(task_spec.id) + # sort environments by name + registered_tasks.sort() + registered_tasks = registered_tasks[:5] + + # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + # print all existing task names + print(">>> All registered environments:", registered_tasks) + return registered_tasks + + +def test_random_actions(registered_tasks): + """Run random actions and check environments return valid signals.""" + # common parameters + num_envs = 64 + device = "cuda" + for task_name in registered_tasks: + # Use pytest's subtests + print(f">>> Running test for environment: {task_name}") + # create a new stage + omni.usd.get_context().new_stage() + # reset the rtx sensors carb setting to False + carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + try: + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # create environment + env = gym.make(task_name, cfg=env_cfg) + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + # wrap environment + env = Sb3VecEnvWrapper(env) + except Exception as e: + if "env" in locals() and hasattr(env, "_is_closed"): env.close() - - """ - Helper functions. + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") + + # reset environment + obs = env.reset() + # check signal + assert _check_valid_array(obs) + + # simulate environment for 100 steps + with torch.inference_mode(): + for _ in range(100): + # sample actions from -1 to 1 + actions = 2 * np.random.rand(env.num_envs, *env.action_space.shape) - 1 + # apply actions + transition = env.step(actions) + # check signals + for data in transition: + assert _check_valid_array(data), f"Invalid data: {data}" + + # close the environment + print(f">>> Closing environment: {task_name}") + env.close() + + +""" +Helper functions. +""" + + +@staticmethod +def _check_valid_array(data: np.ndarray | dict | list) -> bool: + """Checks if given data does not have corrupted values. + + Args: + data: Data buffer. + + Returns: + True if the data is valid. """ - - @staticmethod - def _check_valid_array(data: np.ndarray | dict | list) -> bool: - """Checks if given data does not have corrupted values. - - Args: - data: Data buffer. - - Returns: - True if the data is valid. - """ - if isinstance(data, np.ndarray): - return not np.any(np.isnan(data)) - elif isinstance(data, dict): - valid_array = True - for value in data.values(): - if isinstance(value, dict): - valid_array &= TestStableBaselines3VecEnvWrapper._check_valid_array(value) - elif isinstance(value, np.ndarray): - valid_array &= not np.any(np.isnan(value)) - return valid_array - elif isinstance(data, list): - valid_array = True - for value in data: - valid_array &= TestStableBaselines3VecEnvWrapper._check_valid_array(value) - return valid_array - else: - raise ValueError(f"Input data of invalid type: {type(data)}.") - - -if __name__ == "__main__": - run_tests() + if isinstance(data, np.ndarray): + return not np.any(np.isnan(data)) + elif isinstance(data, dict): + valid_array = True + for value in data.values(): + if isinstance(value, dict): + valid_array &= _check_valid_array(value) + elif isinstance(value, np.ndarray): + valid_array &= not np.any(np.isnan(value)) + return valid_array + elif isinstance(data, list): + valid_array = True + for value in data: + valid_array &= _check_valid_array(value) + return valid_array + else: + raise ValueError(f"Input data of invalid type: {type(data)}.") diff --git a/source/isaaclab_rl/test/test_skrl_wrapper.py b/source/isaaclab_rl/test/test_skrl_wrapper.py index eec816fc033e..25ce35a1c476 100644 --- a/source/isaaclab_rl/test/test_skrl_wrapper.py +++ b/source/isaaclab_rl/test/test_skrl_wrapper.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -15,8 +15,8 @@ """Rest everything follows.""" import gymnasium as gym +import pytest import torch -import unittest import carb import omni.usd @@ -29,114 +29,107 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestSKRLVecEnvWrapper(unittest.TestCase): - """Test that SKRL VecEnv wrapper works as expected.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id: - cfg_entry_point = gym.spec(task_spec.id).kwargs.get("skrl_cfg_entry_point") - if cfg_entry_point is not None: - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - cls.registered_tasks = cls.registered_tasks[:3] - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - # print all existing task names - print(">>> All registered environments:", cls.registered_tasks) - - def setUp(self) -> None: - # common parameters - self.num_envs = 64 - self.device = "cuda" - - def test_random_actions(self): - """Run random actions and check environments return valid signals.""" - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # create a new stage - omni.usd.get_context().new_stage() - # reset the rtx sensors carb setting to False - carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) - try: - # parse configuration - env_cfg = parse_env_cfg(task_name, device=self.device, num_envs=self.num_envs) - # create environment - env = gym.make(task_name, cfg=env_cfg) - if isinstance(env.unwrapped, DirectMARLEnv): - env = multi_agent_to_single_agent(env) - # wrap environment - env = SkrlVecEnvWrapper(env) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # avoid shutdown of process on simulation stop - env.unwrapped.sim._app_control_on_stop_handle = None - - # reset environment - obs, extras = env.reset() - # check signal - self.assertTrue(self._check_valid_tensor(obs)) - self.assertTrue(self._check_valid_tensor(extras)) - - # simulate environment for 100 steps - with torch.inference_mode(): - for _ in range(100): - # sample actions from -1 to 1 - actions = ( - 2 * torch.rand(self.num_envs, *env.action_space.shape, device=env.unwrapped.device) - 1 - ) - # apply actions - transition = env.step(actions) - # check signals - for data in transition: - self.assertTrue(self._check_valid_tensor(data), msg=f"Invalid data: {data}") - - # close the environment - print(f">>> Closing environment: {task_name}") +@pytest.fixture(scope="module") +def registered_tasks(): + # acquire all Isaac environments names + registered_tasks = list() + for task_spec in gym.registry.values(): + if "Isaac" in task_spec.id: + cfg_entry_point = gym.spec(task_spec.id).kwargs.get("skrl_cfg_entry_point") + if cfg_entry_point is not None: + registered_tasks.append(task_spec.id) + # sort environments by name + registered_tasks.sort() + registered_tasks = registered_tasks[:3] + + # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + # print all existing task names + print(">>> All registered environments:", registered_tasks) + return registered_tasks + + +def test_random_actions(registered_tasks): + """Run random actions and check environments return valid signals.""" + # common parameters + num_envs = 64 + device = "cuda" + for task_name in registered_tasks: + # Use pytest's subtests + print(f">>> Running test for environment: {task_name}") + # create a new stage + omni.usd.get_context().new_stage() + # reset the rtx sensors carb setting to False + carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + try: + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # create environment + env = gym.make(task_name, cfg=env_cfg) + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + # wrap environment + env = SkrlVecEnvWrapper(env) + except Exception as e: + if "env" in locals() and hasattr(env, "_is_closed"): env.close() - - """ - Helper functions. + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") + + # avoid shutdown of process on simulation stop + env.unwrapped.sim._app_control_on_stop_handle = None + + # reset environment + obs, extras = env.reset() + # check signal + assert _check_valid_tensor(obs) + assert _check_valid_tensor(extras) + + # simulate environment for 100 steps + with torch.inference_mode(): + for _ in range(100): + # sample actions from -1 to 1 + actions = 2 * torch.rand(num_envs, *env.action_space.shape, device=env.unwrapped.device) - 1 + # apply actions + transition = env.step(actions) + # check signals + for data in transition: + assert _check_valid_tensor(data), f"Invalid data: {data}" + + # close the environment + print(f">>> Closing environment: {task_name}") + env.close() + + +""" +Helper functions. +""" + + +@staticmethod +def _check_valid_tensor(data: torch.Tensor | dict) -> bool: + """Checks if given data does not have corrupted values. + + Args: + data: Data buffer. + + Returns: + True if the data is valid. """ - - @staticmethod - def _check_valid_tensor(data: torch.Tensor | dict) -> bool: - """Checks if given data does not have corrupted values. - - Args: - data: Data buffer. - - Returns: - True if the data is valid. - """ - if isinstance(data, torch.Tensor): - return not torch.any(torch.isnan(data)) - elif isinstance(data, dict): - valid_tensor = True - for value in data.values(): - if isinstance(value, dict): - valid_tensor &= TestSKRLVecEnvWrapper._check_valid_tensor(value) - elif isinstance(value, torch.Tensor): - valid_tensor &= not torch.any(torch.isnan(value)) - return valid_tensor - else: - raise ValueError(f"Input data of invalid type: {type(data)}.") - - -if __name__ == "__main__": - run_tests() + if isinstance(data, torch.Tensor): + return not torch.any(torch.isnan(data)) + elif isinstance(data, dict): + valid_tensor = True + for value in data.values(): + if isinstance(value, dict): + valid_tensor &= _check_valid_tensor(value) + elif isinstance(value, torch.Tensor): + valid_tensor &= not torch.any(torch.isnan(value)) + return valid_tensor + else: + raise ValueError(f"Input data of invalid type: {type(data)}.") diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index e74a43d977c8..ea211a93e2a3 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.26" +version = "0.11.12" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 973a77ae2eb8..f9100882c5d5 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,7 +1,367 @@ Changelog --------- -0.10.26 (2025-03-18) +0.11.12 (2025-12-16) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``Isaac-Deploy-GearAssembly`` environments. + + +0.11.11 (2025-12-16) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added reaching task environments for OpenArm unimanual robot: + * :class:`OpenArmReachEnvCfg`; Gym ID ``Isaac-Reach-OpenArm-v0``. + * :class:`OpenArmReachEnvCfg_PLAY`; Gym ID ``Isaac-Reach-OpenArm-Play-v0``. +* Added lifting a cube task environments for OpenArm unimanual robot: + * :class:`OpenArmCubeLiftEnvCfg`; Gym ID ``Isaac-Lift-Cube-OpenArm-v0``. + * :class:`OpenArmCubeLiftEnvCfg_PLAY`; Gym ID ``Isaac-Lift-Cube-OpenArm-Play-v0``. +* Added opening a drawer task environments for OpenArm unimanual robot: + * :class:`OpenArmCabinetEnvCfg`; Gym ID ``Isaac-Open-Drawer-OpenArm-v0``. + * :class:`OpenArmCabinetEnvCfg_PLAY`; Gym ID ``Isaac-Open-Drawer-OpenArm-Play-v0``. +* Added reaching task environments for OpenArm bimanual robot: + * :class:`OpenArmReachEnvCfg`; Gym ID ``Isaac-Reach-OpenArm-Bi-v0``. + * :class:`OpenArmReachEnvCfg_PLAY`; Gym ID ``Isaac-Reach-OpenArm-Bi-Play-v0``. + + +0.11.10 (2025-12-13) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added obs_groups to the RSL-RL PPO agent configuration for the ``Isaac-Reach-UR10e-v0`` environment. +* Changed self.state_space to 19 in the ``Isaac-Reach-UR10e-ROS-Inference-v0`` environment. + + +0.11.9 (2025-11-10) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added OpenXR motion controller support for the G1 robot locomanipulation environment + ``Isaac-PickPlace-Locomanipulation-G1-Abs-v0``. This enables teleoperation using XR motion controllers + in addition to hand tracking. +* Added :class:`OpenXRDeviceMotionController` for motion controller-based teleoperation with headset anchoring control. +* Added motion controller-specific retargeters: + * :class:`G1TriHandControllerUpperBodyRetargeterCfg` for upper body and hand control using motion controllers. + * :class:`G1LowerBodyStandingControllerRetargeterCfg` for lower body control using motion controllers. + + +0.11.8 (2025-11-06) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed to use of ``num_rerenders_on_reset`` and ``DLAA`` in visuomotor imitation learning environments. + + +0.11.7 (2025-10-22) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Ensured all imports follows the string import style instead of direct import of environment. + + +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) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added SkillGen-specific cube stacking environments: + * :class:`FrankaCubeStackSkillgenEnvCfg`; Gym ID ``Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0``. +* Added bin cube stacking environment for SkillGen/Mimic: + * :class:`FrankaBinStackEnvCfg`; Gym ID ``Isaac-Stack-Cube-Bin-Franka-IK-Rel-Mimic-v0``. + + +0.10.50 (2025-09-05) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added stacking environments for Galbot with suction grippers. + + +0.10.49 (2025-09-05) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added suction gripper stacking environments with UR10 that can be used with teleoperation. + + +0.10.48 (2025-09-03) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``Isaac-Deploy-Reach-UR10e-v0`` environment. + + +0.10.47 (2025-07-25) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* New ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0`` environment that enables the waist degrees-of-freedom for the GR1T2 robot. + + +Changed +^^^^^^^ + +* Updated pink inverse kinematics controller configuration for the following tasks (``Isaac-PickPlace-GR1T2``, ``Isaac-NutPour-GR1T2``, ``Isaac-ExhaustPipe-GR1T2``) + to increase end-effector tracking accuracy and speed. Also added a null-space regularizer that enables turning on of waist degrees-of-freedom without + the robot control drifting to a bending posture. +* Tuned the pink inverse kinematics controller and joint PD controllers for the following tasks (``Isaac-PickPlace-GR1T2``, ``Isaac-NutPour-GR1T2``, ``Isaac-ExhaustPipe-GR1T2``) + to improve the end-effector tracking accuracy and speed. Achieving position and orientation accuracy test within **(2 mm, 1 degree)**. + + +0.10.46 (2025-08-16) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added symmetry data augmentation example with RSL-RL for cartpole and anymal locomotion environments. +* Added :attr:`--agent` to RL workflow scripts to allow switching between different configurations. + + +0.10.45 (2025-07-16) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``from __future__ import annotations`` to isaaclab_tasks files to fix Sphinx + doc warnings for IsaacLab Mimic docs. + + +0.10.44 (2025-07-16) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``Isaac-Forge-PegInsert-Direct-v0``, ``Isaac-Forge-GearMesh-Direct-v0``, + and ``Isaac-Forge-NutThread-Direct-v0`` environments as direct RL envs. These + environments extend ``Isaac-Factory-*-v0`` with force sensing, an excessive force + penalty, dynamics randomization, and success prediction. + + +0.10.43 (2025-07-24) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed un-set camera observations in the ``Isaac-Stack-Cube-Instance-Randomize-Franka-v0`` environment. + + +0.10.42 (2025-07-11) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Organized environment unit tests + + +0.10.41 (2025-07-01) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed the rendering settings used for the Mimic-Cosmos pipeline. + + +0.10.40 (2025-06-26) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Relaxed upper range pin for protobuf python dependency for more permissive installation. + + +0.10.39 (2025-05-22) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed redundant body_names assignment in rough_env_cfg.py for H1 robot. + + +0.10.38 (2025-06-16) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Show available RL library configs on error message when an entry point key is not available for a given task. + + +0.10.37 (2025-05-15) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``Isaac-Assembly-Direct-v0`` environment as a direct RL env that + implements assembly tasks to insert pegs into their corresponding sockets. + + +0.10.36 (2025-05-21) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added unit tests for benchmarking environments with configurable settings. Output KPI payloads + can be pushed to a visualization dashboard to track improvements or regressions. + + +0.10.35 (2025-05-21) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-v0`` stacking environment with multi-modality camera inputs at higher resolution. + +Changed +^^^^^^^ + +* Updated the ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0`` stacking environment to support visual domain randomization events during model evaluation. +* Made the task termination condition for the stacking task more strict. + + +0.10.34 (2025-05-22) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed ``Isaac-PickPlace-GR1T2-Abs-v0`` object asset to a steering wheel. + + +0.10.33 (2025-05-12) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Increase ``Isaac-PickPlace-GR1T2-Abs-v0`` sim dt to 120Hz for improved stability. +* Fix object initial state in ``Isaac-PickPlace-GR1T2-Abs-v0`` to be above the table. + + +0.10.32 (2025-05-01) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added new GR1 tasks (``Isaac-NutPour-GR1T2-Pink-IK-Abs-v0``, and ``Isaac-ExhaustPipe-GR1T2-Pink-IK-Abs-v0``). + + +0.10.31 (2025-04-02) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Adds an idle action parameter to the ``Isaac-PickPlace-GR1T2-Abs-v0`` environment configuration. + + +0.10.30 (2025-03-25) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed environment test failure for ``Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0``. + + +0.10.29 (2025-03-18) ~~~~~~~~~~~~~~~~~~~~ Added @@ -10,12 +370,41 @@ Added * Added Gymnasium spaces showcase tasks (``Isaac-Cartpole-Showcase-*-Direct-v0``, and ``Isaac-Cartpole-Camera-Showcase-*-Direct-v0``). -0.10.25 (2025-03-10) +0.10.28 (2025-03-19) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated the ``Isaac-PickPlace-GR1T2-Abs-v0`` environment with auto termination when the object falls off the table + and refined the success criteria to be more accurate. + + +0.10.27 (2025-03-13) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Blacklisted pick_place task from being imported automatically by isaaclab_tasks. It now has to be imported + manually by the script due to dependencies on the pinocchio import. + + +0.10.26 (2025-03-10) ~~~~~~~~~~~~~~~~~~~~ Added ^^^^^ +* Added the ``Isaac-PickPlace-GR1T2-Abs-v0`` environment that implements a humanoid arm picking and placing a steering wheel task using the PinkIKController. + + +0.10.25 (2025-03-06) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^^^ + * Added ``Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0`` stacking environment with camera inputs. diff --git a/source/isaaclab_tasks/isaaclab_tasks/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/__init__.py index 7de87f5389a0..60b7f852ef26 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/__init__.py @@ -1,9 +1,17 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -"""Package containing task implementations for various robotic environments.""" +"""Package containing task implementations for various robotic environments. + +The package is structured as follows: + +- ``direct``: These include single-file implementations of tasks. +- ``manager_based``: These include task implementations that use the manager-based API. +- ``utils``: These include utility functions for the tasks. + +""" import os import toml @@ -25,6 +33,7 @@ from .utils import import_packages # The blacklist is used to prevent importing configs from sub-packages -_BLACKLIST_PKGS = ["utils", ".mdp"] +# TODO(@ashwinvk): Remove pick_place from the blacklist once pinocchio from Isaac Sim is compatibility +_BLACKLIST_PKGS = ["utils", ".mdp", "pick_place", "direct.humanoid_amp.motions"] # Import all configs in this package import_packages(__name__, _BLACKLIST_PKGS) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/__init__.py index 13e537090546..3e2b7945ebde 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/__init__.py index 88d96cd2d00a..636ad3661659 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rl_games_ppo_cfg.yaml index df7e8b4f7151..36d441d26dd8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py index 67c3f5b2aea8..871250fd0b17 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class AllegroHandPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 10000 save_interval = 250 experiment_name = "allegro_hand" - empirical_normalization = True policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, actor_hidden_dims=[1024, 512, 256, 128], critic_hidden_dims=[1024, 512, 256, 128], activation="elu", 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 ca8c6299351d..de44a576eb98 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,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 @@ -23,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/allegro_hand/allegro_hand_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/allegro_hand_env_cfg.py index 82f81ad44adf..75087bbe0199 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/allegro_hand_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/allegro_hand_env_cfg.py @@ -1,11 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -from 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 @@ -16,6 +14,8 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab_assets.robots.allegro import ALLEGRO_HAND_CFG + @configclass class AllegroHandEnvCfg(DirectRLEnvCfg): @@ -98,7 +98,9 @@ class AllegroHandEnvCfg(DirectRLEnvCfg): }, ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=8192, env_spacing=0.75, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=8192, env_spacing=0.75, replicate_physics=True, clone_in_fabric=True + ) # reset reset_position_noise = 0.01 # range of position at reset reset_dof_pos_noise = 0.2 # range of dof pos at reset diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/__init__.py index 10a9225035de..9881cd66ca74 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rl_games_ppo_cfg.yaml index 7c1c514c2981..a8c24a530962 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py index c76d1991ef88..00eefc843e20 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class AntPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1000 save_interval = 50 experiment_name = "ant_direct" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[400, 200, 100], critic_hidden_dims=[400, 200, 100], activation="elu", 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 5ebf90cb5a4d..bb7382d4a2b8 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -23,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/ant/ant_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/ant_env.py index 166ad3c3437e..39ae57b29677 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/ant_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/ant_env.py @@ -1,12 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -from isaaclab_assets.robots.ant import ANT_CFG - import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg @@ -17,6 +15,8 @@ from isaaclab_tasks.direct.locomotion.locomotion_env import LocomotionEnv +from isaaclab_assets.robots.ant import ANT_CFG + @configclass class AntEnvCfg(DirectRLEnvCfg): @@ -45,7 +45,9 @@ class AntEnvCfg(DirectRLEnvCfg): ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=4096, env_spacing=4.0, replicate_physics=True, clone_in_fabric=True + ) # robot robot: ArticulationCfg = ANT_CFG.replace(prim_path="/World/envs/env_.*/Robot") diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/__init__.py index 790a5d04eb84..26275c97fff9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_flat_ppo_cfg.yaml index 4bb7435f0949..6e8fc0d4ca91 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_flat_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_rough_ppo_cfg.yaml index 1995015eb79a..ef2670326e20 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rl_games_rough_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py index e581763032f9..117ad6e75bed 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class AnymalCFlatPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 500 save_interval = 50 experiment_name = "anymal_c_flat_direct" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[128, 128, 128], critic_hidden_dims=[128, 128, 128], activation="elu", @@ -43,9 +44,10 @@ class AnymalCRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_c_rough_direct" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", 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 aa5828e76868..33bc471a4778 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 1c72b8fb13df..c31294b7fa86 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/anymal_c/anymal_c_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env.py index 1701c176f556..2f7e792f93ad 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -66,6 +66,9 @@ def _setup_scene(self): 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 lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) light_cfg.func("/World/Light", light_cfg) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py index 24bed6353f19..f5e12b599122 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -138,7 +138,7 @@ class AnymalCRoughEnvCfg(AnymalCFlatEnvCfg): height_scanner = RayCasterCfg( prim_path="/World/envs/env_.*/Robot/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=False, mesh_prim_paths=["/World/ground"], diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/__init__.py new file mode 100644 index 000000000000..9da64598c0c4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/__init__.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-AutoMate-Assembly-Direct-v0", + entry_point=f"{__name__}.assembly_env:AssemblyEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.assembly_env:AssemblyEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-AutoMate-Disassembly-Direct-v0", + entry_point=f"{__name__}.disassembly_env:DisassemblyEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.disassembly_env:DisassemblyEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 000000000000..3d8d070248cf --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,105 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py new file mode 100644 index 000000000000..35e999120380 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py @@ -0,0 +1,877 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import json +import os + +import numpy as np +import torch +import warp as wp + +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 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) + 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() + + 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(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.""" + 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) + + # 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 + + 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) + ) + + 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 + 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py new file mode 100644 index 000000000000..5d4f66ec8969 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py @@ -0,0 +1,200 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +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.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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py new file mode 100644 index 000000000000..b651767f7f35 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py @@ -0,0 +1,270 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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" + + +@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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_algo_utils.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_algo_utils.py new file mode 100644 index 000000000000..4588d4e227fd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_algo_utils.py @@ -0,0 +1,314 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import re +import subprocess +import sys + +import torch +import trimesh +import warp as wp + +print("Python Executable:", sys.executable) +print("Python Path:", sys.path) + +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 + +""" +Util Functions +""" + + +def parse_cuda_version(version_string): + """ + Parse CUDA version string into comparable tuple of (major, minor, patch). + + Args: + version_string: Version string like "12.8.9" or "11.2" + + Returns: + Tuple of (major, minor, patch) as integers, where patch defaults to 0 iff + not present. + + Example: + "12.8.9" -> (12, 8, 9) + "11.2" -> (11, 2, 0) + """ + 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): + 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_log_utils.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_log_utils.py new file mode 100644 index 000000000000..f46fcb3479bc --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_log_utils.py @@ -0,0 +1,26 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py new file mode 100644 index 000000000000..a4b454829eac --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py @@ -0,0 +1,872 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import json +import os + +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, 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() + + # 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) + self.actions *= 0.0 + 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) + 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.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) + + print(f"Trajectory collection complete! Collected {len(self.log_arm_dof_pos)} trajectories!") + exit(0) + else: + print( + f"Collected {len(self.log_arm_dof_pos)} trajectories so far (target: > {self.cfg_task.num_log_traj})." + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py new file mode 100644 index 000000000000..9d17f3597587 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py @@ -0,0 +1,196 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +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.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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py new file mode 100644 index 000000000000..d21bd0166e5c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py @@ -0,0 +1,220 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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 = "00015" + 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/factory_control.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/factory_control.py new file mode 100644 index 000000000000..0e51b6e41f6c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/factory_control.py @@ -0,0 +1,196 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 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 + else: + raise ValueError(f"Unsupported rotation error type: {rot_error_type}. Valid: 'quat', 'axis_angle'.") + + +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) # noqa: E501 + + 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(min_singular_value < S, 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/industreal_algo_utils.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/industreal_algo_utils.py new file mode 100644 index 000000000000..c324eb46f46f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/industreal_algo_utils.py @@ -0,0 +1,378 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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 +- Sampling-Based Curriculum (SBC) + +Not intended to be executed as a standalone script. +""" + +# Force garbage collection for large arrays +import gc +import os + +import numpy as np + +# from pysdf import SDF +import torch +import trimesh + +# from urdfpy import URDF +import warp as wp +from trimesh.exchange.load import load + +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) + + del mesh_copy + del mesh_points + del mesh_indices + del sampled_points + + sdf_reward = -torch.log(sdf_reward) + + gc.collect() # Force garbage collection to free memory + 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_disassembly_w_id.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_disassembly_w_id.py new file mode 100644 index 000000000000..89c8a39650bd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_disassembly_w_id.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import argparse +import os +import re +import subprocess +import sys + + +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, + ) + + if sys.platform.startswith("win"): + bash_command = "isaaclab.bat -p" + elif sys.platform.startswith("linux"): + bash_command = "./isaaclab.sh -p" + + bash_command += " 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py new file mode 100644 index 000000000000..18e8914e6704 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py @@ -0,0 +1,89 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import argparse +import re +import subprocess +import sys + + +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.") + parser.add_argument("--max_iterations", type=int, default=1500, help="Number of iteration for policy learning.") + args = parser.parse_args() + + update_task_param(args.cfg_path, args.assembly_id, args.train, args.log_eval) + + # 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" + elif sys.platform.startswith("linux"): + 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)}" + else: + if not args.checkpoint: + raise ValueError("No checkpoint provided for evaluation.") + bash_command += " 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/source/isaaclab_tasks/isaaclab_tasks/direct/automate/soft_dtw_cuda.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/soft_dtw_cuda.py new file mode 100644 index 000000000000..a979ec449381 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/soft_dtw_cuda.py @@ -0,0 +1,451 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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 numba import cuda, jit, prange +from torch.autograd import Function + + +# ---------------------------------------------------------------------------------------------------------------------- +@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 max_j > 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 max_j > 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, device, gamma, bandwidth): + dev = D.device + dtype = D.dtype + 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] + 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, device, 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, device, gamma=1.0, normalize=False, bandwidth=None, dist_func=None): + """Initializes a new instance using the supplied parameters + + Args: + + use_cuda: Whether to use the CUDA implementation. + device: The device to run the SoftDTW computation. + gamma: The SoftDTW's gamma parameter. Default is 1.0. + normalize: Whether to perform normalization. Default is False. + (as discussed in https://github.com/mblondel/soft-dtw/issues/10#issuecomment-383564790) + bandwidth: Sakoe-Chiba bandwidth for pruning. Default is None, which disables pruning. + If provided, must be a float. + dist_func: The point-wise distance function to use. Default is None, which + uses a default Euclidean distance function. + """ + super().__init__() + self.normalize = normalize + 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: + 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.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.device, 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( + f"Profiling forward() + backward() times for batch_size={batch_size}, seq_len_a={seq_len_a}," + f" seq_len_b={seq_len_b}, dims={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) + + # Ignore the first time we run, in case this is a cold start + # (because timings are off at a cold start of the script) + if i > 0: + 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/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/__init__.py index c79040f24df9..b4913fb2c3a9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/rl_games_ppo_cfg.yaml index 6c2a3ad84734..2e97a86a62ff 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 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 bc0c51821792..f9298c9252ac 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,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 dcd794f57a5c..8f192cf2988d 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,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 7c1fd452d707..c5f7943b99d9 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,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/cart_double_pendulum_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py index 5d956e5c0738..e0464a7201c8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,10 +6,9 @@ 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 torch import isaaclab.sim as sim_utils from isaaclab.assets import Articulation, ArticulationCfg @@ -20,6 +19,8 @@ from isaaclab.utils import configclass from isaaclab.utils.math import sample_uniform +from isaaclab_assets.robots.cart_double_pendulum import CART_DOUBLE_PENDULUM_CFG + @configclass class CartDoublePendulumEnvCfg(DirectMARLEnvCfg): @@ -82,6 +83,9 @@ def _setup_scene(self): spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) # 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=[]) # add articulation to scene self.scene.articulations["robot"] = self.robot # add lights diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/__init__.py index a9ee28fbd9f9..f72ee0a6f8dc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_camera_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_camera_ppo_cfg.yaml index ee5ac79b437d..60c37b40476d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_camera_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_camera_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_ppo_cfg.yaml index e78116e70cef..a673a29257ea 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py index dec441a35874..097b7b43a672 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 150 save_interval = 50 experiment_name = "cartpole_direct" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[32, 32], critic_hidden_dims=[32, 32], activation="elu", diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/sb3_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/sb3_ppo_cfg.yaml index 5856f35f8e87..fcb32cd51dd9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/sb3_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/sb3_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 seed: 42 @@ -11,11 +16,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/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 3e2e19584230..282ebb0020c5 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,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} @@ -31,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 1efe67083a5d..b50b4bea69bb 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,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/cartpole_camera_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_env.py index c43f23ddab8a..9606008ccf19 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,10 +6,9 @@ from __future__ import annotations import math -import torch from collections.abc import Sequence -from isaaclab_assets.robots.cartpole import CARTPOLE_CFG +import torch import isaaclab.sim as sim_utils from isaaclab.assets import Articulation, ArticulationCfg @@ -20,6 +19,8 @@ from isaaclab.utils import configclass from isaaclab.utils.math import sample_uniform +from isaaclab_assets.robots.cartpole import CARTPOLE_CFG + @configclass class CartpoleRGBCameraEnvCfg(DirectRLEnvCfg): @@ -58,7 +59,7 @@ class CartpoleRGBCameraEnvCfg(DirectRLEnvCfg): viewer = ViewerCfg(eye=(20.0, 20.0, 20.0)) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1024, env_spacing=20.0, replicate_physics=True) + 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] @@ -91,6 +92,7 @@ class CartpoleDepthCameraEnvCfg(CartpoleRGBCameraEnvCfg): class CartpoleCameraEnv(DirectRLEnv): + """Cartpole Camera Environment.""" cfg: CartpoleRGBCameraEnvCfg | CartpoleDepthCameraEnvCfg @@ -123,6 +125,9 @@ def _setup_scene(self): # clone and replicate self.scene.clone_environments(copy_from_source=False) + if self.device == "cpu": + # we need to explicitly filter collisions for CPU simulation + self.scene.filter_collisions(global_prim_paths=[]) # add articulation and sensors to scene self.scene.articulations["cartpole"] = self._cartpole diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env.py index a52dab45b8a7..f897b64f3ec9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,10 +6,9 @@ from __future__ import annotations import math -import torch from collections.abc import Sequence -from isaaclab_assets.robots.cartpole import CARTPOLE_CFG +import torch import isaaclab.sim as sim_utils from isaaclab.assets import Articulation, ArticulationCfg @@ -20,6 +19,8 @@ from isaaclab.utils import configclass from isaaclab.utils.math import sample_uniform +from isaaclab_assets.robots.cartpole import CARTPOLE_CFG + @configclass class CartpoleEnvCfg(DirectRLEnvCfg): @@ -40,7 +41,9 @@ class CartpoleEnvCfg(DirectRLEnvCfg): pole_dof_name = "cart_to_pole" # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=4096, env_spacing=4.0, replicate_physics=True, clone_in_fabric=True + ) # reset max_cart_pos = 3.0 # the cart is reset if it exceeds that position [m] @@ -73,6 +76,9 @@ def _setup_scene(self): spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) # 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=[]) # add articulation to scene self.scene.articulations["cartpole"] = self.cartpole # add lights diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/__init__.py index 02694c2b6551..d401c413967d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/__init__.py index 74103f188b92..576ccc822edf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_box_ppo_cfg.yaml index a616b88054b2..08d0e729708a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_discrete_ppo_cfg.yaml index 26dbdc6190cc..4d7852d6665c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_multidiscrete_ppo_cfg.yaml index 5f2dc924f3f6..47a764f6117d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_box_ppo_cfg.yaml index af34a7442640..7153e46bcf13 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_discrete_ppo_cfg.yaml index c3cab386c86d..67bb6b932dcf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_multidiscrete_ppo_cfg.yaml index 63d0f930569a..d51d71764315 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_box_ppo_cfg.yaml index d8baac3ec093..f55aa1f21c67 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_discrete_ppo_cfg.yaml index 1994df77c591..ae513a501835 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_multidiscrete_ppo_cfg.yaml index 12613e0fc6d0..7310ed646ca4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_box_ppo_cfg.yaml index c02301ac341b..1ed10841aa02 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_discrete_ppo_cfg.yaml index c3b22efe1c5b..5ed49c9f6692 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_multidiscrete_ppo_cfg.yaml index 552f58bda26a..ad95dab1ba63 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_box_ppo_cfg.yaml index 1213276ed4e0..291610cc73d3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_discrete_ppo_cfg.yaml index 90faeda183f7..cd48d89491ed 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml index 03ef59f96ca6..84ba7d6506be 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env.py index ebaa892ed08c..dc03eb299d0d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -42,7 +42,6 @@ def _apply_action(self) -> None: 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): diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env_cfg.py index 1d0af880a09e..e6e8169b1ba0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -421,10 +421,12 @@ class DictBoxEnvCfg(CartpoleEnvCfg): === === """ - 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} + 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] @@ -451,10 +453,12 @@ class DictDiscreteEnvCfg(CartpoleEnvCfg): === === """ - 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} + 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} @@ -488,10 +492,12 @@ class DictMultiDiscreteEnvCfg(CartpoleEnvCfg): === === """ - 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} + 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}] @@ -521,10 +527,12 @@ class TupleBoxEnvCfg(CartpoleEnvCfg): === === """ - 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) + 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] @@ -551,10 +559,12 @@ class TupleDiscreteEnvCfg(CartpoleEnvCfg): === === """ - 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) + 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} @@ -588,8 +598,10 @@ class TupleMultiDiscreteEnvCfg(CartpoleEnvCfg): === === """ - 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) + 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/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/__init__.py index f1c7e0ba40da..2953ce1a29c0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_box_ppo_cfg.yaml index 082c149b4cf5..77bb2c8eab50 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_discrete_ppo_cfg.yaml index 4aec971cc56d..ac047e6b9361 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_multidiscrete_ppo_cfg.yaml index d633ea140114..8710230931d0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_box_ppo_cfg.yaml index 89b88c175d22..09f598574135 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_discrete_ppo_cfg.yaml index 68be1bf352b8..92a5c1f52b4e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_multidiscrete_ppo_cfg.yaml index 77aa2cb78d3e..2dfd9f889eaf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_box_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_box_ppo_cfg.yaml index d1d5109c9773..423f17203ccc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_box_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_box_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_discrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_discrete_ppo_cfg.yaml index eb46117a0a17..f5aafefa9060 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_discrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_discrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml index 9e1b82a08a4e..5e3637aeb7b2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env.py index 5f5b4452d69c..1c8dc6b4a078 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env_cfg.py index 134739734e1a..5e146041b79b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -163,10 +163,14 @@ class DictBoxEnvCfg(CartpoleCameraEnvCfg): 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]} + 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] @@ -197,10 +201,14 @@ class DictDiscreteEnvCfg(CartpoleCameraEnvCfg): 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]} + 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} @@ -238,10 +246,14 @@ class DictMultiDiscreteEnvCfg(CartpoleCameraEnvCfg): 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]} + 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}] @@ -275,10 +287,12 @@ class TupleBoxEnvCfg(CartpoleCameraEnvCfg): 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) + 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] @@ -309,10 +323,12 @@ class TupleDiscreteEnvCfg(CartpoleCameraEnvCfg): 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) + 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} @@ -350,8 +366,10 @@ class TupleMultiDiscreteEnvCfg(CartpoleCameraEnvCfg): 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) + 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/source/isaaclab_tasks/isaaclab_tasks/direct/factory/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/__init__.py index 40fbde044731..54d69f31f679 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,8 +6,6 @@ import gymnasium as gym from . import agents -from .factory_env import FactoryEnv -from .factory_env_cfg import FactoryTaskGearMeshCfg, FactoryTaskNutThreadCfg, FactoryTaskPegInsertCfg ## # Register Gym environments. @@ -15,30 +13,30 @@ gym.register( id="Isaac-Factory-PegInsert-Direct-v0", - entry_point="isaaclab_tasks.direct.factory:FactoryEnv", + entry_point=f"{__name__}.factory_env:FactoryEnv", disable_env_checker=True, kwargs={ - "env_cfg_entry_point": FactoryTaskPegInsertCfg, + "env_cfg_entry_point": f"{__name__}.factory_env_cfg: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", + entry_point=f"{__name__}.factory_env:FactoryEnv", disable_env_checker=True, kwargs={ - "env_cfg_entry_point": FactoryTaskGearMeshCfg, + "env_cfg_entry_point": f"{__name__}.factory_env_cfg: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", + entry_point=f"{__name__}.factory_env:FactoryEnv", disable_env_checker=True, kwargs={ - "env_cfg_entry_point": FactoryTaskNutThreadCfg, + "env_cfg_entry_point": f"{__name__}.factory_env_cfg:FactoryTaskNutThreadCfg", "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/rl_games_ppo_cfg.yaml index 5494199846bd..f11f6d996745 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 0 algo: diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py index e4745fc968ef..f8ccb0e13451 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,6 +9,7 @@ """ import math + import torch import isaacsim.core.utils.torch as torch_utils @@ -31,6 +32,7 @@ def compute_dof_torque( task_prop_gains, task_deriv_gains, device, + dead_zone_thresholds=None, ): """Compute Franka DOF torque to move fingertips towards target pose.""" # References: @@ -61,12 +63,20 @@ def compute_dof_torque( ) task_wrench += task_wrench_motion + # Offset task_wrench motion by random amount to simulate unreliability at low forces. + # Check if absolute value is less than specified amount. If so, 0 out, otherwise, subtract. + if dead_zone_thresholds is not None: + task_wrench = torch.where( + task_wrench.abs() < dead_zone_thresholds, + torch.zeros_like(task_wrench), + task_wrench.sign() * (task_wrench.abs() - dead_zone_thresholds), + ) + # 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 + # adapted from roboticsproceedings.org/rss07/p31.pdf # useful tensors arm_mass_matrix_inv = torch.inverse(arm_mass_matrix) @@ -118,9 +128,7 @@ def get_pose_error( fingertip_midpoint_quat_norm = torch_utils.quat_mul( fingertip_midpoint_quat, torch_utils.quat_conjugate(fingertip_midpoint_quat) - )[ - :, 0 - ] # scalar component + )[:, 0] # scalar component fingertip_midpoint_quat_inv = torch_utils.quat_conjugate( fingertip_midpoint_quat ) / fingertip_midpoint_quat_norm.unsqueeze(-1) @@ -133,13 +141,15 @@ def get_pose_error( return pos_error, quat_error elif rot_error_type == "axis_angle": return pos_error, axis_angle_error + else: + raise ValueError(f"Unsupported rotation error type: {rot_error_type}. Valid: 'quat', 'axis_angle'.") -def _get_delta_dof_pos(delta_pose, ik_method, jacobian, device): +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) + # 2) https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf (p. 47) # noqa: E501 if ik_method == "pinv": # Jacobian pseudoinverse k_val = 1.0 @@ -165,7 +175,7 @@ def _get_delta_dof_pos(delta_pose, ik_method, jacobian, device): 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)) + S_inv = torch.where(min_singular_value < S, 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) ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index 1d861d5e8767..a4e9c6d9ece9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -16,7 +16,7 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.math import axis_angle_from_quat -from . import factory_control as fc +from . import factory_control, factory_utils from .factory_env_cfg import OBS_DIM_CFG, STATE_DIM_CFG, FactoryEnvCfg @@ -33,18 +33,9 @@ def __init__(self, cfg: FactoryEnvCfg, render_mode: str | None = None, **kwargs) super().__init__(cfg, render_mode, **kwargs) - self._set_body_inertias() + factory_utils.set_body_inertias(self._robot, self.scene.num_envs) 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.""" @@ -60,55 +51,21 @@ def _set_default_dynamics_parameters(self): ) # 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) + factory_utils.set_friction(self._held_asset, self.cfg_task.held_asset_cfg.friction, self.scene.num_envs) + factory_utils.set_friction(self._fixed_asset, self.cfg_task.fixed_asset_cfg.friction, self.scene.num_envs) + factory_utils.set_friction(self._robot, self.cfg_task.robot_cfg.friction, self.scene.num_envs) 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) + self.ema_factor = self.cfg.ctrl.ema_factor + self.dead_zone_thresholds = None # 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") @@ -117,44 +74,14 @@ def _init_tensors(self): # 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_fingertip_quat = ( + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + ) 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)) @@ -173,6 +100,9 @@ def _setup_scene(self): self._large_gear_asset = Articulation(self.cfg_task.large_gear_cfg) self.scene.clone_environments(copy_from_source=False) + if self.device == "cpu": + # we need to explicitly filter collisions for CPU simulation + self.scene.filter_collisions() self.scene.articulations["robot"] = self._robot self.scene.articulations["fixed_asset"] = self._fixed_asset @@ -225,31 +155,10 @@ def _compute_intermediate_values(self, dt): 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.""" + def _get_factory_obs_state_dict(self): + """Populate dictionaries for the policy and critic.""" noisy_fixed_pos = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise prev_actions = self.actions.clone() @@ -280,15 +189,20 @@ def _get_observations(self): "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 obs_dict, state_dict + + def _get_observations(self): + """Get actor/critic inputs using asymmetric critic.""" + obs_dict, state_dict = self._get_factory_obs_state_dict() + + obs_tensors = factory_utils.collapse_obs_dict(obs_dict, self.cfg.obs_order + ["prev_actions"]) + state_tensors = factory_utils.collapse_obs_dict(state_dict, self.cfg.state_order + ["prev_actions"]) return {"policy": obs_tensors, "critic": state_tensors} def _reset_buffers(self, env_ids): """Reset buffers.""" self.ep_succeeded[env_ids] = 0 + self.ep_success_times[env_ids] = 0 def _pre_physics_step(self, action): """Apply policy actions with smoothing.""" @@ -296,18 +210,15 @@ def _pre_physics_step(self, action): 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 - ) + self.actions = self.ema_factor * action.clone().to(self.device) + (1 - self.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 + ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_actions # Interpret actions as target rot (axis-angle) displacements rot_actions = actions[:, 3:6] @@ -323,25 +234,24 @@ def close_gripper_in_place(self): 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) + 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 = torch.stack(torch_utils.get_euler_xyz(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( + 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() + self.generate_ctrl_signals( + ctrl_target_fingertip_midpoint_pos=ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=ctrl_target_fingertip_midpoint_quat, + ctrl_target_gripper_dof_pos=0.0, + ) 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: @@ -356,13 +266,14 @@ def _apply_action(self): 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 + 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 + fixed_pos_action_frame = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + delta_pos = ctrl_target_fingertip_midpoint_pos - 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 + ctrl_target_fingertip_midpoint_pos = fixed_pos_action_frame + pos_error_clipped # Convert to quat and set rot target angle = torch.norm(rot_actions, p=2, dim=-1) @@ -374,53 +285,57 @@ def _apply_action(self): 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) + 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 = torch.stack(torch_utils.get_euler_xyz(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( + 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 + self.generate_ctrl_signals( + ctrl_target_fingertip_midpoint_pos=ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=ctrl_target_fingertip_midpoint_quat, + ctrl_target_gripper_dof_pos=0.0, + ) - def generate_ctrl_signals(self): + def generate_ctrl_signals( + self, ctrl_target_fingertip_midpoint_pos, ctrl_target_fingertip_midpoint_quat, ctrl_target_gripper_dof_pos + ): """Get Jacobian. Set Franka DOF position targets (fingers) or DOF torques (arm).""" - self.joint_torque, self.applied_wrench = fc.compute_dof_torque( + self.joint_torque, self.applied_wrench = factory_control.compute_dof_torque( cfg=self.cfg, dof_pos=self.joint_pos, - dof_vel=self.joint_vel, # _fd, + dof_vel=self.joint_vel, 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, + fingertip_midpoint_linvel=self.fingertip_midpoint_linvel, + fingertip_midpoint_angvel=self.fingertip_midpoint_angvel, 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, + ctrl_target_fingertip_midpoint_pos=ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=ctrl_target_fingertip_midpoint_quat, task_prop_gains=self.task_prop_gains, task_deriv_gains=self.task_deriv_gains, device=self.device, + dead_zone_thresholds=self.dead_zone_thresholds, ) # 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.ctrl_target_joint_pos[:, 7:9] = 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.""" + """Check which environments are terminated. + + For Factory reset logic, it is important that all environments + stay in sync (i.e., _get_dones should return all true or all false). + """ self._compute_intermediate_values(dt=self.physics_dt) time_out = self.episode_length_buf >= self.max_episode_length - 1 return time_out, time_out @@ -429,8 +344,20 @@ 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] + held_base_pos, held_base_quat = factory_utils.get_held_base_pose( + self.held_pos, self.held_quat, self.cfg_task.name, self.cfg_task.fixed_asset_cfg, self.num_envs, self.device + ) + target_held_base_pos, target_held_base_quat = factory_utils.get_target_held_base_pose( + self.fixed_pos, + self.fixed_quat, + self.cfg_task.name, + self.cfg_task.fixed_asset_cfg, + self.num_envs, + self.device, + ) + + xy_dist = torch.linalg.vector_norm(target_held_base_pos[:, 0:2] - held_base_pos[:, 0:2], dim=1) + z_disp = held_base_pos[:, 2] - 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 @@ -447,21 +374,15 @@ def _get_curr_successes(self, success_threshold, check_rot=False): 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_yaw = torch_utils.get_euler_xyz(self.fingertip_midpoint_quat) + curr_yaw = factory_utils.wrap_yaw(curr_yaw) + is_rotated = 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) - + def _log_factory_metrics(self, rew_dict, curr_successes): + """Keep track of episode statistics and log rewards.""" # 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 @@ -478,53 +399,94 @@ def _get_rewards(self): success_times = self.ep_success_times[nonzero_success_ids].sum() / len(nonzero_success_ids) self.extras["success_times"] = success_times + for rew_name, rew in rew_dict.items(): + self.extras[f"logs_rew_{rew_name}"] = rew.mean() + + 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_dict, rew_scales = self._get_factory_rew_dict(curr_successes) + + rew_buf = torch.zeros_like(rew_dict["kp_coarse"]) + for rew_name, rew in rew_dict.items(): + rew_buf += rew_dict[rew_name] * rew_scales[rew_name] + self.prev_actions = self.actions.clone() + + self._log_factory_metrics(rew_dict, curr_successes) return rew_buf - def _update_rew_buf(self, curr_successes): - """Compute reward at current timestep.""" - rew_dict = {} + def _get_factory_rew_dict(self, curr_successes): + """Compute reward terms at current timestep.""" + rew_dict, rew_scales = {}, {} + + # Compute pos of keypoints on held asset, and fixed asset in world frame + held_base_pos, held_base_quat = factory_utils.get_held_base_pose( + self.held_pos, self.held_quat, self.cfg_task.name, self.cfg_task.fixed_asset_cfg, self.num_envs, self.device + ) + target_held_base_pos, target_held_base_quat = factory_utils.get_target_held_base_pose( + self.fixed_pos, + self.fixed_quat, + self.cfg_task.name, + self.cfg_task.fixed_asset_cfg, + self.num_envs, + self.device, + ) - # Keypoint rewards. - def squashing_fn(x, a, b): - return 1 / (torch.exp(a * x) + b + torch.exp(-a * x)) + keypoints_held = torch.zeros((self.num_envs, self.cfg_task.num_keypoints, 3), device=self.device) + keypoints_fixed = torch.zeros((self.num_envs, self.cfg_task.num_keypoints, 3), device=self.device) + offsets = factory_utils.get_keypoint_offsets(self.cfg_task.num_keypoints, self.device) + keypoint_offsets = offsets * self.cfg_task.keypoint_scale + for idx, keypoint_offset in enumerate(keypoint_offsets): + keypoints_held[:, idx] = torch_utils.tf_combine( + held_base_quat, + held_base_pos, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1), + keypoint_offset.repeat(self.num_envs, 1), + )[1] + keypoints_fixed[:, idx] = torch_utils.tf_combine( + target_held_base_quat, + target_held_base_pos, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1), + keypoint_offset.repeat(self.num_envs, 1), + )[1] + keypoint_dist = torch.norm(keypoints_held - keypoints_fixed, p=2, dim=-1).mean(-1) 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 + action_penalty_ee = torch.norm(self.actions, p=2) + action_grad_penalty = torch.norm(self.actions - self.prev_actions, p=2, dim=-1) + curr_engaged = self._get_curr_successes(success_threshold=self.cfg_task.engage_threshold, check_rot=False) + + rew_dict = { + "kp_baseline": factory_utils.squashing_fn(keypoint_dist, a0, b0), + "kp_coarse": factory_utils.squashing_fn(keypoint_dist, a1, b1), + "kp_fine": factory_utils.squashing_fn(keypoint_dist, a2, b2), + "action_penalty_ee": action_penalty_ee, + "action_grad_penalty": action_grad_penalty, + "curr_engaged": curr_engaged.float(), + "curr_success": curr_successes.float(), + } + rew_scales = { + "kp_baseline": 1.0, + "kp_coarse": 1.0, + "kp_fine": 1.0, + "action_penalty_ee": -self.cfg_task.action_penalty_ee_scale, + "action_grad_penalty": -self.cfg_task.action_grad_penalty_scale, + "curr_engaged": 1.0, + "curr_success": 1.0, + } + return rew_dict, rew_scales def _reset_idx(self, env_ids): - """ - We assume all envs will always be reset at the same time. - """ + """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) @@ -533,19 +495,6 @@ def _reset_idx(self, env_ids): 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] @@ -562,16 +511,18 @@ def _set_assets_to_default_pose(self, 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): + def set_pos_inverse_kinematics( + self, ctrl_target_fingertip_midpoint_pos, ctrl_target_fingertip_midpoint_quat, 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( + pos_error, axis_angle_error = factory_control.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], + ctrl_target_fingertip_midpoint_pos=ctrl_target_fingertip_midpoint_pos[env_ids], + ctrl_target_fingertip_midpoint_quat=ctrl_target_fingertip_midpoint_quat[env_ids], jacobian_type="geometric", rot_error_type="axis_angle", ) @@ -579,7 +530,7 @@ def set_pos_inverse_kinematics(self, env_ids): delta_hand_pose = torch.cat((pos_error, axis_angle_error), dim=-1) # Solve DLS problem. - delta_dof_pos = fc._get_delta_dof_pos( + delta_dof_pos = factory_control.get_delta_dof_pos( delta_pose=delta_hand_pose, ik_method="dls", jacobian=self.fingertip_midpoint_jacobian[env_ids], @@ -602,21 +553,25 @@ def set_pos_inverse_kinematics(self, env_ids): 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 = torch.zeros((self.num_envs, 3), device=self.device) 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 = torch.zeros((self.num_envs, 3), device=self.device) + gear_base_offset = self.cfg_task.fixed_asset_cfg.medium_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 + held_asset_relative_pos = factory_utils.get_held_base_pos_local( + self.cfg_task.name, self.cfg_task.fixed_asset_cfg, self.num_envs, self.device + ) else: raise NotImplementedError("Task not implemented") - held_asset_relative_quat = self.identity_quat + held_asset_relative_quat = ( + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + ) 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 @@ -646,7 +601,11 @@ def _set_franka_to_default_pose(self, joints, env_ids): self.step_sim_no_action() def step_sim_no_action(self): - """Step the simulation without an action. Used for resets.""" + """Step the simulation without an action. Used for resets only. + + This method should only be called during resets when all environments + reset at the same time. + """ self.scene.write_data_to_sim() self.sim.step(render=False) self.scene.update(dt=self.physics_dt) @@ -695,14 +654,17 @@ def randomize_initial_state(self, 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 = torch.zeros((self.num_envs, 3), device=self.device) 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_local[:, 0] = self.cfg_task.fixed_asset_cfg.medium_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_quat, + self.fixed_pos, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1), + fixed_tip_pos_local, ) self.fixed_pos_obs_frame[:] = fixed_tip_pos @@ -712,7 +674,6 @@ def randomize_initial_state(self, env_ids): 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] @@ -735,16 +696,16 @@ def randomize_initial_state(self, env_ids): 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, aa_error = self.set_pos_inverse_kinematics( + ctrl_target_fingertip_midpoint_pos=above_fixed_pos, + ctrl_target_fingertip_midpoint_quat=hand_down_quat, + 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) @@ -785,7 +746,7 @@ def randomize_initial_state(self, env_ids): q1=self.fingertip_midpoint_quat, t1=self.fingertip_midpoint_pos, q2=flip_z_quat, - t2=torch.zeros_like(self.fingertip_midpoint_pos), + t2=torch.zeros((self.num_envs, 3), device=self.device), ) # get default gripper in asset transform @@ -800,17 +761,17 @@ def randomize_initial_state(self, env_ids): # 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] + 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[:, 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) + held_asset_pos_noise_level = torch.tensor(self.cfg_task.held_asset_pos_noise, device=self.device) + held_asset_pos_noise = held_asset_pos_noise @ torch.diag(held_asset_pos_noise_level) 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, + q2=torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1), + t2=held_asset_pos_noise, ) held_state = self._held_asset.data.default_root_state.clone() @@ -826,15 +787,16 @@ def randomize_initial_state(self, env_ids): 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.task_prop_gains = reset_task_prop_gains + self.task_deriv_gains = factory_utils.get_deriv_gains( + reset_task_prop_gains, self.cfg.ctrl.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() @@ -846,38 +808,13 @@ def randomize_initial_state(self, env_ids): # 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) + self.task_prop_gains = self.default_gains + self.task_deriv_gains = factory_utils.get_deriv_gains(self.default_gains) physics_sim_view.set_gravity(carb.Float3(*self.cfg.sim.gravity)) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py index 3fe55612abdd..0e6b5db0a73c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -1,10 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg from isaaclab.scene import InteractiveSceneCfg @@ -107,6 +107,7 @@ class FactoryEnvCfg(DirectRLEnvCfg): friction_correlation_distance=0.00625, gpu_max_rigid_contact_count=2**23, gpu_max_rigid_patch_count=2**23, + gpu_collision_stack_size=2**28, gpu_max_num_partitions=1, # Important for stable simulation. ), physics_material=RigidBodyMaterialCfg( @@ -115,7 +116,7 @@ class FactoryEnvCfg(DirectRLEnvCfg): ), ) - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0) + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0, clone_in_fabric=True) robot = ArticulationCfg( prim_path="/World/envs/env_.*/Robot", @@ -162,8 +163,8 @@ class FactoryEnvCfg(DirectRLEnvCfg): damping=0.0, friction=0.0, armature=0.0, - effort_limit=87, - velocity_limit=124.6, + effort_limit_sim=87, + velocity_limit_sim=124.6, ), "panda_arm2": ImplicitActuatorCfg( joint_names_expr=["panda_joint[5-7]"], @@ -171,13 +172,13 @@ class FactoryEnvCfg(DirectRLEnvCfg): damping=0.0, friction=0.0, armature=0.0, - effort_limit=12, - velocity_limit=149.5, + effort_limit_sim=12, + velocity_limit_sim=149.5, ), "panda_hand": ImplicitActuatorCfg( joint_names_expr=["panda_finger_joint[1-2]"], - effort_limit=40.0, - velocity_limit=0.04, + effort_limit_sim=40.0, + velocity_limit_sim=0.04, stiffness=7500.0, damping=173.0, friction=0.1, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_tasks_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_tasks_cfg.py index 00c1e442f0ae..c631856816cb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_tasks_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_tasks_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -67,7 +67,7 @@ class FactoryTask: # Reward ee_success_yaw: float = 0.0 # nut_thread task only. - action_penalty_scale: float = 0.0 + action_penalty_ee_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. @@ -206,7 +206,6 @@ 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" diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_utils.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_utils.py new file mode 100644 index 000000000000..962b3872bf09 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_utils.py @@ -0,0 +1,114 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import torch + +import isaacsim.core.utils.torch as torch_utils + + +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 get_deriv_gains(prop_gains, rot_deriv_scale=1.0): + """Set robot gains using critical damping.""" + deriv_gains = 2 * torch.sqrt(prop_gains) + deriv_gains[:, 3:6] /= rot_deriv_scale + return deriv_gains + + +def wrap_yaw(angle): + """Ensure yaw stays within range.""" + return torch.where(angle > np.deg2rad(235), angle - 2 * np.pi, angle) + + +def set_friction(asset, value, num_envs): + """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(num_envs, device="cpu") + asset.root_physx_view.set_material_properties(materials, env_ids) + + +def set_body_inertias(robot, num_envs): + """Note: this is to account for the asset_options.armature parameter in IGE.""" + inertias = robot.root_physx_view.get_inertias() + offset = torch.zeros_like(inertias) + offset[:, :, [0, 4, 8]] += 0.01 + new_inertias = inertias + offset + robot.root_physx_view.set_inertias(new_inertias, torch.arange(num_envs)) + + +def get_held_base_pos_local(task_name, fixed_asset_cfg, num_envs, device): + """Get transform between asset default frame and geometric base frame.""" + held_base_x_offset = 0.0 + if task_name == "peg_insert": + held_base_z_offset = 0.0 + elif task_name == "gear_mesh": + gear_base_offset = fixed_asset_cfg.medium_gear_base_offset + held_base_x_offset = gear_base_offset[0] + held_base_z_offset = gear_base_offset[2] + elif task_name == "nut_thread": + held_base_z_offset = fixed_asset_cfg.base_height + else: + raise NotImplementedError("Task not implemented") + + held_base_pos_local = torch.tensor([0.0, 0.0, 0.0], device=device).repeat((num_envs, 1)) + held_base_pos_local[:, 0] = held_base_x_offset + held_base_pos_local[:, 2] = held_base_z_offset + + return held_base_pos_local + + +def get_held_base_pose(held_pos, held_quat, task_name, fixed_asset_cfg, num_envs, device): + """Get current poses for keypoint and success computation.""" + held_base_pos_local = get_held_base_pos_local(task_name, fixed_asset_cfg, num_envs, device) + held_base_quat_local = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).unsqueeze(0).repeat(num_envs, 1) + + held_base_quat, held_base_pos = torch_utils.tf_combine( + held_quat, held_pos, held_base_quat_local, held_base_pos_local + ) + return held_base_pos, held_base_quat + + +def get_target_held_base_pose(fixed_pos, fixed_quat, task_name, fixed_asset_cfg, num_envs, device): + """Get target poses for keypoint and success computation.""" + fixed_success_pos_local = torch.zeros((num_envs, 3), device=device) + if task_name == "peg_insert": + fixed_success_pos_local[:, 2] = 0.0 + elif task_name == "gear_mesh": + gear_base_offset = fixed_asset_cfg.medium_gear_base_offset + fixed_success_pos_local[:, 0] = gear_base_offset[0] + fixed_success_pos_local[:, 2] = gear_base_offset[2] + elif task_name == "nut_thread": + head_height = fixed_asset_cfg.base_height + shank_length = fixed_asset_cfg.height + thread_pitch = fixed_asset_cfg.thread_pitch + fixed_success_pos_local[:, 2] = head_height + shank_length - thread_pitch * 1.5 + else: + raise NotImplementedError("Task not implemented") + fixed_success_quat_local = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).unsqueeze(0).repeat(num_envs, 1) + + target_held_base_quat, target_held_base_pos = torch_utils.tf_combine( + fixed_quat, fixed_pos, fixed_success_quat_local, fixed_success_pos_local + ) + return target_held_base_pos, target_held_base_quat + + +def squashing_fn(x, a, b): + """Compute bounded reward function.""" + return 1 / (torch.exp(a * x) + b + torch.exp(-a * x)) + + +def collapse_obs_dict(obs_dict, obs_order): + """Stack observations in given order.""" + obs_tensors = [obs_dict[obs_name] for obs_name in obs_order] + obs_tensors = torch.cat(obs_tensors, dim=-1) + return obs_tensors diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/forge/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/__init__.py new file mode 100644 index 000000000000..6532e3c3b6b5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/__init__.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Forge-PegInsert-Direct-v0", + entry_point=f"{__name__}.forge_env:ForgeEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.forge_env_cfg:ForgeTaskPegInsertCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Forge-GearMesh-Direct-v0", + entry_point=f"{__name__}.forge_env:ForgeEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.forge_env_cfg:ForgeTaskGearMeshCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Forge-NutThread-Direct-v0", + entry_point=f"{__name__}.forge_env:ForgeEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.forge_env_cfg:ForgeTaskNutThreadCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg_nut_thread.yaml", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 000000000000..08081f97e035 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,123 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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: Forge + 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 + 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/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/rl_games_ppo_cfg_nut_thread.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/rl_games_ppo_cfg_nut_thread.yaml new file mode 100644 index 000000000000..a73dd178f6e0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/agents/rl_games_ppo_cfg_nut_thread.yaml @@ -0,0 +1,123 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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: Forge + 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 + truncate_grads: True + e_clip: 0.2 + horizon_length: 256 + 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/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env.py new file mode 100644 index 000000000000..75484cbd8f17 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env.py @@ -0,0 +1,388 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +import numpy as np +import torch + +import isaacsim.core.utils.torch as torch_utils + +from isaaclab.utils.math import axis_angle_from_quat + +from isaaclab_tasks.direct.factory import factory_utils +from isaaclab_tasks.direct.factory.factory_env import FactoryEnv + +from . import forge_utils +from .forge_env_cfg import ForgeEnvCfg + + +class ForgeEnv(FactoryEnv): + cfg: ForgeEnvCfg + + def __init__(self, cfg: ForgeEnvCfg, render_mode: str | None = None, **kwargs): + """Initialize additional randomization and logging tensors.""" + super().__init__(cfg, render_mode, **kwargs) + + # Success prediction. + self.success_pred_scale = 0.0 + self.first_pred_success_tx = {} + for thresh in [0.5, 0.6, 0.7, 0.8, 0.9]: + self.first_pred_success_tx[thresh] = torch.zeros(self.num_envs, device=self.device, dtype=torch.long) + + # Flip quaternions. + self.flip_quats = torch.ones((self.num_envs,), dtype=torch.float32, device=self.device) + + # Force sensor information. + self.force_sensor_body_idx = self._robot.body_names.index("force_sensor") + self.force_sensor_smooth = torch.zeros((self.num_envs, 6), device=self.device) + self.force_sensor_world_smooth = torch.zeros((self.num_envs, 6), device=self.device) + + # Set nominal dynamics parameters for randomization. + self.default_gains = torch.tensor(self.cfg.ctrl.default_task_prop_gains, device=self.device).repeat( + (self.num_envs, 1) + ) + self.default_pos_threshold = torch.tensor(self.cfg.ctrl.pos_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + self.default_rot_threshold = torch.tensor(self.cfg.ctrl.rot_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + self.default_dead_zone = torch.tensor(self.cfg.ctrl.default_dead_zone, device=self.device).repeat( + (self.num_envs, 1) + ) + + self.pos_threshold = self.default_pos_threshold.clone() + self.rot_threshold = self.default_rot_threshold.clone() + + def _compute_intermediate_values(self, dt): + """Add noise to observations for force sensing.""" + super()._compute_intermediate_values(dt) + + # Add noise to fingertip position. + pos_noise_level, rot_noise_level_deg = self.cfg.obs_rand.fingertip_pos, self.cfg.obs_rand.fingertip_rot_deg + fingertip_pos_noise = torch.randn((self.num_envs, 3), dtype=torch.float32, device=self.device) + fingertip_pos_noise = fingertip_pos_noise @ torch.diag( + torch.tensor([pos_noise_level, pos_noise_level, pos_noise_level], dtype=torch.float32, device=self.device) + ) + self.noisy_fingertip_pos = self.fingertip_midpoint_pos + fingertip_pos_noise + + rot_noise_axis = torch.randn((self.num_envs, 3), dtype=torch.float32, device=self.device) + rot_noise_axis /= torch.linalg.norm(rot_noise_axis, dim=1, keepdim=True) + rot_noise_angle = torch.randn((self.num_envs,), dtype=torch.float32, device=self.device) * np.deg2rad( + rot_noise_level_deg + ) + self.noisy_fingertip_quat = torch_utils.quat_mul( + self.fingertip_midpoint_quat, torch_utils.quat_from_angle_axis(rot_noise_angle, rot_noise_axis) + ) + self.noisy_fingertip_quat[:, [0, 3]] = 0.0 + self.noisy_fingertip_quat = self.noisy_fingertip_quat * self.flip_quats.unsqueeze(-1) + + # Repeat finite differencing with noisy fingertip positions. + self.ee_linvel_fd = (self.noisy_fingertip_pos - self.prev_fingertip_pos) / dt + self.prev_fingertip_pos = self.noisy_fingertip_pos.clone() + + # Add state differences if velocity isn't being added. + rot_diff_quat = torch_utils.quat_mul( + self.noisy_fingertip_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.ee_angvel_fd[:, 0:2] = 0.0 + self.prev_fingertip_quat = self.noisy_fingertip_quat.clone() + + # Update and smooth force values. + self.force_sensor_world = self._robot.root_physx_view.get_link_incoming_joint_force()[ + :, self.force_sensor_body_idx + ] + + alpha = self.cfg.ft_smoothing_factor + self.force_sensor_world_smooth = alpha * self.force_sensor_world + (1 - alpha) * self.force_sensor_world_smooth + + self.force_sensor_smooth = torch.zeros_like(self.force_sensor_world) + identity_quat = torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + self.force_sensor_smooth[:, :3], self.force_sensor_smooth[:, 3:6] = forge_utils.change_FT_frame( + self.force_sensor_world_smooth[:, 0:3], + self.force_sensor_world_smooth[:, 3:6], + (identity_quat, torch.zeros((self.num_envs, 3), device=self.device)), + (identity_quat, self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise), + ) + + # Compute noisy force values. + force_noise = torch.randn((self.num_envs, 3), dtype=torch.float32, device=self.device) + force_noise *= self.cfg.obs_rand.ft_force + self.noisy_force = self.force_sensor_smooth[:, 0:3] + force_noise + + def _get_observations(self): + """Add additional FORGE observations.""" + obs_dict, state_dict = self._get_factory_obs_state_dict() + + noisy_fixed_pos = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + prev_actions = self.actions.clone() + prev_actions[:, 3:5] = 0.0 + + obs_dict.update( + { + "fingertip_pos": self.noisy_fingertip_pos, + "fingertip_pos_rel_fixed": self.noisy_fingertip_pos - noisy_fixed_pos, + "fingertip_quat": self.noisy_fingertip_quat, + "force_threshold": self.contact_penalty_thresholds[:, None], + "ft_force": self.noisy_force, + "prev_actions": prev_actions, + } + ) + + state_dict.update( + { + "ema_factor": self.ema_factor, + "ft_force": self.force_sensor_smooth[:, 0:3], + "force_threshold": self.contact_penalty_thresholds[:, None], + "prev_actions": prev_actions, + } + ) + + obs_tensors = factory_utils.collapse_obs_dict(obs_dict, self.cfg.obs_order + ["prev_actions"]) + state_tensors = factory_utils.collapse_obs_dict(state_dict, self.cfg.state_order + ["prev_actions"]) + return {"policy": obs_tensors, "critic": state_tensors} + + def _apply_action(self): + """FORGE actions are defined as targets relative to the fixed asset.""" + if self.last_update_timestamp < self._robot._data._sim_timestamp: + self._compute_intermediate_values(dt=self.physics_dt) + + # Step (0): Scale actions to allowed range. + pos_actions = self.actions[:, 0:3] + pos_actions = pos_actions @ torch.diag(torch.tensor(self.cfg.ctrl.pos_action_bounds, device=self.device)) + + rot_actions = self.actions[:, 3:6] + rot_actions = rot_actions @ torch.diag(torch.tensor(self.cfg.ctrl.rot_action_bounds, device=self.device)) + + # Step (1): Compute desired pose targets in EE frame. + # (1.a) Position. Action frame is assumed to be the top of the bolt (noisy estimate). + fixed_pos_action_frame = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + ctrl_target_fingertip_preclipped_pos = fixed_pos_action_frame + pos_actions + # (1.b) Enforce rotation action constraints. + rot_actions[:, 0:2] = 0.0 + + # Assumes joint limit is in (+x, -y)-quadrant of world frame. + rot_actions[:, 2] = np.deg2rad(-180.0) + np.deg2rad(270.0) * (rot_actions[:, 2] + 1.0) / 2.0 # Joint limit. + # (1.c) Get desired orientation target. + bolt_frame_quat = torch_utils.quat_from_euler_xyz( + roll=rot_actions[:, 0], pitch=rot_actions[:, 1], yaw=rot_actions[:, 2] + ) + + rot_180_euler = torch.tensor([np.pi, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1) + quat_bolt_to_ee = torch_utils.quat_from_euler_xyz( + roll=rot_180_euler[:, 0], pitch=rot_180_euler[:, 1], yaw=rot_180_euler[:, 2] + ) + + ctrl_target_fingertip_preclipped_quat = torch_utils.quat_mul(quat_bolt_to_ee, bolt_frame_quat) + + # Step (2): Clip targets if they are too far from current EE pose. + # (2.a): Clip position targets. + self.delta_pos = ctrl_target_fingertip_preclipped_pos - self.fingertip_midpoint_pos # Used for action_penalty. + pos_error_clipped = torch.clip(self.delta_pos, -self.pos_threshold, self.pos_threshold) + ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_error_clipped + + # (2.b) Clip orientation targets. Use Euler angles. We assume we are near upright, so + # clipping yaw will effectively cause slow motions. When we clip, we also need to make + # sure we avoid the joint limit. + + # (2.b.i) Get current and desired Euler angles. + curr_roll, curr_pitch, curr_yaw = torch_utils.get_euler_xyz(self.fingertip_midpoint_quat) + desired_roll, desired_pitch, desired_yaw = torch_utils.get_euler_xyz(ctrl_target_fingertip_preclipped_quat) + desired_xyz = torch.stack([desired_roll, desired_pitch, desired_yaw], dim=1) + + # (2.b.ii) Correct the direction of motion to avoid joint limit. + # Map yaws between [-125, 235] degrees + # (so that angles appear on a continuous span uninterrupted by the joint limit) + curr_yaw = factory_utils.wrap_yaw(curr_yaw) + desired_yaw = factory_utils.wrap_yaw(desired_yaw) + + # (2.b.iii) Clip motion in the correct direction. + self.delta_yaw = desired_yaw - curr_yaw # Used later for action_penalty. + clipped_yaw = torch.clip(self.delta_yaw, -self.rot_threshold[:, 2], self.rot_threshold[:, 2]) + desired_xyz[:, 2] = curr_yaw + clipped_yaw + + # (2.b.iv) Clip roll and pitch. + desired_roll = torch.where(desired_roll < 0.0, desired_roll + 2 * torch.pi, desired_roll) + desired_pitch = torch.where(desired_pitch < 0.0, desired_pitch + 2 * torch.pi, desired_pitch) + + delta_roll = desired_roll - curr_roll + clipped_roll = torch.clip(delta_roll, -self.rot_threshold[:, 0], self.rot_threshold[:, 0]) + desired_xyz[:, 0] = curr_roll + clipped_roll + + curr_pitch = torch.where(curr_pitch > torch.pi, curr_pitch - 2 * torch.pi, curr_pitch) + desired_pitch = torch.where(desired_pitch > torch.pi, desired_pitch - 2 * torch.pi, desired_pitch) + + delta_pitch = desired_pitch - curr_pitch + clipped_pitch = torch.clip(delta_pitch, -self.rot_threshold[:, 1], self.rot_threshold[:, 1]) + desired_xyz[:, 1] = curr_pitch + clipped_pitch + + ctrl_target_fingertip_midpoint_quat = torch_utils.quat_from_euler_xyz( + roll=desired_xyz[:, 0], pitch=desired_xyz[:, 1], yaw=desired_xyz[:, 2] + ) + + self.generate_ctrl_signals( + ctrl_target_fingertip_midpoint_pos=ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=ctrl_target_fingertip_midpoint_quat, + ctrl_target_gripper_dof_pos=0.0, + ) + + def _get_rewards(self): + """FORGE reward includes a contact penalty and success prediction error.""" + # Use same base rewards as Factory. + rew_buf = super()._get_rewards() + + rew_dict, rew_scales = {}, {} + # Calculate action penalty for the asset-relative action space. + pos_error = torch.norm(self.delta_pos, p=2, dim=-1) / self.cfg.ctrl.pos_action_threshold[0] + rot_error = torch.abs(self.delta_yaw) / self.cfg.ctrl.rot_action_threshold[0] + # Contact penalty. + contact_force = torch.norm(self.force_sensor_smooth[:, 0:3], p=2, dim=-1, keepdim=False) + contact_penalty = torch.nn.functional.relu(contact_force - self.contact_penalty_thresholds) + # Add success prediction rewards. + check_rot = self.cfg_task.name == "nut_thread" + true_successes = self._get_curr_successes( + success_threshold=self.cfg_task.success_threshold, check_rot=check_rot + ) + policy_success_pred = (self.actions[:, 6] + 1) / 2 # rescale from [-1, 1] to [0, 1] + success_pred_error = (true_successes.float() - policy_success_pred).abs() + # Delay success prediction penalty until some successes have occurred. + if true_successes.float().mean() >= self.cfg_task.delay_until_ratio: + self.success_pred_scale = 1.0 + + # Add new FORGE reward terms. + rew_dict = { + "action_penalty_asset": pos_error + rot_error, + "contact_penalty": contact_penalty, + "success_pred_error": success_pred_error, + } + rew_scales = { + "action_penalty_asset": -self.cfg_task.action_penalty_asset_scale, + "contact_penalty": -self.cfg_task.contact_penalty_scale, + "success_pred_error": -self.success_pred_scale, + } + for rew_name, rew in rew_dict.items(): + rew_buf += rew_dict[rew_name] * rew_scales[rew_name] + + self._log_forge_metrics(rew_dict, policy_success_pred) + return rew_buf + + def _reset_idx(self, env_ids): + """Perform additional randomizations.""" + super()._reset_idx(env_ids) + + # Compute initial action for correct EMA computation. + fixed_pos_action_frame = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + pos_actions = self.fingertip_midpoint_pos - 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 + self.actions[:, 6] = self.prev_actions[:, 6] = -1.0 + + # EMA randomization. + ema_rand = torch.rand((self.num_envs, 1), dtype=torch.float32, device=self.device) + ema_lower, ema_upper = self.cfg.ctrl.ema_factor_range + self.ema_factor = ema_lower + ema_rand * (ema_upper - ema_lower) + + # Set initial gains for the episode. + prop_gains = self.default_gains.clone() + self.pos_threshold = self.default_pos_threshold.clone() + self.rot_threshold = self.default_rot_threshold.clone() + prop_gains = forge_utils.get_random_prop_gains( + prop_gains, self.cfg.ctrl.task_prop_gains_noise_level, self.num_envs, self.device + ) + self.pos_threshold = forge_utils.get_random_prop_gains( + self.pos_threshold, self.cfg.ctrl.pos_threshold_noise_level, self.num_envs, self.device + ) + self.rot_threshold = forge_utils.get_random_prop_gains( + self.rot_threshold, self.cfg.ctrl.rot_threshold_noise_level, self.num_envs, self.device + ) + self.task_prop_gains = prop_gains + self.task_deriv_gains = factory_utils.get_deriv_gains(prop_gains) + + contact_rand = torch.rand((self.num_envs,), dtype=torch.float32, device=self.device) + contact_lower, contact_upper = self.cfg.task.contact_penalty_threshold_range + self.contact_penalty_thresholds = contact_lower + contact_rand * (contact_upper - contact_lower) + + self.dead_zone_thresholds = ( + torch.rand((self.num_envs, 6), dtype=torch.float32, device=self.device) * self.default_dead_zone + ) + + self.force_sensor_world_smooth[:, :] = 0.0 + + self.flip_quats = torch.ones((self.num_envs,), dtype=torch.float32, device=self.device) + rand_flips = torch.rand(self.num_envs) > 0.5 + self.flip_quats[rand_flips] = -1.0 + + def _reset_buffers(self, env_ids): + """Reset additional logging metrics.""" + super()._reset_buffers(env_ids) + # Reset success pred metrics. + for thresh in [0.5, 0.6, 0.7, 0.8, 0.9]: + self.first_pred_success_tx[thresh][env_ids] = 0 + + def _log_forge_metrics(self, rew_dict, policy_success_pred): + """Log metrics to evaluate success prediction performance.""" + for rew_name, rew in rew_dict.items(): + self.extras[f"logs_rew_{rew_name}"] = rew.mean() + + for thresh, first_success_tx in self.first_pred_success_tx.items(): + curr_predicted_success = policy_success_pred > thresh + first_success_idxs = torch.logical_and(curr_predicted_success, first_success_tx == 0) + + first_success_tx[:] = torch.where(first_success_idxs, self.episode_length_buf, first_success_tx) + + # Only log at the end. + if torch.any(self.reset_buf): + # Log prediction delay. + delay_ids = torch.logical_and(self.ep_success_times != 0, first_success_tx != 0) + delay_times = (first_success_tx[delay_ids] - self.ep_success_times[delay_ids]).sum() / delay_ids.sum() + if delay_ids.sum().item() > 0: + self.extras[f"early_term_delay_all/{thresh}"] = delay_times + + correct_delay_ids = torch.logical_and(delay_ids, first_success_tx > self.ep_success_times) + correct_delay_times = ( + first_success_tx[correct_delay_ids] - self.ep_success_times[correct_delay_ids] + ).sum() / correct_delay_ids.sum() + if correct_delay_ids.sum().item() > 0: + self.extras[f"early_term_delay_correct/{thresh}"] = correct_delay_times.item() + + # Log early-term success rate (for all episodes we have "stopped", did we succeed?). + pred_success_idxs = first_success_tx != 0 # Episodes which we have predicted success. + + true_success_preds = torch.logical_and( + self.ep_success_times[pred_success_idxs] > 0, # Success has actually occurred. + self.ep_success_times[pred_success_idxs] + < first_success_tx[pred_success_idxs], # Success occurred before we predicted it. + ) + + num_pred_success = pred_success_idxs.sum().item() + et_prec = true_success_preds.sum() / num_pred_success + if num_pred_success > 0: + self.extras[f"early_term_precision/{thresh}"] = et_prec + + true_success_idxs = self.ep_success_times > 0 + num_true_success = true_success_idxs.sum().item() + et_recall = true_success_preds.sum() / num_true_success + if num_true_success > 0: + self.extras[f"early_term_recall/{thresh}"] = et_recall diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env_cfg.py new file mode 100644 index 000000000000..5da73aa2ae35 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env_cfg.py @@ -0,0 +1,150 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.envs.mdp as mdp +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.direct.factory.factory_env_cfg import OBS_DIM_CFG, STATE_DIM_CFG, CtrlCfg, FactoryEnvCfg, ObsRandCfg + +from .forge_events import randomize_dead_zone +from .forge_tasks_cfg import ForgeGearMesh, ForgeNutThread, ForgePegInsert, ForgeTask + +OBS_DIM_CFG.update({"force_threshold": 1, "ft_force": 3}) + +STATE_DIM_CFG.update({"force_threshold": 1, "ft_force": 3}) + + +@configclass +class ForgeCtrlCfg(CtrlCfg): + ema_factor_range = [0.025, 0.1] + default_task_prop_gains = [565.0, 565.0, 565.0, 28.0, 28.0, 28.0] + task_prop_gains_noise_level = [0.41, 0.41, 0.41, 0.41, 0.41, 0.41] + pos_threshold_noise_level = [0.25, 0.25, 0.25] + rot_threshold_noise_level = [0.29, 0.29, 0.29] + default_dead_zone = [5.0, 5.0, 5.0, 1.0, 1.0, 1.0] + + +@configclass +class ForgeObsRandCfg(ObsRandCfg): + fingertip_pos = 0.00025 + fingertip_rot_deg = 0.1 + ft_force = 1.0 + + +@configclass +class EventCfg: + object_scale_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("held_asset"), + "mass_distribution_params": (-0.005, 0.005), + "operation": "add", + "distribution": "uniform", + }, + ) + + held_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("held_asset"), + "static_friction_range": (0.75, 0.75), + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 1, + }, + ) + + fixed_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("fixed_asset"), + "static_friction_range": (0.25, 1.25), # TODO: Set these values based on asset type. + "dynamic_friction_range": (0.25, 0.25), + "restitution_range": (0.0, 0.0), + "num_buckets": 128, + }, + ) + + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.75, 0.75), + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 1, + }, + ) + + dead_zone_thresholds = EventTerm( + func=randomize_dead_zone, + mode="interval", + interval_range_s=(2.0, 2.0), # (0.25, 0.25) + ) + + +@configclass +class ForgeEnvCfg(FactoryEnvCfg): + action_space: int = 7 + obs_rand: ForgeObsRandCfg = ForgeObsRandCfg() + ctrl: ForgeCtrlCfg = ForgeCtrlCfg() + task: ForgeTask = ForgeTask() + events: EventCfg = EventCfg() + + ft_smoothing_factor: float = 0.25 + + obs_order: list = [ + "fingertip_pos_rel_fixed", + "fingertip_quat", + "ee_linvel", + "ee_angvel", + "ft_force", + "force_threshold", + ] + 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_prop_gains", + "ema_factor", + "ft_force", + "pos_threshold", + "rot_threshold", + "force_threshold", + ] + + +@configclass +class ForgeTaskPegInsertCfg(ForgeEnvCfg): + task_name = "peg_insert" + task = ForgePegInsert() + episode_length_s = 10.0 + + +@configclass +class ForgeTaskGearMeshCfg(ForgeEnvCfg): + task_name = "gear_mesh" + task = ForgeGearMesh() + episode_length_s = 20.0 + + +@configclass +class ForgeTaskNutThreadCfg(ForgeEnvCfg): + task_name = "nut_thread" + task = ForgeNutThread() + episode_length_s = 30.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_events.py b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_events.py new file mode 100644 index 000000000000..15ced1c2b1a9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_events.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + +import torch + +from isaaclab.envs import DirectRLEnv + + +def randomize_dead_zone(env: DirectRLEnv, env_ids: torch.Tensor | None): + env.dead_zone_thresholds = ( + torch.rand((env.num_envs, 6), dtype=torch.float32, device=env.device) * env.default_dead_zone + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_tasks_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_tasks_cfg.py new file mode 100644 index 000000000000..1529543e1889 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_tasks_cfg.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_tasks.direct.factory.factory_tasks_cfg import FactoryTask, GearMesh, NutThread, PegInsert + + +@configclass +class ForgeTask(FactoryTask): + action_penalty_ee_scale: float = 0.0 + action_penalty_asset_scale: float = 0.001 + action_grad_penalty_scale: float = 0.1 + contact_penalty_scale: float = 0.05 + delay_until_ratio: float = 0.25 + contact_penalty_threshold_range = [5.0, 10.0] + + +@configclass +class ForgePegInsert(PegInsert, ForgeTask): + contact_penalty_scale: float = 0.2 + + +@configclass +class ForgeGearMesh(GearMesh, ForgeTask): + contact_penalty_scale: float = 0.05 + + +@configclass +class ForgeNutThread(NutThread, ForgeTask): + contact_penalty_scale: float = 0.05 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_utils.py b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_utils.py new file mode 100644 index 000000000000..e966cf93f218 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_utils.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch + +import isaacsim.core.utils.torch as torch_utils + + +def get_random_prop_gains(default_values, noise_levels, num_envs, device): + """Helper function to randomize controller gains.""" + c_param_noise = torch.rand((num_envs, default_values.shape[1]), dtype=torch.float32, device=device) + c_param_noise = c_param_noise @ torch.diag(torch.tensor(noise_levels, dtype=torch.float32, device=device)) + c_param_multiplier = 1.0 + c_param_noise + decrease_param_flag = torch.rand((num_envs, default_values.shape[1]), dtype=torch.float32, device=device) > 0.5 + c_param_multiplier = torch.where(decrease_param_flag, 1.0 / c_param_multiplier, c_param_multiplier) + + prop_gains = default_values * c_param_multiplier + + return prop_gains + + +def change_FT_frame(source_F, source_T, source_frame, target_frame): + """Convert force/torque reading from source to target frame.""" + # Modern Robotics eq. 3.95 + source_frame_inv = torch_utils.tf_inverse(source_frame[0], source_frame[1]) + target_T_source_quat, target_T_source_pos = torch_utils.tf_combine( + source_frame_inv[0], source_frame_inv[1], target_frame[0], target_frame[1] + ) + target_F = torch_utils.quat_apply(target_T_source_quat, source_F) + target_T = torch_utils.quat_apply( + target_T_source_quat, (source_T + torch.cross(target_T_source_pos, source_F, dim=-1)) + ) + return target_F, target_T diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/__init__.py index f1db37821d9a..c282be85730f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rl_games_ppo_cfg.yaml index dd9cb448ae75..d4d882905a6c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py index b88ae719d085..a2304fb2c4b7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class FrankaCabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "franka_cabinet_direct" - empirical_normalization = True policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, actor_hidden_dims=[256, 128, 64], critic_hidden_dims=[256, 128, 64], activation="elu", 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 ca858319e608..e1c7fe9676e2 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -23,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/franka_cabinet/franka_cabinet_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py index f0c93527985d..8b87e1bdb258 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,19 +7,19 @@ 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.actuators 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.sim.utils.stage import get_current_stage from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR from isaaclab.utils.math import sample_uniform @@ -46,13 +46,15 @@ class FrankaCabinetEnvCfg(DirectRLEnvCfg): ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=3.0, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=4096, env_spacing=3.0, replicate_physics=True, clone_in_fabric=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", + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd", activate_contact_sensors=False, rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, @@ -79,22 +81,19 @@ class FrankaCabinetEnvCfg(DirectRLEnvCfg): actuators={ "panda_shoulder": ImplicitActuatorCfg( joint_names_expr=["panda_joint[1-4]"], - effort_limit=87.0, - velocity_limit=2.175, + effort_limit_sim=87.0, stiffness=80.0, damping=4.0, ), "panda_forearm": ImplicitActuatorCfg( joint_names_expr=["panda_joint[5-7]"], - effort_limit=12.0, - velocity_limit=2.61, + effort_limit_sim=12.0, stiffness=80.0, damping=4.0, ), "panda_hand": ImplicitActuatorCfg( joint_names_expr=["panda_finger_joint.*"], - effort_limit=200.0, - velocity_limit=0.2, + effort_limit_sim=200.0, stiffness=2e3, damping=1e2, ), @@ -121,15 +120,13 @@ class FrankaCabinetEnvCfg(DirectRLEnvCfg): actuators={ "drawers": ImplicitActuatorCfg( joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], - effort_limit=87.0, - velocity_limit=100.0, + effort_limit_sim=87.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, + effort_limit_sim=87.0, stiffness=10.0, damping=2.5, ), @@ -272,6 +269,9 @@ def _setup_scene(self): # 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 lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py index c6cef3d69dd5..f7e0f518b666 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rl_games_ppo_cfg.yaml index 0b54af26e5bc..4ff1aced918c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py index e39b83d0fd3f..778d73f09119 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class HumanoidPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1000 save_interval = 50 experiment_name = "humanoid_direct" - empirical_normalization = True policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, actor_hidden_dims=[400, 200, 100], critic_hidden_dims=[400, 200, 100], activation="elu", 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 b3ffec362d24..f56d1fc45336 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 200, 100] activations: elu output: ACTIONS @@ -23,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/humanoid_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/humanoid_env.py index 654965974cc1..402409e9d35b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/humanoid_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/humanoid_env.py @@ -1,12 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -from isaaclab_assets import HUMANOID_CFG - import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg @@ -17,6 +15,8 @@ from isaaclab_tasks.direct.locomotion.locomotion_env import LocomotionEnv +from isaaclab_assets import HUMANOID_CFG + @configclass class HumanoidEnvCfg(DirectRLEnvCfg): @@ -45,7 +45,9 @@ class HumanoidEnvCfg(DirectRLEnvCfg): ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=4096, env_spacing=4.0, replicate_physics=True, clone_in_fabric=True + ) # robot robot: ArticulationCfg = HUMANOID_CFG.replace(prim_path="/World/envs/env_.*/Robot") diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/__init__.py index 1a0de033f0ce..36c93d5a1c53 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause 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 b64fe9b1cc98..3071d039b88c 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -15,7 +20,7 @@ models: fixed_log_std: True network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ACTIONS @@ -24,7 +29,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE @@ -33,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 e435b44eac9c..0f6fcdc1a03f 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -15,7 +20,7 @@ models: fixed_log_std: True network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ACTIONS @@ -24,7 +29,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE @@ -33,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 f37842008767..efb34f0d2f5b 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -15,7 +20,7 @@ models: fixed_log_std: True network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ACTIONS @@ -24,7 +29,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE @@ -33,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/humanoid_amp_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env.py index 1e1e6de8587c..5d6a01e9d4e4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -13,7 +13,7 @@ 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_rotate +from isaaclab.utils.math import quat_apply from .humanoid_amp_env_cfg import HumanoidAmpEnvCfg from .motions import MotionLoader @@ -64,6 +64,10 @@ def _setup_scene(self): ) # 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=["/World/ground"]) + # add articulation to scene self.scene.articulations["robot"] = self.robot # add lights @@ -208,8 +212,8 @@ def quaternion_to_tangent_and_normal(q: torch.Tensor) -> torch.Tensor: ref_normal = torch.zeros_like(q[..., :3]) ref_tangent[..., 0] = 1 ref_normal[..., -1] = 1 - tangent = quat_rotate(q, ref_tangent) - normal = quat_rotate(q, ref_normal) + tangent = quat_apply(q, ref_tangent) + normal = quat_apply(q, ref_normal) return torch.cat([tangent, normal], dim=len(tangent.shape) - 1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env_cfg.py index 7840ed3fc283..c7178f746c3f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,8 +8,6 @@ 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 @@ -17,6 +15,8 @@ from isaaclab.sim import PhysxCfg, SimulationCfg from isaaclab.utils import configclass +from isaaclab_assets import HUMANOID_28_CFG + MOTIONS_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "motions") @@ -66,9 +66,11 @@ class HumanoidAmpEnvCfg(DirectRLEnvCfg): actuators={ "body": ImplicitActuatorCfg( joint_names_expr=[".*"], - velocity_limit=100.0, stiffness=None, damping=None, + velocity_limit_sim={ + ".*": 100.0, + }, ), }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/__init__.py index 4004097db4c8..06a047fc65ec 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,3 +8,4 @@ """ from .motion_loader import MotionLoader +from .motion_viewer import MotionViewer diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py index c1072a9f4f02..354332de1b24 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py @@ -1,12 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -import numpy as np +from __future__ import annotations + import os + +import numpy as np import torch -from typing import Optional class MotionLoader: @@ -71,10 +73,10 @@ 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, + b: torch.Tensor | None = None, + blend: torch.Tensor | None = None, + start: np.ndarray | None = None, + end: np.ndarray | None = None, ) -> torch.Tensor: """Linear interpolation between consecutive values. @@ -102,10 +104,10 @@ 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, + q1: torch.Tensor | None = None, + blend: torch.Tensor | None = None, + start: np.ndarray | None = None, + end: np.ndarray | None = None, ) -> torch.Tensor: """Interpolation between consecutive rotations (Spherical Linear Interpolation). @@ -190,13 +192,13 @@ def sample_times(self, num_samples: int, duration: float | None = None) -> np.nd 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})" + 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 + self, num_samples: int, times: np.ndarray | None = None, duration: float | None = None ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: """Sample motion data. @@ -209,9 +211,13 @@ def sample( 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)). + A tuple containing sampled motion data: + - 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)) + - 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) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_viewer.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_viewer.py index bd98bb24fecc..62438f5e3c68 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_viewer.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/motions/motion_viewer.py @@ -1,16 +1,21 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import matplotlib import matplotlib.animation import matplotlib.pyplot as plt +import mpl_toolkits.mplot3d # noqa: F401 import numpy as np import torch -import mpl_toolkits.mplot3d # noqa: F401 -from motion_loader import MotionLoader +try: + from .motion_loader import MotionLoader +except ImportError: + from motion_loader import MotionLoader class MotionViewer: diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py index f0563ecf5f17..c8d4fbf9e2d0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,11 +6,12 @@ from __future__ import annotations -import numpy as np -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import numpy as np +import torch + import isaaclab.sim as sim_utils from isaaclab.assets import Articulation, RigidObject from isaaclab.envs import DirectRLEnv @@ -396,7 +397,6 @@ def compute_rewards( 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) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py index 5b6ad8ed65a6..faac10e1a718 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -53,6 +53,9 @@ def _setup_scene(self): 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 lights diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/__init__.py index c7ca6680db6a..a1a5c9ef913a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rl_games_ppo_cfg.yaml index 193ed86f17f8..36e2d8f61fbf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py index 219b02131806..607d9f0fb0ea 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class QuadcopterPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 200 save_interval = 50 experiment_name = "quadcopter_direct" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[64, 64], critic_hidden_dims=[64, 64], activation="elu", 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 c9df438bbbe0..c84e3f5674bd 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ACTIONS @@ -23,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/quadcopter/quadcopter_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/quadcopter_env.py index 8a493315319b..02857c63d348 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/quadcopter_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/quadcopter_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -85,7 +85,9 @@ class QuadcopterEnvCfg(DirectRLEnvCfg): ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=2.5, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=4096, env_spacing=2.5, replicate_physics=True, clone_in_fabric=True + ) # robot robot: ArticulationCfg = CRAZYFLIE_CFG.replace(prim_path="/World/envs/env_.*/Robot") @@ -138,6 +140,9 @@ def _setup_scene(self): 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 lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) light_cfg.func("/World/Light", light_cfg) @@ -148,11 +153,13 @@ def _pre_physics_step(self, actions: torch.Tensor): 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) + self._robot.permanent_wrench_composer.set_forces_and_torques( + body_ids=self._body_id, forces=self._thrust, torques=self._moment + ) def _get_observations(self) -> dict: desired_pos_b, _ = subtract_frame_transforms( - self._robot.data.root_state_w[:, :3], self._robot.data.root_state_w[:, 3:7], self._desired_pos_w + self._robot.data.root_pos_w, self._robot.data.root_quat_w, self._desired_pos_w ) obs = torch.cat( [ @@ -229,7 +236,7 @@ def _reset_idx(self, env_ids: torch.Tensor | None): 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 + # create markers if necessary for the first time if debug_vis: if not hasattr(self, "goal_pos_visualizer"): marker_cfg = CUBOID_MARKER_CFG.copy() diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/__init__.py index 8dee0ea215d8..4d2f0f3eee50 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -51,7 +51,9 @@ }, ) -### Vision +# ------- +# Vision +# ------- gym.register( id="Isaac-Repose-Cube-Shadow-Vision-Direct-v0", diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_cfg.yaml index 8f18adc4bd02..30b3b0b012f4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_ff_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_ff_cfg.yaml index 98cc0b7d09e6..6724046a9a59 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_ff_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_ff_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 @@ -79,7 +84,7 @@ params: bounds_loss_coef: 0.0001 central_value_config: - minibatch_size: 32864 + minibatch_size: 32768 mini_epochs: 4 learning_rate: 5e-4 lr_schedule: adaptive diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_lstm_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_lstm_cfg.yaml index c404d96d9f40..0aea26e9cde6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_lstm_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_lstm_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_vision_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_vision_cfg.yaml index f77e6cc98ac6..43fa1d20fb88 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_vision_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_vision_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py index 0539ff3e07e1..6ab4c9e56f5a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class ShadowHandPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 10000 save_interval = 250 experiment_name = "shadow_hand" - empirical_normalization = True policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, actor_hidden_dims=[512, 512, 256, 128], critic_hidden_dims=[512, 512, 256, 128], activation="elu", @@ -43,9 +44,10 @@ class ShadowHandAsymFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 10000 save_interval = 250 experiment_name = "shadow_hand_openai_ff" - empirical_normalization = True policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, actor_hidden_dims=[400, 400, 200, 100], critic_hidden_dims=[512, 512, 256, 128], activation="elu", @@ -72,9 +74,10 @@ class ShadowHandVisionFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 50000 save_interval = 250 experiment_name = "shadow_hand_vision" - empirical_normalization = True policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, actor_hidden_dims=[1024, 512, 512, 256, 128], critic_hidden_dims=[1024, 512, 512, 256, 128], activation="elu", 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 c977ab4b6600..0831dd7b4125 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,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 @@ -23,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 37a635e669b0..9bba87a54552 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,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 @@ -23,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 1366dc3c84e1..75a7b6d04a20 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 @@ -1,10 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import glob import os + import torch import torch.nn as nn import torchvision @@ -39,9 +40,11 @@ def __init__(self): 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]), - ]) + 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) @@ -73,12 +76,14 @@ 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. Default is None, which uses the local + "logs" folder resolved relative to this file. """ self.cfg = cfg @@ -89,7 +94,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) @@ -175,14 +183,14 @@ def step( pose_loss.backward() self.optimizer.step() - self.step_count += 1 - 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) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py index 57c54144af03..f9c92f18fbe1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py @@ -1,11 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -from 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 @@ -20,6 +18,8 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.noise import GaussianNoiseCfg, NoiseModelWithAdditiveBiasCfg +from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG + @configclass class EventCfg: @@ -191,6 +191,7 @@ class ShadowHandEnvCfg(DirectRLEnvCfg): max_depenetration_velocity=1000.0, ), mass_props=sim_utils.MassPropertiesCfg(density=567.0), + semantic_tags=[("class", "cube")], ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.39, 0.6), rot=(1.0, 0.0, 0.0, 0.0)), ) @@ -205,7 +206,9 @@ class ShadowHandEnvCfg(DirectRLEnvCfg): }, ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=8192, env_spacing=0.75, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=8192, env_spacing=0.75, replicate_physics=True, clone_in_fabric=True + ) # reset reset_position_noise = 0.01 # range of position at reset 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 988b7c5f3578..b5c781c1a9f2 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 @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,14 +8,6 @@ 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 @@ -65,7 +57,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 @@ -77,15 +70,6 @@ def _setup_scene(self): 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 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/__init__.py index d3ff177f441c..0fbb815b98f2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/rl_games_ppo_cfg.yaml index daa9060d6899..3849c1dd8dab 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 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 18cde08c4ed9..60be7d18110e 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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 f67cc31b249a..57c1c455185d 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,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 @@ -23,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 9cfc4b64f20a..9ab45d268337 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,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 @@ -23,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/shadow_hand_over_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py index 6e01214f015c..09bbff6e97c0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,9 +6,10 @@ from __future__ import annotations +from collections.abc import Sequence + import numpy as np import torch -from collections.abc import Sequence import isaaclab.sim as sim_utils from isaaclab.assets import Articulation, RigidObject diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py index 1580bb0031b0..855939392a22 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py @@ -1,11 +1,9 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -from 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 @@ -18,6 +16,8 @@ from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg from isaaclab.utils import configclass +from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG + @configclass class EventCfg: diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/__init__.py index 7b13258dfc3f..47e4a4aac392 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/__init__.py index 7c3a9eedbd8f..79c13e2aa8f4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/__init__.py index 35819468c23a..df1a4db3a042 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rl_games_ppo_cfg.yaml index b391efc6d99d..aad56e76525c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py index 1d16afe5cfd1..986461733663 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class AntPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1000 save_interval = 50 experiment_name = "ant" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[400, 200, 100], critic_hidden_dims=[400, 200, 100], activation="elu", 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 2c2ed974aa4b..126885cbcd44 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 @@ -1,24 +1,30 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # 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: "dict( - log_std_init=-1, - ortho_init=False, - activation_fn=nn.ReLU, - net_arch=dict(pi=[256, 256], vf=[256, 256]) - )" +policy_kwargs: + log_std_init: -1 + ortho_init: False + activation_fn: 'nn.ReLU' + net_arch: + pi: [256, 256] + vf: [256, 256] 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 945900eb1286..3ff54f7e76e8 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -23,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/ant/ant_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py index 50cd0dd00b3a..289d4f75f8c4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -160,7 +160,7 @@ class AntEnvCfg(ManagerBasedRLEnvCfg): """Configuration for the MuJoCo-style Ant walking environment.""" # Scene settings - scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=5.0) + scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=5.0, clone_in_fabric=True) # Basic settings observations: ObservationsCfg = ObservationsCfg() actions: ActionsCfg = ActionsCfg() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/__init__.py index 50e6d0ea0595..68eef31a0cc8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -23,6 +23,7 @@ "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", + "rsl_rl_with_symmetry_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CartpolePPORunnerWithSymmetryCfg", "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/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_camera_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_camera_ppo_cfg.yaml index 7d1dcf7945e4..abdccfc4b4ac 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_camera_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_camera_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_feature_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_feature_ppo_cfg.yaml index 41e265e9f29a..f28e2c85d54a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_feature_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_feature_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 @@ -52,7 +57,7 @@ params: mixed_precision: False normalize_input: True normalize_value: True - value_bootstraop: True + value_bootstrap: True num_actors: -1 # configured from the script (based on num_envs) reward_shaper: scale_value: 1.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_ppo_cfg.yaml index 7e5707b194fa..eae63873c89d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py index 71900f9c1700..2a266a098df2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py @@ -1,11 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg + +import isaaclab_tasks.manager_based.classic.cartpole.mdp.symmetry as symmetry @configclass @@ -14,9 +16,10 @@ class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 150 save_interval = 50 experiment_name = "cartpole" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[32, 32], critic_hidden_dims=[32, 32], activation="elu", @@ -35,3 +38,27 @@ class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): desired_kl=0.01, max_grad_norm=1.0, ) + + +@configclass +class CartpolePPORunnerWithSymmetryCfg(CartpolePPORunnerCfg): + """Configuration for the PPO agent with symmetry augmentation.""" + + # all the other settings are inherited from the parent class + 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, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, data_augmentation_func=symmetry.compute_symmetric_states + ), + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/sb3_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/sb3_ppo_cfg.yaml index 5856f35f8e87..fcb32cd51dd9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/sb3_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/sb3_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 seed: 42 @@ -11,11 +16,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/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 d6f4edb6d530..6485d4ed57fb 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,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/cartpole/cartpole_camera_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py index ef4f645682e7..d0840c4c1ed6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -21,6 +21,7 @@ @configclass class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg): + """Configuration for the cartpole environment with RGB camera.""" # add camera to the scene tiled_camera: TiledCameraCfg = TiledCameraCfg( @@ -37,7 +38,6 @@ class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg): @configclass class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg): - # add camera to the scene tiled_camera: TiledCameraCfg = TiledCameraCfg( prim_path="{ENV_REGEX_NS}/Camera", @@ -134,7 +134,7 @@ class TheiaTinyFeaturesCameraPolicyCfg(ObsGroup): class CartpoleRGBCameraEnvCfg(CartpoleEnvCfg): """Configuration for the cartpole environment with RGB camera.""" - scene: CartpoleRGBCameraSceneCfg = CartpoleRGBCameraSceneCfg(num_envs=1024, env_spacing=20) + scene: CartpoleRGBCameraSceneCfg = CartpoleRGBCameraSceneCfg(num_envs=512, env_spacing=20) observations: RGBObservationsCfg = RGBObservationsCfg() def __post_init__(self): @@ -150,7 +150,7 @@ def __post_init__(self): class CartpoleDepthCameraEnvCfg(CartpoleEnvCfg): """Configuration for the cartpole environment with depth camera.""" - scene: CartpoleDepthCameraSceneCfg = CartpoleDepthCameraSceneCfg(num_envs=1024, env_spacing=20) + scene: CartpoleDepthCameraSceneCfg = CartpoleDepthCameraSceneCfg(num_envs=512, env_spacing=20) observations: DepthObservationsCfg = DepthObservationsCfg() def __post_init__(self): diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py index 4cfe16baa9b4..788920af058c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -159,7 +159,7 @@ class CartpoleEnvCfg(ManagerBasedRLEnvCfg): """Configuration for the cartpole environment.""" # Scene settings - scene: CartpoleSceneCfg = CartpoleSceneCfg(num_envs=4096, env_spacing=4.0) + scene: CartpoleSceneCfg = CartpoleSceneCfg(num_envs=4096, env_spacing=4.0, clone_in_fabric=True) # Basic settings observations: ObservationsCfg = ObservationsCfg() actions: ActionsCfg = ActionsCfg() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/__init__.py index f81e2559cee3..155079c558f6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/rewards.py index acfc7389010c..5500089d7f94 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation from isaaclab.managers import SceneEntityCfg from isaaclab.utils.math import wrap_to_pi diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/symmetry.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/symmetry.py new file mode 100644 index 000000000000..3997d2ae1395 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/mdp/symmetry.py @@ -0,0 +1,68 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Functions to specify the symmetry in the observation and action space for cartpole.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +from tensordict import TensorDict + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +# specify the functions that are available for import +__all__ = ["compute_symmetric_states"] + + +@torch.no_grad() +def compute_symmetric_states( + env: ManagerBasedRLEnv, + obs: TensorDict | None = None, + actions: torch.Tensor | None = None, +): + """Augments the given observations and actions by applying symmetry transformations. + + This function creates augmented versions of the provided observations and actions by applying + two symmetrical transformations: original, left-right. The symmetry + transformations are beneficial for reinforcement learning tasks by providing additional + diverse data without requiring additional data collection. + + Args: + env: The environment instance. + obs: The original observation tensor dictionary. Defaults to None. + actions: The original actions tensor. Defaults to None. + + Returns: + Augmented observations and actions tensors, or None if the respective input was None. + """ + + # observations + if obs is not None: + batch_size = obs.batch_size[0] + # since we have 2 different symmetries, we need to augment the batch size by 2 + obs_aug = obs.repeat(2) + # -- original + obs_aug["policy"][:batch_size] = obs["policy"][:] + # -- left-right + obs_aug["policy"][batch_size : 2 * batch_size] = -obs["policy"] + else: + obs_aug = None + + # actions + if actions is not None: + batch_size = actions.shape[0] + # since we have 4 different symmetries, we need to augment the batch size by 4 + actions_aug = torch.zeros(batch_size * 2, actions.shape[1], device=actions.device) + # -- original + actions_aug[:batch_size] = actions[:] + # -- left-right + actions_aug[batch_size : 2 * batch_size] = -actions + else: + actions_aug = None + + return obs_aug, actions_aug diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/__init__.py index bbcb56fcdde3..67c17ab3bf34 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rl_games_ppo_cfg.yaml index 95e5f8c4b3b7..efb7a8afef89 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,16 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ========================================= IMPORTANT NOTICE ========================================= +# +# This file defines the agent configuration used to generate the "Training Performance" table in +# https://isaac-sim.github.io/IsaacLab/main/source/overview/reinforcement-learning/rl_frameworks.html. +# Ensure that the configurations for the other RL libraries are updated if this one is modified. +# +# ==================================================================================================== + params: seed: 42 @@ -45,13 +58,13 @@ params: device_name: 'cuda:0' multi_gpu: False ppo: True - mixed_precision: True + mixed_precision: False normalize_input: True normalize_value: True value_bootstrap: True num_actors: -1 reward_shaper: - scale_value: 0.6 + scale_value: 1.0 normalize_advantage: True gamma: 0.99 tau: 0.95 @@ -67,7 +80,7 @@ params: truncate_grads: True e_clip: 0.2 horizon_length: 32 - minibatch_size: 32768 + minibatch_size: 32768 # num_envs * horizon_length / num_mini_batches mini_epochs: 5 critic_coef: 4 clip_value: True diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py index 7f64ebe0aa0c..f2c7f48e4558 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py @@ -1,8 +1,18 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +""" +========================================= IMPORTANT NOTICE ========================================= + +This file defines the agent configuration used to generate the "Training Performance" table in +https://isaac-sim.github.io/IsaacLab/main/source/overview/reinforcement-learning/rl_frameworks.html. +Ensure that the configurations for the other RL libraries are updated if this one is modified. + +==================================================================================================== +""" + from isaaclab.utils import configclass from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg @@ -12,17 +22,18 @@ class HumanoidPPORunnerCfg(RslRlOnPolicyRunnerCfg): num_steps_per_env = 32 max_iterations = 1000 - save_interval = 50 + save_interval = 100 experiment_name = "humanoid" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, actor_hidden_dims=[400, 200, 100], critic_hidden_dims=[400, 200, 100], activation="elu", ) algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, + value_loss_coef=2.0, use_clipped_value_loss=True, clip_param=0.2, entropy_coef=0.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/sb3_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/sb3_ppo_cfg.yaml index b6db545ff228..cd8fd9887416 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/sb3_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/sb3_ppo_cfg.yaml @@ -1,22 +1,34 @@ -# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L245 -seed: 42 +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ========================================= IMPORTANT NOTICE ========================================= +# +# This file defines the agent configuration used to generate the "Training Performance" table in +# https://isaac-sim.github.io/IsaacLab/main/source/overview/reinforcement-learning/rl_frameworks.html. +# Ensure that the configurations for the other RL libraries are updated if this one is modified. +# +# ==================================================================================================== -policy: 'MlpPolicy' +seed: 42 +policy: "MlpPolicy" n_timesteps: !!float 5e7 -batch_size: 256 -n_steps: 512 +# For 4 minibatches with 4096 envs +# batch_size = (n_envs * n_steps) / n_minibatches = 32768 +n_minibatches: 4 +n_steps: 32 gamma: 0.99 -learning_rate: !!float 2.5e-4 +learning_rate: !!float 5e-4 ent_coef: 0.0 clip_range: 0.2 -n_epochs: 10 +n_epochs: 5 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]) - )" +vf_coef: 2.0 +policy_kwargs: + activation_fn: 'nn.ELU' + net_arch: [400, 200, 100] + optimizer_kwargs: + eps: !!float 1e-8 + ortho_init: False 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 9c668ca8315a..468421e4253f 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 @@ -1,3 +1,16 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ========================================= IMPORTANT NOTICE ========================================= +# +# This file defines the agent configuration used to generate the "Training Performance" table in +# https://isaac-sim.github.io/IsaacLab/main/source/overview/reinforcement-learning/rl_frameworks.html. +# Ensure that the configurations for the other RL libraries are updated if this one is modified. +# +# ==================================================================================================== + seed: 42 @@ -14,7 +27,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 200, 100] activations: elu output: ACTIONS @@ -23,7 +36,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 200, 100] activations: elu output: ONE @@ -62,14 +75,13 @@ agent: 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 + write_interval: 32 + checkpoint_interval: 3200 # Sequential trainer diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py index 07fa5f10da1a..37b9426df9b6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py @@ -1,11 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.assets import AssetBaseCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup @@ -16,10 +15,12 @@ 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 +from isaaclab_assets.robots.humanoid import HUMANOID_CFG # isort:skip + + ## # Scene definition ## @@ -39,56 +40,7 @@ class MySceneCfg(InteractiveSceneCfg): ) # 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, - }, - ), - }, - ) + robot = HUMANOID_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") # lights light = AssetBaseCfg( @@ -245,7 +197,7 @@ class HumanoidEnvCfg(ManagerBasedRLEnvCfg): """Configuration for the MuJoCo-style Humanoid walking environment.""" # Scene settings - scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=5.0) + scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=5.0, clone_in_fabric=True) # Basic settings observations: ObservationsCfg = ObservationsCfg() actions: ActionsCfg = ActionsCfg() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/__init__.py index d7f97391717b..9fd05f556350 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/observations.py index a53010940e7c..123c4eb7de34 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/observations.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils from isaaclab.assets import Articulation from isaaclab.managers import SceneEntityCfg @@ -50,7 +51,7 @@ def base_heading_proj( to_target_pos[:, 2] = 0.0 to_target_dir = math_utils.normalize(to_target_pos) # compute base forward vector - heading_vec = math_utils.quat_rotate(asset.data.root_quat_w, asset.data.FORWARD_VEC_B) + 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)) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/rewards.py index 04a61632b586..51b47d11449e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils import isaaclab.utils.string as string_utils from isaaclab.assets import Articulation @@ -82,10 +83,10 @@ class joint_pos_limits_penalty_ratio(ManagerTermBase): def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): # add default argument - if "asset_cfg" not in cfg.params: - cfg.params["asset_cfg"] = SceneEntityCfg("robot") + asset_cfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) # extract the used quantities (to enable type-hinting) - asset: Articulation = env.scene[cfg.params["asset_cfg"].name] + 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( @@ -95,7 +96,11 @@ def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): 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 + 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] @@ -118,10 +123,10 @@ class power_consumption(ManagerTermBase): def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): # add default argument - if "asset_cfg" not in cfg.params: - cfg.params["asset_cfg"] = SceneEntityCfg("robot") + asset_cfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) # extract the used quantities (to enable type-hinting) - asset: Articulation = env.scene[cfg.params["asset_cfg"].name] + 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( @@ -130,7 +135,9 @@ def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): 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) -> torch.Tensor: + 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) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/__init__.py new file mode 100644 index 000000000000..0fbb15561904 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Drone ARL environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.py new file mode 100644 index 000000000000..bc4d65f5b1f2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the drone ARL environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from isaaclab_contrib.mdp import * # noqa: F401, F403 + +from .commands import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/__init__.py new file mode 100644 index 000000000000..a7386d3ce539 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Various command terms that can be used in the environment.""" + +from .commands_cfg import DroneUniformPoseCommandCfg +from .drone_pose_command import DroneUniformPoseCommand diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/commands_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/commands_cfg.py new file mode 100644 index 000000000000..f12cf1be082f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/commands_cfg.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.envs.mdp.commands.commands_cfg import UniformPoseCommandCfg +from isaaclab.utils import configclass + +from .drone_pose_command import DroneUniformPoseCommand + + +@configclass +class DroneUniformPoseCommandCfg(UniformPoseCommandCfg): + """Configuration for uniform drone pose command generator.""" + + class_type: type = DroneUniformPoseCommand diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py new file mode 100644 index 000000000000..f33aa41be4c9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py @@ -0,0 +1,67 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command generators for pose tracking.""" + +from __future__ import annotations + +import torch + +from isaaclab.envs.mdp.commands.pose_command import UniformPoseCommand +from isaaclab.utils.math import combine_frame_transforms, compute_pose_error + + +class DroneUniformPoseCommand(UniformPoseCommand): + """Drone-specific UniformPoseCommand extensions. + + This class customizes the generic :class:`UniformPoseCommand` for drone (multirotor) + use-cases. Main differences and additions: + + - Transforms pose commands from the drone's base frame to the world frame before use. + - Accounts for per-environment origin offsets (``scene.env_origins``) when computing + position errors so tasks running on shifted/sub-terrain environments compute + meaningful errors. + - Computes and exposes simple metrics used by higher-level code: ``position_error`` + and ``orientation_error`` (stored in ``self.metrics``). + - Provides a debug visualization callback that renders the goal pose (with + sub-terrain shift) and current body pose using the existing visualizers. + + The implementation overrides :meth:`_update_metrics` and :meth:`_debug_vis_callback` + from the base class to implement these drone-specific behaviors. + """ + + 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( + # Sub-terrain shift for correct position error calculation @grzemal + self.pose_command_b[:, :3] + self._env.scene.env_origins, + 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 _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 + # Sub-terrain shift for visualization purposes @grzemal + self.goal_pose_visualizer.visualize( + self.pose_command_b[:, :3] + self._env.scene.env_origins, self.pose_command_b[:, 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py new file mode 100644 index 000000000000..866eb04e00d5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py @@ -0,0 +1,102 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to create drone 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 + +from typing import TYPE_CHECKING + +import torch +import torch.jit + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation +from isaaclab.managers import SceneEntityCfg + +from isaaclab_contrib.assets import Multirotor + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv + +from isaaclab.envs.utils.io_descriptors import generic_io_descriptor, record_shape + +""" +State. +""" + + +def base_roll_pitch(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Return the base roll and pitch in the simulation world frame. + + Parameters: + env: Manager-based environment providing the scene and tensors. + asset_cfg: Scene entity config pointing to the target robot (default: "robot"). + + Returns: + torch.Tensor: Shape (num_envs, 2). Column 0 is roll, column 1 is pitch. + Values are radians normalized to [-pi, pi], expressed in the world frame. + + Notes: + - Euler angles are computed from asset.data.root_quat_w using XYZ convention. + - Only roll and pitch are returned; yaw is omitted. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # extract euler angles (in world frame) + roll, pitch, _ = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) + # normalize angle to [-pi, pi] + roll = math_utils.wrap_to_pi(roll) + pitch = math_utils.wrap_to_pi(pitch) + + return torch.cat((roll.unsqueeze(-1), pitch.unsqueeze(-1)), dim=-1) + + +""" +Commands. +""" + + +@generic_io_descriptor(dtype=torch.float32, observation_type="Command", on_inspect=[record_shape]) +def generated_drone_commands( + env: ManagerBasedRLEnv, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Generate a body-frame direction and distance to the commanded position. + + This observation reads a command from env.command_manager identified by command_name, + interprets its first three components as a target position in the world frame, and + returns: + [dir_x, dir_y, dir_z, distance] + where dir_* is the unit vector from the current body origin to the target, expressed + in the multirotor body (root link) frame, and distance is the Euclidean separation. + + Parameters: + env: Manager-based RL environment providing scene and command manager. + command_name: Name of the command term to query from the command manager. + asset_cfg: Scene entity config for the multirotor asset (default: "robot"). + + Returns: + torch.Tensor: Shape (num_envs, 4) with body-frame unit direction (3) and distance (1). + + Frame conventions: + - Current position is asset.data.root_pos_w relative to env.scene.env_origins (world frame). + - Body orientation uses asset.data.root_link_quat_w to rotate world vectors into the body frame. + + Assumptions: + - env.command_manager.get_command(command_name) returns at least three values + representing a world-frame target position per environment. + - A small epsilon (1e-8) is used to guard against zero-length direction vectors. + """ + asset: Multirotor = env.scene[asset_cfg.name] + current_position_w = asset.data.root_pos_w - env.scene.env_origins + command = env.command_manager.get_command(command_name) + current_position_b = math_utils.quat_apply_inverse(asset.data.root_link_quat_w, command[:, :3] - current_position_w) + current_position_b_dir = current_position_b / (torch.norm(current_position_b, dim=-1, keepdim=True) + 1e-8) + current_position_b_mag = torch.norm(current_position_b, dim=-1, keepdim=True) + return torch.cat((current_position_b_dir, current_position_b_mag), dim=-1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py new file mode 100644 index 000000000000..4ad040563a42 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py @@ -0,0 +1,147 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +""" +Drone control rewards. +""" + + +def distance_to_goal_exp( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + std: float = 1.0, + command_name: str = "target_pose", +) -> torch.Tensor: + """Reward the distance to a goal position using an exponential kernel. + + This reward computes an exponential falloff of the squared Euclidean distance + between the commanded target position and the asset (robot) root position. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel; larger values + produce a gentler falloff. + command_name: Name of the command to read the target pose from the + environment's command manager. The function expects the command + tensor to contain positions in its first three columns. + + Returns: + A 1-D tensor of shape (num_envs,) containing the per-environment reward + values in [0, 1], with 1.0 when the position error is zero. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + + target_position_w = command[:, :3].clone() + current_position = asset.data.root_pos_w - env.scene.env_origins + + # compute the error + position_error_square = torch.sum(torch.square(target_position_w - current_position), dim=1) + return torch.exp(-position_error_square / std**2) + + +def ang_vel_xyz_exp( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 1.0 +) -> torch.Tensor: + """Penalize angular velocity magnitude with an exponential kernel. + + This reward computes exp(-||omega||^2 / std^2) where omega is the body-frame + angular velocity of the asset. It is useful for encouraging low rotational + rates. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel; controls + sensitivity to angular velocity magnitude. + + Returns: + A 1-D tensor of shape (num_envs,) with values in (0, 1], where 1 indicates + zero angular velocity. + """ + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # compute squared magnitude of angular velocity (all axes) + ang_vel_squared = torch.sum(torch.square(asset.data.root_ang_vel_b), dim=1) + + return torch.exp(-ang_vel_squared / std**2) + + +def lin_vel_xyz_exp( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 1.0 +) -> torch.Tensor: + """Penalize linear velocity magnitude with an exponential kernel. + + Computes exp(-||v||^2 / std^2) where v is the asset's linear velocity in + world frame. Useful for encouraging the agent to reduce translational speed. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel. + + Returns: + A 1-D tensor of shape (num_envs,) with values in (0, 1], where 1 indicates + zero linear velocity. + """ + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # compute squared magnitude of linear velocity (all axes) + lin_vel_squared = torch.sum(torch.square(asset.data.root_lin_vel_w), dim=1) + + return torch.exp(-lin_vel_squared / std**2) + + +def yaw_aligned( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 0.5 +) -> torch.Tensor: + """Reward alignment of the vehicle's yaw to zero using an exponential kernel. + + The function extracts the yaw (rotation about Z) from the world-frame root + quaternion and computes exp(-yaw^2 / std^2). This encourages heading + alignment to a zero-yaw reference. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel; smaller values + make the reward more sensitive to yaw deviations. + + Returns: + A 1-D tensor of shape (num_envs,) with values in (0, 1], where 1 indicates + perfect yaw alignment (yaw == 0). + """ + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # extract yaw from current orientation + _, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) + + # normalize yaw to [-pi, pi] (target is 0) + yaw = math_utils.wrap_to_pi(yaw) + + # return exponential reward (1 when yaw=0, approaching 0 when rotated) + return torch.exp(-(yaw**2) / std**2) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/__init__.py new file mode 100644 index 000000000000..36cc6105ac85 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Drone ARL state-based control environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/__init__.py new file mode 100644 index 000000000000..5d69799b587f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for state-based control environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/__init__.py new file mode 100644 index 000000000000..0dddd02bc027 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/__init__.py @@ -0,0 +1,36 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-TrackPositionNoObstacles-ARL-Robot-1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.no_obstacle_env_cfg:NoObstacleEnvCfg", + "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:TrackPositionNoObstaclesEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-TrackPositionNoObstacles-ARL-Robot-1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.no_obstacle_env_cfg:NoObstacleEnvCfg_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:TrackPositionNoObstaclesEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 000000000000..d454c4caee22 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,87 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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] + d2rl: False + activation: elu + initializer: + name: default + scale: 2 + rnn: + name: gru + units: 64 + layers: 1 + # before_mlp: False + # layer_norm: True + config: + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + env_config: + num_envs: 8192 + + name: arl_robot_1_track_position_state_based + reward_shaper: + # min_val: -1 + scale_value: 0.1 + + normalize_advantage: True + gamma: 0.98 + tau: 0.95 + ppo: True + learning_rate: 1e-4 + lr_schedule: adaptive + kl_threshold: 0.016 + save_best_after: 10 + score_to_win: 100000 + grad_norm: 1.0 + entropy_coef: 0 + truncate_grads: True + e_clip: 0.2 + clip_value: False + num_actors: 1024 + horizon_length: 32 + minibatch_size: 2048 + mini_epochs: 4 + critic_coef: 2 + normalize_input: True + bounds_loss_coef: 0.0001 + max_epochs: 1500 + normalize_value: True + use_diagnostics: True + value_bootstrap: True + #weight_decay: 0.0001 + use_smooth_clamp: False + + player: + render: True + deterministic: True + games_num: 100000 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..b53c53dbdd0c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class TrackPositionNoObstaclesEnvPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "arl_robot_1_track_position_state_based" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=0.5, + 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.001, + num_learning_epochs=4, + num_mini_batches=4, + learning_rate=4.0e-4, + schedule="adaptive", + gamma=0.98, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/skrl_ppo_cfg.yaml new file mode 100644 index 000000000000..3a5779e0106a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,95 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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: + 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: mlp + input: STATES + layers: [256, 128, 64] + activations: elu + - name: gru + input: mlp + type: GRU + layers: [64] + num_layers: 1 + output: ACTIONS + value: + class: DeterministicMixin + clip_actions: False + network: + - name: mlp + input: STATES + layers: [256, 128, 64] + activations: elu + - name: gru + input: mlp + type: GRU + layers: [64] + num_layers: 1 + 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: "arl_robot_1_track_position_state_based" + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/no_obstacle_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/no_obstacle_env_cfg.py new file mode 100644 index 000000000000..92a11d824420 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/no_obstacle_env_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_assets.robots.arl_robot_1 import ARL_ROBOT_1_CFG + +from .track_position_state_based_env_cfg import TrackPositionNoObstaclesEnvCfg + +## +# Pre-defined configs +## + + +@configclass +class NoObstacleEnvCfg(TrackPositionNoObstaclesEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # switch robot to arl_robot_1 + self.scene.robot = ARL_ROBOT_1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.actuators["thrusters"].dt = self.sim.dt + + +@configclass +class NoObstacleEnvCfg_PLAY(NoObstacleEnvCfg): + 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 + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/track_position_state_based_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/track_position_state_based_env_cfg.py new file mode 100644 index 000000000000..238ca65b5ef8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/track_position_state_based_env_cfg.py @@ -0,0 +1,229 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from dataclasses import MISSING + +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.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +from isaaclab_contrib.assets import MultirotorCfg + +import isaaclab_tasks.manager_based.drone_arl.mdp as mdp + + +## +# Scene definition +## +@configclass +class MySceneCfg(InteractiveSceneCfg): + """Configuration for the terrain scene with a flying robot.""" + + # robots + robot: MultirotorCfg = MISSING + + # 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.""" + + target_pose = mdp.DroneUniformPoseCommandCfg( + asset_name="robot", + body_name="base_link", + resampling_time_range=(10.0, 10.0), + debug_vis=True, + ranges=mdp.DroneUniformPoseCommandCfg.Ranges( + pos_x=(-0.0, 0.0), + pos_y=(-0.0, 0.0), + pos_z=(-0.0, 0.0), + roll=(-0.0, 0.0), + pitch=(-0.0, 0.0), + yaw=(-0.0, 0.0), + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + thrust_command = mdp.ThrustActionCfg( + asset_name="robot", + scale=3.0, + offset=3.0, + preserve_order=False, + use_default_offset=False, + clip={ + "back_left_prop": (0.0, 6.0), + "back_right_prop": (0.0, 6.0), + "front_left_prop": (0.0, 6.0), + "front_right_prop": (0.0, 6.0), + }, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + base_link_position = ObsTerm(func=mdp.root_pos_w, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_orientation = ObsTerm(func=mdp.root_quat_w, noise=Unoise(n_min=-0.1, n_max=0.1)) + 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.1, n_max=0.1)) + last_action = ObsTerm(func=mdp.last_action, noise=Unoise(n_min=-0.0, n_max=0.0)) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + # reset + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": (-1.0, 1.0), + "y": (-1.0, 1.0), + "z": (-1.0, 1.0), + "yaw": (-math.pi / 6.0, math.pi / 6.0), + "roll": (-math.pi / 6.0, math.pi / 6.0), + "pitch": (-math.pi / 6.0, math.pi / 6.0), + }, + "velocity_range": { + "x": (-0.2, 0.2), + "y": (-0.2, 0.2), + "z": (-0.2, 0.2), + "roll": (-0.2, 0.2), + "pitch": (-0.2, 0.2), + "yaw": (-0.2, 0.2), + }, + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + distance_to_goal_exp = RewTerm( + func=mdp.distance_to_goal_exp, + weight=25.0, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "std": 1.5, + "command_name": "target_pose", + }, + ) + flat_orientation_l2 = RewTerm( + func=mdp.flat_orientation_l2, + weight=1.0, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + yaw_aligned = RewTerm( + func=mdp.yaw_aligned, + weight=2.0, + params={"asset_cfg": SceneEntityCfg("robot"), "std": 1.0}, + ) + lin_vel_xyz_exp = RewTerm( + func=mdp.lin_vel_xyz_exp, + weight=2.5, + params={"asset_cfg": SceneEntityCfg("robot"), "std": 2.0}, + ) + ang_vel_xyz_exp = RewTerm( + func=mdp.ang_vel_xyz_exp, + weight=10.0, + params={"asset_cfg": SceneEntityCfg("robot"), "std": 10.0}, + ) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.05) + action_magnitude_l2 = RewTerm(func=mdp.action_l2, weight=-0.05) + + termination_penalty = RewTerm( + func=mdp.is_terminated, + weight=-5.0, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + crash = DoneTerm(func=mdp.root_height_below_minimum, params={"minimum_height": -3.0}) + + +## +# Environment configuration +## + + +@configclass +class TrackPositionNoObstaclesEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the state-based drone pose-control 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() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 10 + self.episode_length_s = 5.0 + # simulation settings + self.sim.dt = 0.01 + self.sim.render_interval = self.decimation + self.sim.physics_material = sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ) + self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15 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 000000000000..72b01368b499 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""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 000000000000..ff7e927b05e8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py @@ -0,0 +1,31 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""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 000000000000..c1dce5f832c8 --- /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 000000000000..b195334ba684 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/action_cfg.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..6fd0b6dbdf9d --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 000000000000..488d22c137b8 --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for 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 000000000000..87c100a6cec3 --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +import 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_assets.robots.unitree import G1_29DOF_CFG + +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" # noqa: E501 + + # 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 000000000000..9ff8102fe2e5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py @@ -0,0 +1,254 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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.g1_motion_controller_locomotion import ( + G1LowerBodyStandingMotionControllerRetargeterCfg, +) +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_motion_ctrl_retargeter import ( + G1TriHandUpperBodyMotionControllerRetargeterCfg, +) +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeterCfg, +) +from isaaclab.devices.openxr.xr_cfg import XrAnchorRotationMode +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_assets.robots.unitree import G1_29DOF_CFG + +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" # noqa: E501 + + # 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.xr.anchor_prim_path = "/World/envs/env_0/Robot/pelvis" + self.xr.fixed_anchor_height = True + # Ensure XR anchor rotation follows the robot pelvis (yaw only), with smoothing for comfort + self.xr.anchor_rotation_mode = XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED + + 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, + ), + "motion_controllers": OpenXRDeviceCfg( + retargeters=[ + G1TriHandUpperBodyMotionControllerRetargeterCfg( + enable_visualization=True, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + G1LowerBodyStandingMotionControllerRetargeterCfg( + 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 000000000000..7e559309b5cf --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""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 000000000000..64d27dbc2f2a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/actions.py @@ -0,0 +1,126 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +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 000000000000..d4b3f2b4bdf4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/observations.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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/locomanipulation/tracking/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/__init__.py new file mode 100644 index 000000000000..e952f370f823 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/__init__.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## +gym.register( + id="Isaac-Tracking-LocoManip-Digit-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.loco_manip_env_cfg:DigitLocoManipEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DigitLocoManipPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Tracking-LocoManip-Digit-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.loco_manip_env_cfg:DigitLocoManipEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DigitLocoManipPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/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 new file mode 100644 index 000000000000..c98c2030a2ca --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class DigitLocoManipPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 2000 + save_interval = 50 + experiment_name = "digit_loco_manip" + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, + actor_hidden_dims=[256, 128, 128], + critic_hidden_dims=[256, 128, 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, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/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 new file mode 100644 index 000000000000..a91ff0907dc7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/loco_manip_env_cfg.py @@ -0,0 +1,249 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +from isaaclab.managers import EventTermCfg, SceneEntityCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.utils import configclass +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +import isaaclab_tasks.manager_based.manipulation.reach.mdp as manipulation_mdp +from isaaclab_tasks.manager_based.locomotion.velocity.config.digit.rough_env_cfg import DigitRewards, DigitRoughEnvCfg +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import EventCfg + +from isaaclab_assets.robots.agility import ARM_JOINT_NAMES, LEG_JOINT_NAMES + + +@configclass +class DigitLocoManipRewards(DigitRewards): + joint_deviation_arms = None + + joint_vel_hip_yaw = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.001, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_leg_hip_yaw"])}, + ) + + left_ee_pos_tracking = RewTerm( + func=manipulation_mdp.position_command_error, + weight=-2.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="left_arm_wrist_yaw"), + "command_name": "left_ee_pose", + }, + ) + + left_ee_pos_tracking_fine_grained = RewTerm( + func=manipulation_mdp.position_command_error_tanh, + weight=2.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="left_arm_wrist_yaw"), + "std": 0.05, + "command_name": "left_ee_pose", + }, + ) + + left_end_effector_orientation_tracking = RewTerm( + func=manipulation_mdp.orientation_command_error, + weight=-0.2, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="left_arm_wrist_yaw"), + "command_name": "left_ee_pose", + }, + ) + + right_ee_pos_tracking = RewTerm( + func=manipulation_mdp.position_command_error, + weight=-2.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="right_arm_wrist_yaw"), + "command_name": "right_ee_pose", + }, + ) + + right_ee_pos_tracking_fine_grained = RewTerm( + func=manipulation_mdp.position_command_error_tanh, + weight=2.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="right_arm_wrist_yaw"), + "std": 0.05, + "command_name": "right_ee_pose", + }, + ) + + right_end_effector_orientation_tracking = RewTerm( + func=manipulation_mdp.orientation_command_error, + weight=-0.2, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="right_arm_wrist_yaw"), + "command_name": "right_ee_pose", + }, + ) + + +@configclass +class DigitLocoManipObservations: + """Configuration for the Digit Locomanipulation environment.""" + + @configclass + class PolicyCfg(ObsGroup): + 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"}, + ) + left_ee_pose_command = ObsTerm( + func=mdp.generated_commands, + params={"command_name": "left_ee_pose"}, + ) + right_ee_pose_command = ObsTerm( + func=mdp.generated_commands, + params={"command_name": "right_ee_pose"}, + ) + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=LEG_JOINT_NAMES + ARM_JOINT_NAMES)}, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=LEG_JOINT_NAMES + ARM_JOINT_NAMES)}, + noise=Unoise(n_min=-1.5, n_max=1.5), + ) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + policy = PolicyCfg() + + +@configclass +class DigitLocoManipCommands: + base_velocity = mdp.UniformVelocityCommandCfg( + asset_name="robot", + resampling_time_range=(10.0, 10.0), + rel_standing_envs=0.25, + rel_heading_envs=1.0, + heading_command=True, + 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), + ), + ) + + left_ee_pose = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="left_arm_wrist_yaw", + resampling_time_range=(1.0, 3.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.10, 0.50), + pos_y=(0.05, 0.50), + pos_z=(-0.20, 0.20), + roll=(-0.1, 0.1), + pitch=(-0.1, 0.1), + yaw=(math.pi / 2.0 - 0.1, math.pi / 2.0 + 0.1), + ), + ) + + right_ee_pose = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="right_arm_wrist_yaw", + resampling_time_range=(1.0, 3.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.10, 0.50), + pos_y=(-0.50, -0.05), + pos_z=(-0.20, 0.20), + roll=(-0.1, 0.1), + pitch=(-0.1, 0.1), + yaw=(-math.pi / 2.0 - 0.1, -math.pi / 2.0 + 0.1), + ), + ) + + +@configclass +class DigitEvents(EventCfg): + # Add an external force to simulate a payload being carried. + left_hand_force = EventTermCfg( + func=mdp.apply_external_force_torque, + mode="interval", + interval_range_s=(10.0, 15.0), + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="left_arm_wrist_yaw"), + "force_range": (-10.0, 10.0), + "torque_range": (-1.0, 1.0), + }, + ) + + right_hand_force = EventTermCfg( + func=mdp.apply_external_force_torque, + mode="interval", + interval_range_s=(10.0, 15.0), + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="right_arm_wrist_yaw"), + "force_range": (-10.0, 10.0), + "torque_range": (-1.0, 1.0), + }, + ) + + +@configclass +class DigitLocoManipEnvCfg(DigitRoughEnvCfg): + rewards: DigitLocoManipRewards = DigitLocoManipRewards() + observations: DigitLocoManipObservations = DigitLocoManipObservations() + commands: DigitLocoManipCommands = DigitLocoManipCommands() + + def __post_init__(self): + super().__post_init__() + + self.episode_length_s = 14.0 + + # Rewards: + self.rewards.flat_orientation_l2.weight = -10.5 + self.rewards.termination_penalty.weight = -100.0 + + # Change terrain to flat. + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # Remove height scanner. + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # Remove terrain curriculum. + self.curriculum.terrain_levels = None + + +class DigitLocoManipEnvCfg_PLAY(DigitLocoManipEnvCfg): + def __post_init__(self) -> None: + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/__init__.py index 062c8d1a858f..5ce57dc1bd56 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/__init__.py index 8186f0cc7c63..7971b7cd3640 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/__init__.py index b5dbf55024f4..26f3257daef4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/__init__.py index 423dde8532e7..99ead146751d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -19,6 +19,7 @@ "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", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", }, ) @@ -30,6 +31,7 @@ "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", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", }, ) @@ -41,6 +43,7 @@ "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", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", }, ) @@ -52,5 +55,6 @@ "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", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py index 364399b0d61a..972ebf937367 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class UnitreeA1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "unitree_a1_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/sb3_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/sb3_ppo_cfg.yaml new file mode 100644 index 000000000000..cc16b3ce79bd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/sb3_ppo_cfg.yaml @@ -0,0 +1,28 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Adapted from rsl_rl config +seed: 42 +n_timesteps: !!float 5e7 +policy: 'MlpPolicy' +n_steps: 24 +n_minibatches: 4 # batch_size=24576 for n_envs=4096 and n_steps=24 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 5 +ent_coef: 0.005 +learning_rate: !!float 1e-3 +clip_range: !!float 0.2 +policy_kwargs: + activation_fn: 'nn.ELU' + net_arch: [512, 256, 128] + optimizer_kwargs: + eps: !!float 1e-8 + ortho_init: False +vf_coef: 1.0 +max_grad_norm: 1.0 +normalize_input: True +normalize_value: False +clip_obs: 10.0 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 5e2a20e07645..eeb09d2b8a13 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 17dbaa0c8e65..e7eff6aa9a83 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/a1/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/flat_env_cfg.py index 252cd3619c10..eb239d363775 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py index bad8baa2c6fc..371ccb5b0cd4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -46,6 +46,7 @@ def __post_init__(self): "yaw": (0.0, 0.0), }, } + self.events.base_com = None # rewards self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/__init__.py index 46b745c5d1ee..e0f3eafd39e5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -17,6 +17,7 @@ kwargs={ "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalBFlatEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBFlatPPORunnerCfg", + "rsl_rl_with_symmetry_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBFlatPPORunnerWithSymmetryCfg", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", }, ) @@ -28,6 +29,7 @@ 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", + "rsl_rl_with_symmetry_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBFlatPPORunnerWithSymmetryCfg", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", }, ) @@ -39,6 +41,9 @@ kwargs={ "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalBRoughEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBRoughPPORunnerCfg", + "rsl_rl_with_symmetry_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBRoughPPORunnerWithSymmetryCfg" + ), "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", }, ) @@ -50,6 +55,9 @@ 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", + "rsl_rl_with_symmetry_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBRoughPPORunnerWithSymmetryCfg" + ), "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py index 1752357d0116..f6d0c585dd15 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py @@ -1,11 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg + +from isaaclab_tasks.manager_based.locomotion.velocity.mdp.symmetry import anymal @configclass @@ -14,9 +16,10 @@ class AnymalBRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_b_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", @@ -46,3 +49,51 @@ def __post_init__(self): self.experiment_name = "anymal_b_flat" self.policy.actor_hidden_dims = [128, 128, 128] self.policy.critic_hidden_dims = [128, 128, 128] + + +@configclass +class AnymalBFlatPPORunnerWithSymmetryCfg(AnymalBFlatPPORunnerCfg): + """Configuration for the PPO agent with symmetry augmentation.""" + + # all the other settings are inherited from the parent class + 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, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, data_augmentation_func=anymal.compute_symmetric_states + ), + ) + + +@configclass +class AnymalBRoughPPORunnerWithSymmetryCfg(AnymalBRoughPPORunnerCfg): + """Configuration for the PPO agent with symmetry augmentation.""" + + # all the other settings are inherited from the parent class + 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, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, data_augmentation_func=anymal.compute_symmetric_states + ), + ) 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 7715792ff6f4..4daadf173833 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 480b038e0d7e..51dd9c727236 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/flat_env_cfg.py index effaef2ccb95..134fd0154bf8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/rough_env_cfg.py index 429fc9b7efae..dd11ad858479 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/__init__.py index 8a3b8e92b158..39b8e5caeaa8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -18,6 +18,7 @@ kwargs={ "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalCFlatEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCFlatPPORunnerCfg", + "rsl_rl_with_symmetry_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCFlatPPORunnerWithSymmetryCfg", "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", }, @@ -29,8 +30,9 @@ 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", + "rsl_rl_with_symmetry_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCFlatPPORunnerWithSymmetryCfg", + "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", }, ) @@ -41,8 +43,11 @@ 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", + "rsl_rl_with_symmetry_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCRoughPPORunnerWithSymmetryCfg" + ), + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_rough_ppo_cfg.yaml", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", }, ) @@ -53,8 +58,11 @@ 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", + "rsl_rl_with_symmetry_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCRoughPPORunnerWithSymmetryCfg" + ), + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_rough_ppo_cfg.yaml", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_flat_ppo_cfg.yaml index c472ce673cf5..95916252ac26 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_flat_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_rough_ppo_cfg.yaml index 042799da1ee4..54a9847bbef2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_rough_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py index 09bd622048da..45f434fe7f0d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -1,11 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg + +from isaaclab_tasks.manager_based.locomotion.velocity.mdp.symmetry import anymal @configclass @@ -14,9 +16,10 @@ class AnymalCRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_c_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", @@ -46,3 +49,51 @@ def __post_init__(self): self.experiment_name = "anymal_c_flat" self.policy.actor_hidden_dims = [128, 128, 128] self.policy.critic_hidden_dims = [128, 128, 128] + + +@configclass +class AnymalCFlatPPORunnerWithSymmetryCfg(AnymalCFlatPPORunnerCfg): + """Configuration for the PPO agent with symmetry augmentation.""" + + # all the other settings are inherited from the parent class + 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, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, data_augmentation_func=anymal.compute_symmetric_states + ), + ) + + +@configclass +class AnymalCRoughPPORunnerWithSymmetryCfg(AnymalCRoughPPORunnerCfg): + """Configuration for the PPO agent with symmetry augmentation.""" + + # all the other settings are inherited from the parent class + 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, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, data_augmentation_func=anymal.compute_symmetric_states + ), + ) 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 cb90016afa57..087eed1e266c 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 d7c6faadeab6..1baab1c851bc 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/flat_env_cfg.py index 34a64373d997..76ccb79b48ab 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/rough_env_cfg.py index 31279fdce935..ed62e06fc944 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/__init__.py index 60e41b7f1640..41b0398e5206 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -18,6 +18,10 @@ kwargs={ "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalDFlatEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDFlatPPORunnerCfg", + "rsl_rl_distillation_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_distillation_cfg:AnymalDFlatDistillationRunnerCfg" + ), + "rsl_rl_with_symmetry_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDFlatPPORunnerWithSymmetryCfg", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", }, ) @@ -29,6 +33,10 @@ 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", + "rsl_rl_distillation_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_distillation_cfg:AnymalDFlatDistillationRunnerCfg" + ), + "rsl_rl_with_symmetry_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDFlatPPORunnerWithSymmetryCfg", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", }, ) @@ -40,6 +48,9 @@ kwargs={ "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalDRoughEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDRoughPPORunnerCfg", + "rsl_rl_with_symmetry_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDRoughPPORunnerWithSymmetryCfg" + ), "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", }, ) @@ -51,6 +62,9 @@ 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", + "rsl_rl_with_symmetry_cfg_entry_point": ( + f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDRoughPPORunnerWithSymmetryCfg" + ), "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_distillation_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_distillation_cfg.py new file mode 100644 index 000000000000..ea3d5f521ac3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_distillation_cfg.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import ( + RslRlDistillationAlgorithmCfg, + RslRlDistillationRunnerCfg, + RslRlDistillationStudentTeacherCfg, +) + + +@configclass +class AnymalDFlatDistillationRunnerCfg(RslRlDistillationRunnerCfg): + num_steps_per_env = 120 + max_iterations = 300 + save_interval = 50 + experiment_name = "anymal_d_flat" + obs_groups = {"policy": ["policy"], "teacher": ["policy"]} + policy = RslRlDistillationStudentTeacherCfg( + init_noise_std=0.1, + noise_std_type="scalar", + student_obs_normalization=False, + teacher_obs_normalization=False, + student_hidden_dims=[128, 128, 128], + teacher_hidden_dims=[128, 128, 128], + activation="elu", + ) + algorithm = RslRlDistillationAlgorithmCfg( + num_learning_epochs=2, + learning_rate=1.0e-3, + gradient_length=15, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py index e2c57102b461..220efdd6e8c4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py @@ -1,11 +1,13 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg + +from isaaclab_tasks.manager_based.locomotion.velocity.mdp.symmetry import anymal @configclass @@ -14,9 +16,10 @@ class AnymalDRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_d_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", @@ -46,3 +49,50 @@ def __post_init__(self): self.experiment_name = "anymal_d_flat" self.policy.actor_hidden_dims = [128, 128, 128] self.policy.critic_hidden_dims = [128, 128, 128] + + +@configclass +class AnymalDFlatPPORunnerWithSymmetryCfg(AnymalDFlatPPORunnerCfg): + """Configuration for the PPO agent with symmetry augmentation.""" + + 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, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, data_augmentation_func=anymal.compute_symmetric_states + ), + ) + + +@configclass +class AnymalDRoughPPORunnerWithSymmetryCfg(AnymalDRoughPPORunnerCfg): + """Configuration for the PPO agent with symmetry augmentation.""" + + # all the other settings are inherited from the parent class + 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, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, data_augmentation_func=anymal.compute_symmetric_states + ), + ) 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 b770447834b4..f5510e337706 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 465c0c080dd6..a612f624db17 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/flat_env_cfg.py index 430c2d7c4ac1..7abab44fdb9b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py index 9ec9a1a51b71..c672dcacc0ce 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/__init__.py index d8661a39042b..7cb2be156961 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py index 16c28a8a3af3..93cce1bb9294 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class CassieRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "cassie_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", 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 2c7eb12c32de..0f55bd81a18c 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 0b5686f39718..ddd65baaa3ab 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/flat_env_cfg.py index 09724fe70def..5ca23455cd0e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py index 905710ff4b6e..2a13f35213ca 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -77,6 +77,7 @@ def __post_init__(self): "yaw": (0.0, 0.0), }, } + self.events.base_com = None # terminations self.terminations.base_contact.params["sensor_cfg"].body_names = [".*pelvis"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/__init__.py new file mode 100644 index 000000000000..8311b225698d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/__init__.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## +gym.register( + id="Isaac-Velocity-Flat-Digit-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:DigitFlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DigitFlatPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Velocity-Flat-Digit-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:DigitFlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DigitFlatPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Velocity-Rough-Digit-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:DigitRoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DigitRoughPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Velocity-Rough-Digit-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:DigitRoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DigitRoughPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..72eb4a2aa3ff --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,50 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class DigitRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 3000 + save_interval = 50 + experiment_name = "digit_rough" + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, + 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 DigitFlatPPORunnerCfg(DigitRoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 2000 + self.experiment_name = "digit_flat" + + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/flat_env_cfg.py new file mode 100644 index 000000000000..48a647e17a64 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/flat_env_cfg.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import DigitRoughEnvCfg + + +@configclass +class DigitFlatEnvCfg(DigitRoughEnvCfg): + def __post_init__(self): + super().__post_init__() + + # Change terrain to flat. + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # Remove height scanner. + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # Remove terrain curriculum. + self.curriculum.terrain_levels = None + + +class DigitFlatEnvCfg_PLAY(DigitFlatEnvCfg): + def __post_init__(self) -> None: + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py new file mode 100644 index 000000000000..792e6f639479 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py @@ -0,0 +1,266 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +from isaaclab.managers import ObservationGroupCfg, ObservationTermCfg, RewardTermCfg, SceneEntityCfg, TerminationTermCfg +from isaaclab.utils import configclass +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +from isaaclab_assets.robots.agility import ARM_JOINT_NAMES, DIGIT_V4_CFG, LEG_JOINT_NAMES + + +@configclass +class DigitRewards: + termination_penalty = RewardTermCfg( + func=mdp.is_terminated, + weight=-100.0, + ) + track_lin_vel_xy_exp = RewardTermCfg( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=1.0, + params={"command_name": "base_velocity", "std": math.sqrt(0.25)}, + ) + track_ang_vel_z_exp = RewardTermCfg( + func=mdp.track_ang_vel_z_world_exp, + weight=1.0, + params={ + "command_name": "base_velocity", + "std": math.sqrt(0.25), + }, + ) + feet_air_time = RewardTermCfg( + func=mdp.feet_air_time_positive_biped, + weight=0.25, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_leg_toe_roll"), + "threshold": 0.8, + "command_name": "base_velocity", + }, + ) + feet_slide = RewardTermCfg( + func=mdp.feet_slide, + weight=-0.25, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_leg_toe_roll"), + "asset_cfg": SceneEntityCfg("robot", body_names=".*_leg_toe_roll"), + }, + ) + dof_torques_l2 = RewardTermCfg( + func=mdp.joint_torques_l2, + weight=-1.0e-6, + ) + dof_acc_l2 = RewardTermCfg( + func=mdp.joint_acc_l2, + weight=-2.0e-7, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=LEG_JOINT_NAMES + ARM_JOINT_NAMES)}, + ) + action_rate_l2 = RewardTermCfg( + func=mdp.action_rate_l2, + weight=-0.008, + ) + flat_orientation_l2 = RewardTermCfg( + func=mdp.flat_orientation_l2, + weight=-2.5, + ) + stand_still = RewardTermCfg( + func=mdp.stand_still_joint_deviation_l1, + weight=-0.4, + params={ + "command_name": "base_velocity", + "asset_cfg": SceneEntityCfg("robot", joint_names=LEG_JOINT_NAMES), + }, + ) + lin_vel_z_l2 = RewardTermCfg( + func=mdp.lin_vel_z_l2, + weight=-2.0, + ) + ang_vel_xy_l2 = RewardTermCfg( + func=mdp.ang_vel_xy_l2, + weight=-0.1, + ) + no_jumps = RewardTermCfg( + func=mdp.desired_contacts, + weight=-0.5, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[".*_leg_toe_roll"])}, + ) + dof_pos_limits = RewardTermCfg( + func=mdp.joint_pos_limits, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_leg_toe_roll", ".*_leg_toe_pitch", ".*_tarsus"])}, + ) + joint_deviation_hip_roll = RewardTermCfg( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_leg_hip_roll")}, + ) + joint_deviation_hip_yaw = RewardTermCfg( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_leg_hip_yaw")}, + ) + joint_deviation_knee = RewardTermCfg( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_tarsus")}, + ) + joint_deviation_feet = RewardTermCfg( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_toe_a", ".*_toe_b"])}, + ) + joint_deviation_arms = RewardTermCfg( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=".*_arm_.*"), + }, + ) + + undesired_contacts = RewardTermCfg( + func=mdp.undesired_contacts, + weight=-0.1, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=[".*_rod", ".*_tarsus"]), + "threshold": 1.0, + }, + ) + + +@configclass +class DigitObservations: + @configclass + class PolicyCfg(ObservationGroupCfg): + base_lin_vel = ObservationTermCfg( + func=mdp.base_lin_vel, + noise=Unoise(n_min=-0.1, n_max=0.1), + ) + base_ang_vel = ObservationTermCfg( + func=mdp.base_ang_vel, + noise=Unoise(n_min=-0.2, n_max=0.2), + ) + projected_gravity = ObservationTermCfg( + func=mdp.projected_gravity, + noise=Unoise(n_min=-0.05, n_max=0.05), + ) + velocity_commands = ObservationTermCfg( + func=mdp.generated_commands, + params={"command_name": "base_velocity"}, + ) + joint_pos = ObservationTermCfg( + func=mdp.joint_pos_rel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=LEG_JOINT_NAMES + ARM_JOINT_NAMES)}, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + joint_vel = ObservationTermCfg( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=LEG_JOINT_NAMES + ARM_JOINT_NAMES)}, + noise=Unoise(n_min=-1.5, n_max=1.5), + ) + actions = ObservationTermCfg(func=mdp.last_action) + height_scan = ObservationTermCfg( + 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 TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = TerminationTermCfg(func=mdp.time_out, time_out=True) + base_contact = TerminationTermCfg( + func=mdp.illegal_contact, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["torso_base"]), + "threshold": 1.0, + }, + ) + base_orientation = TerminationTermCfg( + func=mdp.bad_orientation, + params={"limit_angle": 0.7}, + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_pos = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=LEG_JOINT_NAMES + ARM_JOINT_NAMES, + scale=0.5, + use_default_offset=True, + ) + + +@configclass +class DigitRoughEnvCfg(LocomotionVelocityRoughEnvCfg): + rewards: DigitRewards = DigitRewards() + observations: DigitObservations = DigitObservations() + terminations: TerminationsCfg = TerminationsCfg() + actions: ActionsCfg = ActionsCfg() + + def __post_init__(self): + super().__post_init__() + self.decimation = 4 + self.sim.dt = 0.005 + + # Scene + self.scene.robot = DIGIT_V4_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_base" + self.scene.contact_forces.history_length = self.decimation + self.scene.contact_forces.update_period = self.sim.dt + self.scene.height_scanner.update_period = self.decimation * self.sim.dt + + # Events: + self.events.add_base_mass.params["asset_cfg"] = SceneEntityCfg("robot", body_names="torso_base") + self.events.base_external_force_torque.params["asset_cfg"] = SceneEntityCfg("robot", body_names="torso_base") + # Don't randomize the initial joint positions because we have closed loops. + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + # remove COM randomization + self.events.base_com = None + + # Commands + self.commands.base_velocity.ranges.lin_vel_x = (-0.8, 0.8) + self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + self.commands.base_velocity.rel_standing_envs = 0.1 + self.commands.base_velocity.resampling_time_range = (3.0, 8.0) + + +@configclass +class DigitRoughEnvCfg_PLAY(DigitRoughEnvCfg): + def __post_init__(self): + 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. + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/__init__.py index 1a31ab9759c3..30861ec5a3a5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py index 22f23703ffc9..61a6d0261b9f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class G1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 3000 save_interval = 50 experiment_name = "g1_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", 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 a25b969a9129..711b7190245e 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 128] activations: elu output: ACTIONS @@ -23,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 07e71559e523..b54682b45cd1 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/flat_env_cfg.py index 060e6e7c9915..e8d3b5edc451 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py index 1eff2bf55550..04971c3d9f20 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -127,6 +127,7 @@ def __post_init__(self): "yaw": (0.0, 0.0), }, } + self.events.base_com = None # Rewards self.rewards.lin_vel_z_l2.weight = 0.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/__init__.py index 658d60ea6b0b..def24b8e1442 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py index 10ae1865b079..9baa2b371ea3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class UnitreeGo1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "unitree_go1_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", 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 dabee2d24bde..d125c913446f 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 bd87c9b22b77..47888d623e91 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/flat_env_cfg.py index 917d05448a4f..760c1f5f5d07 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py index 5badde697101..91efcc170245 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -46,6 +46,7 @@ def __post_init__(self): "yaw": (0.0, 0.0), }, } + self.events.base_com = None # rewards self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/__init__.py index 9e5ed3293be1..4ea7d3fce718 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py index e41f8092e6ee..9777785f7e30 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class UnitreeGo2RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "unitree_go2_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", 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 fea8fcc70c2b..e36d3a574867 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 808974198ec7..4c89ca249f01 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/flat_env_cfg.py index cb0820c6290b..8bf8bb1373f9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py index 7d99166531df..69e6adddd042 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -46,6 +46,7 @@ def __post_init__(self): "yaw": (0.0, 0.0), }, } + self.events.base_com = None # rewards self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/__init__.py index 7e9bcc48fdaa..6a218243e371 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py index bde55dd7ef7c..102359770864 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class H1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 3000 save_interval = 50 experiment_name = "h1_rough" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", 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 c509d4ee3fa4..ed1dbeb89d11 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -23,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 a841751fd6cf..c5f49d24efdb 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/flat_env_cfg.py index 18c78161c7c4..e9b9e2a1fa27 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py index d4b9465244c8..799a7b95cc40 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -95,9 +95,7 @@ def __post_init__(self): "yaw": (0.0, 0.0), }, } - - # Terminations - self.terminations.base_contact.params["sensor_cfg"].body_names = [".*torso_link"] + self.events.base_com = None # Rewards self.rewards.undesired_contacts = None @@ -111,7 +109,7 @@ def __post_init__(self): 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 + # Terminations self.terminations.base_contact.params["sensor_cfg"].body_names = ".*torso_link" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/__init__.py index 4aed450b2a18..28572a7dfa5d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py index 8552dc6a79d0..3985f6b3b491 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,10 +14,11 @@ class SpotFlatPPORunnerCfg(RslRlOnPolicyRunnerCfg): 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_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[512, 256, 128], critic_hidden_dims=[512, 256, 128], activation="elu", 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 e412959c3680..dcbf8926268b 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py index c24f3ab3db0b..6bf334e24536 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -295,8 +295,9 @@ class SpotTerminationsCfg: @configclass class SpotFlatEnvCfg(LocomotionVelocityRoughEnvCfg): + """Configuration for the Spot robot in a flat environment.""" - # Basic settings' + # Basic settings observations: SpotObservationsCfg = SpotObservationsCfg() actions: SpotActionsCfg = SpotActionsCfg() commands: SpotCommandsCfg = SpotCommandsCfg() @@ -375,5 +376,3 @@ def __post_init__(self) -> None: # 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/__init__.py index a53f3c1b4287..cf460b5f33fe 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/events.py index 7f15bbe2518e..b1a47934d95e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/events.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -12,9 +12,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation from isaaclab.managers import SceneEntityCfg from isaaclab.utils.math import sample_uniform diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py index 3d83ca05b102..05680e437355 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,9 +11,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation, RigidObject from isaaclab.managers import ManagerTermBase, SceneEntityCfg from isaaclab.sensors import ContactSensor @@ -86,9 +87,10 @@ def base_linear_velocity_reward( 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. + 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): diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py index 928e70431784..6f6cad007128 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py index e655ce385733..88187a6b816b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,10 +11,11 @@ from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation from isaaclab.managers import SceneEntityCfg from isaaclab.terrains import TerrainImporter diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py index a4245a206fbd..f804aa6884c5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,12 +11,14 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + +from isaaclab.envs import mdp from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import ContactSensor -from isaaclab.utils.math import quat_rotate_inverse, yaw_quat +from isaaclab.utils.math import quat_apply_inverse, yaw_quat if TYPE_CHECKING: from isaaclab.envs import ManagerBasedRLEnv @@ -86,10 +88,12 @@ def feet_slide(env, sensor_cfg: SceneEntityCfg, asset_cfg: SceneEntityCfg = Scen 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.""" + """Reward tracking of linear velocity commands (xy axes) in the gravity aligned + robot frame using an exponential kernel. + """ # extract the used quantities (to enable type-hinting) asset = env.scene[asset_cfg.name] - vel_yaw = quat_rotate_inverse(yaw_quat(asset.data.root_quat_w), asset.data.root_lin_vel_w[:, :3]) + 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 ) @@ -104,3 +108,12 @@ def track_ang_vel_z_world_exp( 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) + + +def stand_still_joint_deviation_l1( + env, command_name: str, command_threshold: float = 0.06, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Penalize offsets from the default joint positions when the command is very small.""" + command = env.command_manager.get_command(command_name) + # Penalize motion when command is nearly zero. + return mdp.joint_deviation_l1(env, asset_cfg) * (torch.norm(command[:, :2], dim=1) < command_threshold) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/symmetry/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/symmetry/__init__.py new file mode 100644 index 000000000000..abbd6c26ca51 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/symmetry/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Symmetry functions for the velocity tasks. + +These functions are used to augment the observations and actions of the environment. +They are specific to the velocity task and the choice of the robot. +""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/symmetry/anymal.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/symmetry/anymal.py new file mode 100644 index 000000000000..f4197ccbe76e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/symmetry/anymal.py @@ -0,0 +1,261 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Functions to specify the symmetry in the observation and action space for ANYmal.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +from tensordict import TensorDict + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +# specify the functions that are available for import +__all__ = ["compute_symmetric_states"] + + +@torch.no_grad() +def compute_symmetric_states( + env: ManagerBasedRLEnv, + obs: TensorDict | None = None, + actions: torch.Tensor | None = None, +): + """Augments the given observations and actions by applying symmetry transformations. + + This function creates augmented versions of the provided observations and actions by applying + four symmetrical transformations: original, left-right, front-back, and diagonal. The symmetry + transformations are beneficial for reinforcement learning tasks by providing additional + diverse data without requiring additional data collection. + + Args: + env: The environment instance. + obs: The original observation tensor dictionary. Defaults to None. + actions: The original actions tensor. Defaults to None. + + Returns: + Augmented observations and actions tensors, or None if the respective input was None. + """ + + # observations + if obs is not None: + batch_size = obs.batch_size[0] + # since we have 4 different symmetries, we need to augment the batch size by 4 + obs_aug = obs.repeat(4) + + # policy observation group + # -- original + obs_aug["policy"][:batch_size] = obs["policy"][:] + # -- left-right + obs_aug["policy"][batch_size : 2 * batch_size] = _transform_policy_obs_left_right(env.unwrapped, obs["policy"]) + # -- front-back + obs_aug["policy"][2 * batch_size : 3 * batch_size] = _transform_policy_obs_front_back( + env.unwrapped, obs["policy"] + ) + # -- diagonal + obs_aug["policy"][3 * batch_size :] = _transform_policy_obs_front_back( + env.unwrapped, obs_aug["policy"][batch_size : 2 * batch_size] + ) + else: + obs_aug = None + + # actions + if actions is not None: + batch_size = actions.shape[0] + # since we have 4 different symmetries, we need to augment the batch size by 4 + actions_aug = torch.zeros(batch_size * 4, actions.shape[1], device=actions.device) + # -- original + actions_aug[:batch_size] = actions[:] + # -- left-right + actions_aug[batch_size : 2 * batch_size] = _transform_actions_left_right(actions) + # -- front-back + actions_aug[2 * batch_size : 3 * batch_size] = _transform_actions_front_back(actions) + # -- diagonal + actions_aug[3 * batch_size :] = _transform_actions_front_back(actions_aug[batch_size : 2 * batch_size]) + else: + actions_aug = None + + return obs_aug, actions_aug + + +""" +Symmetry functions for observations. +""" + + +def _transform_policy_obs_left_right(env: ManagerBasedRLEnv, obs: torch.Tensor) -> torch.Tensor: + """Apply a left-right symmetry transformation to the observation tensor. + + This function modifies the given observation tensor by applying transformations + that represent a symmetry with respect to the left-right axis. This includes + negating certain components of the linear and angular velocities, projected gravity, + velocity commands, and flipping the joint positions, joint velocities, and last actions + for the ANYmal robot. Additionally, if height-scan data is present, it is flipped + along the relevant dimension. + + Args: + env: The environment instance from which the observation is obtained. + obs: The observation tensor to be transformed. + + Returns: + The transformed observation tensor with left-right symmetry applied. + """ + # copy observation tensor + obs = obs.clone() + device = obs.device + # lin vel + obs[:, :3] = obs[:, :3] * torch.tensor([1, -1, 1], device=device) + # ang vel + obs[:, 3:6] = obs[:, 3:6] * torch.tensor([-1, 1, -1], device=device) + # projected gravity + obs[:, 6:9] = obs[:, 6:9] * torch.tensor([1, -1, 1], device=device) + # velocity command + obs[:, 9:12] = obs[:, 9:12] * torch.tensor([1, -1, -1], device=device) + # joint pos + obs[:, 12:24] = _switch_anymal_joints_left_right(obs[:, 12:24]) + # joint vel + obs[:, 24:36] = _switch_anymal_joints_left_right(obs[:, 24:36]) + # last actions + obs[:, 36:48] = _switch_anymal_joints_left_right(obs[:, 36:48]) + + # note: this is hard-coded for grid-pattern of ordering "xy" and size (1.6, 1.0) + if "height_scan" in env.observation_manager.active_terms["policy"]: + obs[:, 48:235] = obs[:, 48:235].view(-1, 11, 17).flip(dims=[1]).view(-1, 11 * 17) + + return obs + + +def _transform_policy_obs_front_back(env: ManagerBasedRLEnv, obs: torch.Tensor) -> torch.Tensor: + """Applies a front-back symmetry transformation to the observation tensor. + + This function modifies the given observation tensor by applying transformations + that represent a symmetry with respect to the front-back axis. This includes negating + certain components of the linear and angular velocities, projected gravity, velocity commands, + and flipping the joint positions, joint velocities, and last actions for the ANYmal robot. + Additionally, if height-scan data is present, it is flipped along the relevant dimension. + + Args: + env: The environment instance from which the observation is obtained. + obs: The observation tensor to be transformed. + + Returns: + The transformed observation tensor with front-back symmetry applied. + """ + # copy observation tensor + obs = obs.clone() + device = obs.device + # lin vel + obs[:, :3] = obs[:, :3] * torch.tensor([-1, 1, 1], device=device) + # ang vel + obs[:, 3:6] = obs[:, 3:6] * torch.tensor([1, -1, -1], device=device) + # projected gravity + obs[:, 6:9] = obs[:, 6:9] * torch.tensor([-1, 1, 1], device=device) + # velocity command + obs[:, 9:12] = obs[:, 9:12] * torch.tensor([-1, 1, -1], device=device) + # joint pos + obs[:, 12:24] = _switch_anymal_joints_front_back(obs[:, 12:24]) + # joint vel + obs[:, 24:36] = _switch_anymal_joints_front_back(obs[:, 24:36]) + # last actions + obs[:, 36:48] = _switch_anymal_joints_front_back(obs[:, 36:48]) + + # note: this is hard-coded for grid-pattern of ordering "xy" and size (1.6, 1.0) + if "height_scan" in env.observation_manager.active_terms["policy"]: + obs[:, 48:235] = obs[:, 48:235].view(-1, 11, 17).flip(dims=[2]).view(-1, 11 * 17) + + return obs + + +""" +Symmetry functions for actions. +""" + + +def _transform_actions_left_right(actions: torch.Tensor) -> torch.Tensor: + """Applies a left-right symmetry transformation to the actions tensor. + + This function modifies the given actions tensor by applying transformations + that represent a symmetry with respect to the left-right axis. This includes + flipping the joint positions, joint velocities, and last actions for the + ANYmal robot. + + Args: + actions: The actions tensor to be transformed. + + Returns: + The transformed actions tensor with left-right symmetry applied. + """ + actions = actions.clone() + actions[:] = _switch_anymal_joints_left_right(actions[:]) + return actions + + +def _transform_actions_front_back(actions: torch.Tensor) -> torch.Tensor: + """Applies a front-back symmetry transformation to the actions tensor. + + This function modifies the given actions tensor by applying transformations + that represent a symmetry with respect to the front-back axis. This includes + flipping the joint positions, joint velocities, and last actions for the + ANYmal robot. + + Args: + actions: The actions tensor to be transformed. + + Returns: + The transformed actions tensor with front-back symmetry applied. + """ + actions = actions.clone() + actions[:] = _switch_anymal_joints_front_back(actions[:]) + return actions + + +""" +Helper functions for symmetry. + +In Isaac Sim, the joint ordering is as follows: +[ + 'LF_HAA', 'LH_HAA', 'RF_HAA', 'RH_HAA', + 'LF_HFE', 'LH_HFE', 'RF_HFE', 'RH_HFE', + 'LF_KFE', 'LH_KFE', 'RF_KFE', 'RH_KFE' +] + +Correspondingly, the joint ordering for the ANYmal robot is: + +* LF = left front --> [0, 4, 8] +* LH = left hind --> [1, 5, 9] +* RF = right front --> [2, 6, 10] +* RH = right hind --> [3, 7, 11] +""" + + +def _switch_anymal_joints_left_right(joint_data: torch.Tensor) -> torch.Tensor: + """Applies a left-right symmetry transformation to the joint data tensor.""" + joint_data_switched = torch.zeros_like(joint_data) + # left <-- right + joint_data_switched[..., [0, 4, 8, 1, 5, 9]] = joint_data[..., [2, 6, 10, 3, 7, 11]] + # right <-- left + joint_data_switched[..., [2, 6, 10, 3, 7, 11]] = joint_data[..., [0, 4, 8, 1, 5, 9]] + + # Flip the sign of the HAA joints + joint_data_switched[..., [0, 1, 2, 3]] *= -1.0 + + return joint_data_switched + + +def _switch_anymal_joints_front_back(joint_data: torch.Tensor) -> torch.Tensor: + """Applies a front-back symmetry transformation to the joint data tensor.""" + joint_data_switched = torch.zeros_like(joint_data) + # front <-- hind + joint_data_switched[..., [0, 4, 8, 2, 6, 10]] = joint_data[..., [1, 5, 9, 3, 7, 11]] + # hind <-- front + joint_data_switched[..., [1, 5, 9, 3, 7, 11]] = joint_data[..., [0, 4, 8, 2, 6, 10]] + + # Flip the sign of the HFE and KFE joints + joint_data_switched[..., 4:] *= -1 + + return joint_data_switched diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py index cc305e97c4b3..6c037d01ea51 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,9 +11,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import RigidObject from isaaclab.managers import SceneEntityCfg @@ -30,7 +31,8 @@ def terrain_out_of_bounds( 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 + # we have infinite terrain because it is a plane + return torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) elif env.scene.cfg.terrain.terrain_type == "generator": # obtain the size of the sub-terrains terrain_gen_cfg = env.scene.terrain.cfg.terrain_generator diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index 591716ef1e58..d7094e777014 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -66,7 +66,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=False, mesh_prim_paths=["/World/ground"], @@ -173,6 +173,15 @@ class EventCfg: }, ) + 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, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/__init__.py index ca9539a3a55e..eaf0b09fbb66 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/__init__.py index db34a1826b31..7ea0d7159142 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index cfdaa4066f6d..85b7e5ae9ba8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,7 +7,7 @@ from dataclasses import MISSING import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import EventTermCfg as EventTerm @@ -71,15 +71,13 @@ class CabinetSceneCfg(InteractiveSceneCfg): actuators={ "drawers": ImplicitActuatorCfg( joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], - effort_limit=87.0, - velocity_limit=100.0, + effort_limit_sim=87.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, + effort_limit_sim=87.0, stiffness=10.0, damping=2.5, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/__init__.py index aab8e27eebf2..d7c38f5c0b04 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py index 980500ed06ab..1f8b763b4228 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rl_games_ppo_cfg.yaml index 69fbb2a236fc..8fc9b9773d5d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py index 75c3729d53f9..0ccb4787cdd6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class CabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 400 save_interval = 50 experiment_name = "franka_open_drawer" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[256, 128, 64], critic_hidden_dims=[256, 128, 64], activation="elu", 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 dedde5178a2f..ca95cb45ecec 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -23,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/cabinet/config/franka/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py index b8144a34a7e5..2f47f3239580 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py index 39f9c23d75d0..aaaa644ce1c2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py index a485ffdcf9bb..04624c0e6389 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/__init__.py new file mode 100644 index 000000000000..a4630e8b3bfd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/__init__.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Open-Drawer-OpenArm-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmCabinetEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:OpenArmCabinetPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Open-Drawer-OpenArm-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmCabinetEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:OpenArmCabinetPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 000000000000..52d5a7dfae87 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,81 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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: openarm_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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..67f3498c361d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class OpenArmCabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 96 + max_iterations = 600 + save_interval = 50 + experiment_name = "openarm_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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py new file mode 100644 index 000000000000..6e3eecb59382 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py @@ -0,0 +1,282 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +We modified parts of the environment, such as the target's position and orientation, +as well as certain object properties, to better suit the smaller robot. +""" + +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 + +## +# 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) + +from ... import mdp + +## +# 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, + scale=(0.75, 0.75, 0.75), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.7, 0, 0.3), + 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_bottom", + name="drawer_handle_bottom", + offset=OffsetCfg( + pos=(0.222, 0.0, 0.005), + 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_bottom_joint"])}, + ) + cabinet_joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_bottom_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_bottom"), + "static_friction_range": (2.25, 2.5), + "dynamic_friction_range": (2.0, 2.25), + "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_bottom_joint"])}, + ) + multi_stage_open_drawer = RewTerm( + func=mdp.multi_stage_open_drawer, + weight=1.0, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_bottom_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.01 + self.sim.physx.friction_correlation_distance = 0.00625 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/joint_pos_env_cfg.py new file mode 100644 index 000000000000..05d03942700e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/joint_pos_env_cfg.py @@ -0,0 +1,93 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +## +# Pre-defined configs +## +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_assets.robots.openarm import OPENARM_UNI_CFG + +from isaaclab_tasks.manager_based.manipulation.cabinet.config.openarm.cabinet_openarm_env_cfg import ( # isort: skip + FRAME_MARKER_SMALL_CFG, + CabinetEnvCfg, +) + + +@configclass +class OpenArmCabinetEnvCfg(CabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set OpenArm as robot + self.scene.robot = OPENARM_UNI_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set Actions for the specific robot type (OpenArm) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=["openarm_joint.*"], + scale=1.0, + use_default_offset=True, + ) + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["openarm_finger_joint.*"], + open_command_expr={"openarm_finger_joint.*": 0.044}, + close_command_expr={"openarm_finger_joint.*": 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/openarm_link0", + visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/EndEffectorFrameTransformer"), + debug_vis=False, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_ee_tcp", + name="ee_tcp", + offset=OffsetCfg( + pos=(0.0, 0.0, -0.003), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_left_finger", + name="tool_leftfinger", + offset=OffsetCfg( + pos=(0.0, -0.005, 0.075), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_right_finger", + name="tool_rightfinger", + offset=OffsetCfg( + pos=(0.0, 0.005, 0.075), + ), + ), + ], + ) + + # override rewards + self.rewards.approach_gripper_handle.params["offset"] = 0.04 + self.rewards.grasp_handle.params["open_joint_pos"] = 0.044 + self.rewards.grasp_handle.params["asset_cfg"].joint_names = ["openarm_finger_joint.*"] + + +@configclass +class OpenArmCabinetEnvCfg_PLAY(OpenArmCabinetEnvCfg): + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/__init__.py index 43ec606ed6b6..79a9af2f736b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py index 5599adb1203d..66fb8bb38e97 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils from isaaclab.assets import ArticulationData from isaaclab.sensors import FrameTransformerData diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/rewards.py index 1f156fe0ea1e..433a2a87732a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.managers import SceneEntityCfg from isaaclab.utils.math import matrix_from_quat diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/__init__.py new file mode 100644 index 000000000000..61fccf6b2c14 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Deployment environments for manipulation tasks. + +These environments are designed for real-world deployment of manipulation tasks. +They containconfigurations and implementations that have been tested +and deployed on physical robots. + +The deploy module includes: +- Reach environments for end-effector pose tracking + +""" + +from .reach import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/__init__.py new file mode 100644 index 000000000000..a7b2bf171a20 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Assemble 3 gears into a base.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/__init__.py new file mode 100644 index 000000000000..177e08ed7349 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for arm-based gear assembly 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/deploy/gear_assembly/config/ur_10e/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/__init__.py new file mode 100644 index 000000000000..ad5dff78db64 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/__init__.py @@ -0,0 +1,75 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + + +# UR10e with 2F-140 gripper +gym.register( + id="Isaac-Deploy-GearAssembly-UR10e-2F140-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10e2F140GearAssemblyEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UR10GearAssemblyRNNPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Deploy-GearAssembly-UR10e-2F140-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10e2F140GearAssemblyEnvCfg_PLAY", + }, +) + +# UR10e with 2F-85 gripper +gym.register( + id="Isaac-Deploy-GearAssembly-UR10e-2F85-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10e2F85GearAssemblyEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UR10GearAssemblyRNNPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Deploy-GearAssembly-UR10e-2F85-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10e2F85GearAssemblyEnvCfg_PLAY", + }, +) + +# UR10e with 2F-140 gripper - ROS Inference +gym.register( + id="Isaac-Deploy-GearAssembly-UR10e-2F140-ROS-Inference-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.ros_inference_env_cfg:UR10e2F140GearAssemblyROSInferenceEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UR10GearAssemblyRNNPPORunnerCfg", + }, +) + +# UR10e with 2F-85 gripper - ROS Inference +gym.register( + id="Isaac-Deploy-GearAssembly-UR10e-2F85-ROS-Inference-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.ros_inference_env_cfg:UR10e2F85GearAssemblyROSInferenceEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UR10GearAssemblyRNNPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/__init__.py new file mode 100644 index 000000000000..cf59b16a1e2e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..ac1ecba8463d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticRecurrentCfg, RslRlPpoAlgorithmCfg + + +@configclass +class UR10GearAssemblyRNNPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 512 + max_iterations = 1500 + save_interval = 50 + experiment_name = "gear_assembly_ur10e" + clip_actions = 1.0 + resume = False + obs_groups = { + "policy": ["policy"], + "critic": ["critic"], + } + policy = RslRlPpoActorCriticRecurrentCfg( + state_dependent_std=True, + init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + noise_std_type="log", + activation="elu", + rnn_type="lstm", + rnn_hidden_dim=256, + rnn_num_layers=2, + ) + 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=16, + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/joint_pos_env_cfg.py new file mode 100644 index 000000000000..22921e717895 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/joint_pos_env_cfg.py @@ -0,0 +1,520 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.deploy.mdp as mdp +import isaaclab_tasks.manager_based.manipulation.deploy.mdp.events as gear_assembly_events +from isaaclab_tasks.manager_based.manipulation.deploy.gear_assembly.gear_assembly_env_cfg import GearAssemblyEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.universal_robots import UR10e_ROBOTIQ_GRIPPER_CFG, UR10e_ROBOTIQ_2F_85_CFG # isort: skip + + +## +# Gripper-specific helper functions +## + + +def set_finger_joint_pos_robotiq_2f140( + joint_pos: torch.Tensor, + reset_ind_joint_pos: list[int], + finger_joints: list[int], + finger_joint_position: float, +): + """Set finger joint positions for Robotiq 2F-140 gripper. + + Args: + joint_pos: Joint positions tensor + reset_ind_joint_pos: Row indices into the sliced joint_pos tensor + finger_joints: List of finger joint indices + finger_joint_position: Target position for finger joints + """ + for idx in reset_ind_joint_pos: + # For 2F-140 gripper (8 joints expected) + # Joint structure: [finger_joint, finger_joint, outer_joints x2, inner_finger_joints x2, pad_joints x2] + if len(finger_joints) < 8: + raise ValueError(f"2F-140 gripper requires at least 8 finger joints, got {len(finger_joints)}") + + joint_pos[idx, finger_joints[0]] = finger_joint_position + joint_pos[idx, finger_joints[1]] = finger_joint_position + + # outer finger joints set to 0 + joint_pos[idx, finger_joints[2]] = 0 + joint_pos[idx, finger_joints[3]] = 0 + + # inner finger joints: multiply by -1 + joint_pos[idx, finger_joints[4]] = -finger_joint_position + joint_pos[idx, finger_joints[5]] = -finger_joint_position + + joint_pos[idx, finger_joints[6]] = finger_joint_position + joint_pos[idx, finger_joints[7]] = finger_joint_position + + +def set_finger_joint_pos_robotiq_2f85( + joint_pos: torch.Tensor, + reset_ind_joint_pos: list[int], + finger_joints: list[int], + finger_joint_position: float, +): + """Set finger joint positions for Robotiq 2F-85 gripper. + + Args: + joint_pos: Joint positions tensor + reset_ind_joint_pos: Row indices into the sliced joint_pos tensor + finger_joints: List of finger joint indices + finger_joint_position: Target position for finger joints + """ + for idx in reset_ind_joint_pos: + # For 2F-85 gripper (6 joints expected) + # Joint structure: [finger_joint, finger_joint, inner_finger_joints x2, inner_finger_knuckle_joints x2] + if len(finger_joints) < 6: + raise ValueError(f"2F-85 gripper requires at least 6 finger joints, got {len(finger_joints)}") + + # Multiply specific indices by -1: [2, 4, 5] + # These correspond to: + # ['left_inner_finger_joint', 'right_inner_finger_knuckle_joint', 'left_inner_finger_knuckle_joint'] + joint_pos[idx, finger_joints[0]] = finger_joint_position + joint_pos[idx, finger_joints[1]] = finger_joint_position + joint_pos[idx, finger_joints[2]] = -finger_joint_position + joint_pos[idx, finger_joints[3]] = finger_joint_position + joint_pos[idx, finger_joints[4]] = -finger_joint_position + joint_pos[idx, finger_joints[5]] = -finger_joint_position + + +## +# Environment configuration +## + + +@configclass +class EventCfg: + """Configuration for events.""" + + robot_joint_stiffness_and_damping = EventTerm( + func=mdp.randomize_actuator_gains, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg( + "robot", joint_names=["shoulder_.*", "elbow_.*", "wrist_.*"] + ), # only the arm joints are randomized + "stiffness_distribution_params": (0.75, 1.5), + "damping_distribution_params": (0.3, 3.0), + "operation": "scale", + "distribution": "log_uniform", + }, + ) + + joint_friction = EventTerm( + func=mdp.randomize_joint_parameters, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=["shoulder_.*", "elbow_.*", "wrist_.*"]), + "friction_distribution_params": (0.3, 0.7), + "operation": "add", + "distribution": "uniform", + }, + ) + + small_gear_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("factory_gear_small", body_names=".*"), + "static_friction_range": (0.75, 0.75), + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + medium_gear_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("factory_gear_medium", body_names=".*"), + "static_friction_range": (0.75, 0.75), + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + large_gear_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("factory_gear_large", body_names=".*"), + "static_friction_range": (0.75, 0.75), + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + gear_base_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("factory_gear_base", body_names=".*"), + "static_friction_range": (0.75, 0.75), + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*finger"), + "static_friction_range": (0.75, 0.75), + "dynamic_friction_range": (0.75, 0.75), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + randomize_gear_type = EventTerm( + func=gear_assembly_events.randomize_gear_type, + mode="reset", + params={"gear_types": ["gear_small", "gear_medium", "gear_large"]}, + ) + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + randomize_gears_and_base_pose = EventTerm( + func=gear_assembly_events.randomize_gears_and_base_pose, + mode="reset", + params={ + "pose_range": { + "x": [-0.1, 0.1], + "y": [-0.25, 0.25], + "z": [-0.1, 0.1], + "roll": [-math.pi / 90, math.pi / 90], # 2 degree + "pitch": [-math.pi / 90, math.pi / 90], # 2 degree + "yaw": [-math.pi / 6, math.pi / 6], # 2 degree + }, + "gear_pos_range": { + "x": [-0.02, 0.02], + "y": [-0.02, 0.02], + "z": [0.0575, 0.0775], # 0.045 + 0.0225 + }, + "velocity_range": {}, + }, + ) + + set_robot_to_grasp_pose = EventTerm( + func=gear_assembly_events.set_robot_to_grasp_pose, + mode="reset", + params={ + "robot_asset_cfg": SceneEntityCfg("robot"), + "pos_randomization_range": {"x": [-0.0, 0.0], "y": [-0.005, 0.005], "z": [-0.003, 0.003]}, + }, + ) + + +@configclass +class UR10eGearAssemblyEnvCfg(GearAssemblyEnvCfg): + """Base configuration for UR10e Gear Assembly Environment. + + This class contains common setup shared across different gripper configurations. + Subclasses should configure gripper-specific parameters. + """ + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Robot-specific parameters (can be overridden for other robots) + self.end_effector_body_name = "wrist_3_link" # End effector body name for IK and termination checks + self.num_arm_joints = 6 # Number of arm joints (excluding gripper) + self.grasp_rot_offset = [ + 0.0, + math.sqrt(2) / 2, + math.sqrt(2) / 2, + 0.0, + ] # Rotation offset for grasp pose (quaternion [w, x, y, z]) + self.gripper_joint_setter_func = None # Gripper-specific joint setter function (set in subclass) + + # Gear orientation termination thresholds (in degrees) + self.gear_orientation_roll_threshold_deg = 15.0 # Maximum allowed roll deviation + self.gear_orientation_pitch_threshold_deg = 15.0 # Maximum allowed pitch deviation + self.gear_orientation_yaw_threshold_deg = 180.0 # Maximum allowed yaw deviation + + # Common observation configuration + self.observations.policy.joint_pos.params["asset_cfg"].joint_names = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ] + self.observations.policy.joint_vel.params["asset_cfg"].joint_names = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ] + + # override events + self.events = EventCfg() + + # Update termination thresholds from config + self.terminations.gear_orientation_exceeded.params["roll_threshold_deg"] = ( + self.gear_orientation_roll_threshold_deg + ) + self.terminations.gear_orientation_exceeded.params["pitch_threshold_deg"] = ( + self.gear_orientation_pitch_threshold_deg + ) + self.terminations.gear_orientation_exceeded.params["yaw_threshold_deg"] = ( + self.gear_orientation_yaw_threshold_deg + ) + + # override command generator body + self.joint_action_scale = 0.025 + self.actions.arm_action = mdp.RelativeJointPositionActionCfg( + asset_name="robot", + joint_names=[ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + scale=self.joint_action_scale, + use_zero_offset=True, + ) + + +@configclass +class UR10e2F140GearAssemblyEnvCfg(UR10eGearAssemblyEnvCfg): + """Configuration for UR10e with Robotiq 2F-140 gripper.""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch robot to ur10e with 2F-140 gripper + self.scene.robot = UR10e_ROBOTIQ_GRIPPER_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=UR10e_ROBOTIQ_GRIPPER_CFG.spawn.replace( + 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=4, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=1 + ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + # Joint positions based on IK from center of distribution for randomized gear positions + # This is done so that the start for the differential IK search after randomizing + # is close to the optimal grasp pose + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "shoulder_pan_joint": 2.7228, + "shoulder_lift_joint": -8.3962e-01, + "elbow_joint": 1.3684, + "wrist_1_joint": -2.1048, + "wrist_2_joint": -1.5691, + "wrist_3_joint": -1.9896, + }, + pos=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + + # 2F-140 gripper actuator configuration + self.scene.robot.actuators["gripper_finger"] = ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_joint"], + effort_limit_sim=10.0, + velocity_limit_sim=10.0, + stiffness=10.0, + damping=0.05, + friction=0.0, + armature=0.0, + ) + + # Set gripper-specific joint setter function + self.gripper_joint_setter_func = set_finger_joint_pos_robotiq_2f140 + + # gear offsets and grasp positions for the 2F-140 gripper + self.gear_offsets_grasp = { + "gear_small": [0.0, self.gear_offsets["gear_small"][0], -0.26], + "gear_medium": [0.0, self.gear_offsets["gear_medium"][0], -0.26], + "gear_large": [0.0, self.gear_offsets["gear_large"][0], -0.26], + } + + # Grasp widths for 2F-140 gripper + self.hand_grasp_width = {"gear_small": 0.64, "gear_medium": 0.54, "gear_large": 0.51} + + # Close widths for 2F-140 gripper + self.hand_close_width = {"gear_small": 0.69, "gear_medium": 0.59, "gear_large": 0.56} + + # Populate event term parameters + self.events.set_robot_to_grasp_pose.params["gear_offsets_grasp"] = self.gear_offsets_grasp + self.events.set_robot_to_grasp_pose.params["end_effector_body_name"] = self.end_effector_body_name + self.events.set_robot_to_grasp_pose.params["num_arm_joints"] = self.num_arm_joints + self.events.set_robot_to_grasp_pose.params["grasp_rot_offset"] = self.grasp_rot_offset + self.events.set_robot_to_grasp_pose.params["gripper_joint_setter_func"] = self.gripper_joint_setter_func + + # Populate termination term parameters + self.terminations.gear_dropped.params["gear_offsets_grasp"] = self.gear_offsets_grasp + self.terminations.gear_dropped.params["end_effector_body_name"] = self.end_effector_body_name + self.terminations.gear_dropped.params["grasp_rot_offset"] = self.grasp_rot_offset + + self.terminations.gear_orientation_exceeded.params["end_effector_body_name"] = self.end_effector_body_name + self.terminations.gear_orientation_exceeded.params["grasp_rot_offset"] = self.grasp_rot_offset + + +@configclass +class UR10e2F85GearAssemblyEnvCfg(UR10eGearAssemblyEnvCfg): + """Configuration for UR10e with Robotiq 2F-85 gripper.""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch robot to ur10e with 2F-85 gripper + self.scene.robot = UR10e_ROBOTIQ_2F_85_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=UR10e_ROBOTIQ_2F_85_CFG.spawn.replace( + 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=4, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=1 + ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + # Joint positions based on IK from center of distribution for randomized gear positions + # This is done so that the start for the differential IK search after randomizing + # is close to the optimal grasp pose + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "shoulder_pan_joint": 2.7228, + "shoulder_lift_joint": -8.3962e-01, + "elbow_joint": 1.3684, + "wrist_1_joint": -2.1048, + "wrist_2_joint": -1.5691, + "wrist_3_joint": -1.9896, + }, + pos=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + + # 2F-85 gripper actuator configuration (higher effort limits than 2F-140) + self.scene.robot.actuators["gripper_finger"] = ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_joint"], + effort_limit_sim=10.0, + velocity_limit_sim=10.0, + stiffness=10.0, + damping=0.05, + friction=0.0, + armature=0.0, + ) + self.scene.robot.actuators["gripper_drive"] = ImplicitActuatorCfg( + joint_names_expr=["finger_joint"], + effort_limit_sim=10.0, + velocity_limit_sim=1.0, + stiffness=40.0, + damping=1.0, + friction=0.0, + armature=0.0, + ) + + # Set gripper-specific joint setter function + self.gripper_joint_setter_func = set_finger_joint_pos_robotiq_2f85 + + # gear offsets and grasp positions for the 2F-85 gripper + self.gear_offsets_grasp = { + "gear_small": [0.0, self.gear_offsets["gear_small"][0], -0.19], + "gear_medium": [0.0, self.gear_offsets["gear_medium"][0], -0.19], + "gear_large": [0.0, self.gear_offsets["gear_large"][0], -0.19], + } + + # Grasp widths for 2F-85 gripper + self.hand_grasp_width = {"gear_small": 0.64, "gear_medium": 0.46, "gear_large": 0.4} + + # Close widths for 2F-85 gripper + self.hand_close_width = {"gear_small": 0.69, "gear_medium": 0.51, "gear_large": 0.45} + + # Populate event term parameters + self.events.set_robot_to_grasp_pose.params["gear_offsets_grasp"] = self.gear_offsets_grasp + self.events.set_robot_to_grasp_pose.params["end_effector_body_name"] = self.end_effector_body_name + self.events.set_robot_to_grasp_pose.params["num_arm_joints"] = self.num_arm_joints + self.events.set_robot_to_grasp_pose.params["grasp_rot_offset"] = self.grasp_rot_offset + self.events.set_robot_to_grasp_pose.params["gripper_joint_setter_func"] = self.gripper_joint_setter_func + + # Populate termination term parameters + self.terminations.gear_dropped.params["gear_offsets_grasp"] = self.gear_offsets_grasp + self.terminations.gear_dropped.params["end_effector_body_name"] = self.end_effector_body_name + self.terminations.gear_dropped.params["grasp_rot_offset"] = self.grasp_rot_offset + + self.terminations.gear_orientation_exceeded.params["end_effector_body_name"] = self.end_effector_body_name + self.terminations.gear_orientation_exceeded.params["grasp_rot_offset"] = self.grasp_rot_offset + + +@configclass +class UR10e2F140GearAssemblyEnvCfg_PLAY(UR10e2F140GearAssemblyEnvCfg): + """Play configuration for UR10e with Robotiq 2F-140 gripper.""" + + 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 + + +@configclass +class UR10e2F85GearAssemblyEnvCfg_PLAY(UR10e2F85GearAssemblyEnvCfg): + """Play configuration for UR10e with Robotiq 2F-85 gripper.""" + + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/ros_inference_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/ros_inference_env_cfg.py new file mode 100644 index 000000000000..450a454f78f0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/ros_inference_env_cfg.py @@ -0,0 +1,197 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +from isaaclab.assets import RigidObjectCfg +from isaaclab.utils import configclass + +from .joint_pos_env_cfg import UR10e2F85GearAssemblyEnvCfg, UR10e2F140GearAssemblyEnvCfg + + +@configclass +class UR10e2F140GearAssemblyROSInferenceEnvCfg(UR10e2F140GearAssemblyEnvCfg): + """Configuration for ROS inference with UR10e and Robotiq 2F-140 gripper. + + This configuration: + - Exposes variables needed for ROS inference + - Overrides robot and gear initial poses for fixed/deterministic setup + """ + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Variables used by Isaac Manipulator for on robot inference + # These parameters allow the ROS inference node to validate environment configuration, + # perform checks during inference, and correctly interpret observations and actions. + self.obs_order = ["arm_dof_pos", "arm_dof_vel", "shaft_pos", "shaft_quat"] + self.policy_action_space = "joint" + # Use inherited joint names from parent's observation configuration + self.arm_joint_names = self.observations.policy.joint_pos.params["asset_cfg"].joint_names + # Use inherited num_arm_joints from parent + self.action_space = self.num_arm_joints + # State space and observation space are set as constants for now + self.state_space = 42 + self.observation_space = 19 + + # Set joint_action_scale from the existing arm_action.scale + self.joint_action_scale = self.actions.arm_action.scale + + # Dynamically generate action_scale_joint_space based on action_space + self.action_scale_joint_space = [self.joint_action_scale] * self.action_space + + # Override robot initial pose for ROS inference (fixed pose, no randomization) + # Note: The policy is trained to work with respect to the UR robot's 'base' frame + # (rotated 180ยฐ around Z from base_link), not the base_link frame (USD origin). + # See: https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_description/doc/robot_frames.html + # Joint positions and pos are inherited from parent, only override rotation to be deterministic + self.scene.robot.init_state.rot = (0.0, 0.0, 0.0, 1.0) + + # Override gear base initial pose (fixed pose for ROS inference) + self.scene.factory_gear_base.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Override gear initial poses (fixed poses for ROS inference) + # Small gear + self.scene.factory_gear_small.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), # z = base_z + 0.1675 (above base) + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Medium gear + self.scene.factory_gear_medium.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Large gear + self.scene.factory_gear_large.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Fixed asset parameters for ROS inference - derived from configuration + # These parameters are used by the ROS inference node to validate the environment setup + # and apply appropriate noise models for robust real-world deployment. + # Derive position center from gear base init state + self.fixed_asset_init_pos_center = list(self.scene.factory_gear_base.init_state.pos) + # Derive position range from parent's randomize_gears_and_base_pose event pose_range + pose_range = self.events.randomize_gears_and_base_pose.params["pose_range"] + self.fixed_asset_init_pos_range = [ + pose_range["x"][1], # max value + pose_range["y"][1], # max value + pose_range["z"][1], # max value + ] + # Orientation in degrees (quaternion (-0.70711, 0.0, 0.0, 0.70711) = -90ยฐ around Z) + self.fixed_asset_init_orn_deg = [0.0, 0.0, -90.0] + # Derive orientation range from parent's pose_range (radians to degrees) + self.fixed_asset_init_orn_deg_range = [ + math.degrees(pose_range["roll"][1]), # convert radians to degrees + math.degrees(pose_range["pitch"][1]), + math.degrees(pose_range["yaw"][1]), + ] + # Derive observation noise level from parent's gear_shaft_pos noise configuration + gear_shaft_pos_noise = self.observations.policy.gear_shaft_pos.noise.noise_cfg.n_max + self.fixed_asset_pos_obs_noise_level = [ + gear_shaft_pos_noise, + gear_shaft_pos_noise, + gear_shaft_pos_noise, + ] + + +@configclass +class UR10e2F85GearAssemblyROSInferenceEnvCfg(UR10e2F85GearAssemblyEnvCfg): + """Configuration for ROS inference with UR10e and Robotiq 2F-85 gripper. + + This configuration: + - Exposes variables needed for ROS inference + - Overrides robot and gear initial poses for fixed/deterministic setup + """ + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Variables used by Isaac Manipulator for on robot inference + # These parameters allow the ROS inference node to validate environment configuration, + # perform checks during inference, and correctly interpret observations and actions. + self.obs_order = ["arm_dof_pos", "arm_dof_vel", "shaft_pos", "shaft_quat"] + self.policy_action_space = "joint" + # Use inherited joint names from parent's observation configuration + self.arm_joint_names = self.observations.policy.joint_pos.params["asset_cfg"].joint_names + # Use inherited num_arm_joints from parent + self.action_space = self.num_arm_joints + # State space and observation space are set as constants for now + self.state_space = 38 + self.observation_space = 19 + + # Set joint_action_scale from the existing arm_action.scale + self.joint_action_scale = self.actions.arm_action.scale + + # Dynamically generate action_scale_joint_space based on action_space + self.action_scale_joint_space = [self.joint_action_scale] * self.action_space + + # Override robot initial pose for ROS inference (fixed pose, no randomization) + # Note: The policy is trained to work with respect to the UR robot's 'base' frame + # (rotated 180ยฐ around Z from base_link), not the base_link frame (USD origin). + # See: https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_description/doc/robot_frames.html + # Joint positions and pos are inherited from parent, only override rotation to be deterministic + self.scene.robot.init_state.rot = (0.0, 0.0, 0.0, 1.0) + + # Override gear base initial pose (fixed pose for ROS inference) + self.scene.factory_gear_base.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Override gear initial poses (fixed poses for ROS inference) + # Small gear + self.scene.factory_gear_small.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), # z = base_z + 0.1675 (above base) + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Medium gear + self.scene.factory_gear_medium.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Large gear + self.scene.factory_gear_large.init_state = RigidObjectCfg.InitialStateCfg( + pos=(1.0200, -0.2100, -0.1), + rot=(-0.70711, 0.0, 0.0, 0.70711), + ) + + # Fixed asset parameters for ROS inference - derived from configuration + # These parameters are used by the ROS inference node to validate the environment setup + # and apply appropriate noise models for robust real-world deployment. + # Derive position center from gear base init state + self.fixed_asset_init_pos_center = list(self.scene.factory_gear_base.init_state.pos) + # Derive position range from parent's randomize_gears_and_base_pose event pose_range + pose_range = self.events.randomize_gears_and_base_pose.params["pose_range"] + self.fixed_asset_init_pos_range = [ + pose_range["x"][1], # max value + pose_range["y"][1], # max value + pose_range["z"][1], # max value + ] + # Orientation in degrees (quaternion (-0.70711, 0.0, 0.0, 0.70711) = -90ยฐ around Z) + self.fixed_asset_init_orn_deg = [0.0, 0.0, -90.0] + # Derive orientation range from parent's pose_range (radians to degrees) + self.fixed_asset_init_orn_deg_range = [ + math.degrees(pose_range["roll"][1]), # convert radians to degrees + math.degrees(pose_range["pitch"][1]), + math.degrees(pose_range["yaw"][1]), + ] + # Derive observation noise level from parent's gear_shaft_pos noise configuration + gear_shaft_pos_noise = self.observations.policy.gear_shaft_pos.noise.noise_cfg.n_max + self.fixed_asset_pos_obs_noise_level = [ + gear_shaft_pos_noise, + gear_shaft_pos_noise, + gear_shaft_pos_noise, + ] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/gear_assembly_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/gear_assembly_env_cfg.py new file mode 100644 index 000000000000..8a15d7b3a52d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/gear_assembly_env_cfg.py @@ -0,0 +1,330 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +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 ActionTermCfg as ActionTerm +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.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import UniformNoiseCfg + +import isaaclab_tasks.manager_based.manipulation.deploy.mdp as mdp +import isaaclab_tasks.manager_based.manipulation.deploy.mdp.terminations as gear_assembly_terminations +from isaaclab_tasks.manager_based.manipulation.deploy.mdp.noise_models import ResetSampledConstantNoiseModelCfg + +# Get the directory where this configuration file is located +CONFIG_DIR = os.path.dirname(os.path.abspath(__file__)) +ASSETS_DIR = os.path.join(CONFIG_DIR, "assets") + +## +# Environment configuration +## + + +@configclass +class GearAssemblySceneCfg(InteractiveSceneCfg): + """Configuration for the scene with a robotic arm.""" + + # Disable scene replication to allow USD-level randomization + replicate_physics = False + + # world + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + factory_gear_base = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/FactoryGearBase", + # TODO: change to common isaac sim directory + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Factory/gear_assets/factory_gear_base/factory_gear_base.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + kinematic_enabled=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=32, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=None), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.02, rest_offset=0.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-1.0200, 0.2100, -0.1), rot=(0.70711, 0.0, 0.0, 0.70711)), + ) + + factory_gear_small = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/FactoryGearSmall", + # TODO: change to common isaac sim directory + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Factory/gear_assets/factory_gear_small/factory_gear_small.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + kinematic_enabled=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=32, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=None), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.02, rest_offset=0.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-1.0200, 0.2100, -0.1), rot=(0.70711, 0.0, 0.0, 0.70711)), + ) + + factory_gear_medium = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/FactoryGearMedium", + # TODO: change to common isaac sim directory + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Factory/gear_assets/factory_gear_medium/factory_gear_medium.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + kinematic_enabled=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=32, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=None), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.02, rest_offset=0.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-1.0200, 0.2100, -0.1), rot=(0.70711, 0.0, 0.0, 0.70711)), + ) + + factory_gear_large = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/FactoryGearLarge", + # TODO: change to common isaac sim directory + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Factory/gear_assets/factory_gear_large/factory_gear_large.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + kinematic_enabled=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=32, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=None), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.02, rest_offset=0.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-1.0200, 0.2100, -0.1), 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), + ) + + stand = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Stand", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0) + ), + ) + + +@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, params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}) + joint_vel = ObsTerm(func=mdp.joint_vel, params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}) + gear_shaft_pos = ObsTerm( + func=mdp.gear_shaft_pos_w, + params={}, # Will be populated in __post_init__ + noise=ResetSampledConstantNoiseModelCfg( + noise_cfg=UniformNoiseCfg(n_min=-0.005, n_max=0.005, operation="add") + ), + ) + gear_shaft_quat = ObsTerm(func=mdp.gear_shaft_quat_w) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + @configclass + class CriticCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + joint_pos = ObsTerm(func=mdp.joint_pos, params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}) + joint_vel = ObsTerm(func=mdp.joint_vel, params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}) + gear_shaft_pos = ObsTerm(func=mdp.gear_shaft_pos_w, params={}) # Will be populated in __post_init__ + gear_shaft_quat = ObsTerm(func=mdp.gear_shaft_quat_w) + + gear_pos = ObsTerm(func=mdp.gear_pos_w) + gear_quat = ObsTerm(func=mdp.gear_quat_w) + + # observation groups + policy: PolicyCfg = PolicyCfg() + critic: CriticCfg = CriticCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_gear = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": [-0.05, 0.05], + "y": [-0.05, 0.05], + "z": [0.1, 0.15], + }, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("factory_gear_small"), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + end_effector_gear_keypoint_tracking = RewTerm( + func=mdp.keypoint_entity_error, + weight=-1.5, + params={ + "asset_cfg_1": SceneEntityCfg("factory_gear_base"), + "keypoint_scale": 0.15, + }, + ) + + end_effector_gear_keypoint_tracking_exp = RewTerm( + func=mdp.keypoint_entity_error_exp, + weight=1.5, + params={ + "asset_cfg_1": SceneEntityCfg("factory_gear_base"), + "kp_exp_coeffs": [(50, 0.0001), (300, 0.0001)], + "kp_use_sum_of_exps": False, + "keypoint_scale": 0.15, + }, + ) + + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-5.0e-06) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + gear_dropped = DoneTerm( + func=gear_assembly_terminations.reset_when_gear_dropped, + params={ + "distance_threshold": 0.15, # 15cm from gripper + "robot_asset_cfg": SceneEntityCfg("robot"), + }, + ) + + gear_orientation_exceeded = DoneTerm( + func=gear_assembly_terminations.reset_when_gear_orientation_exceeds_threshold, + params={ + "roll_threshold_deg": 7.0, # Maximum roll deviation in degrees + "pitch_threshold_deg": 7.0, # Maximum pitch deviation in degrees + "yaw_threshold_deg": 180.0, # Maximum yaw deviation in degrees + "robot_asset_cfg": SceneEntityCfg("robot"), + }, + ) + + +@configclass +class GearAssemblyEnvCfg(ManagerBasedRLEnvCfg): + # Scene settings + scene: GearAssemblySceneCfg = GearAssemblySceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + sim: SimulationCfg = SimulationCfg( + physx=PhysxCfg( + # Important to prevent collisionStackSize buffer overflow in contact-rich environments. + gpu_collision_stack_size=2**28, + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + ), + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.episode_length_s = 6.66 + self.viewer.eye = (3.5, 3.5, 3.5) + # simulation settings + self.decimation = 4 + self.sim.render_interval = self.decimation + self.sim.dt = 1.0 / 120.0 + + self.gear_offsets = { + "gear_small": [0.076125, 0.0, 0.0], + "gear_medium": [0.030375, 0.0, 0.0], + "gear_large": [-0.045375, 0.0, 0.0], + } + + # Populate observation term parameters with gear offsets + self.observations.policy.gear_shaft_pos.params["gear_offsets"] = self.gear_offsets + self.observations.critic.gear_shaft_pos.params["gear_offsets"] = self.gear_offsets diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/__init__.py new file mode 100644 index 000000000000..10ab3ea7e7fd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the locomotion environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .events import * # noqa: F401, F403 +from .noise_models 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/deploy/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py new file mode 100644 index 000000000000..7666875435fb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py @@ -0,0 +1,481 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Class-based event terms specific to the gear assembly manipulation environments.""" + +from __future__ import annotations + +import random +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg + +from isaaclab_tasks.direct.automate import factory_control as fc + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +class randomize_gear_type(ManagerTermBase): + """Randomize and manage the gear type being used for each environment. + + This class stores the current gear type for each environment and provides a mapping + from gear type names to indices. It serves as the central manager for gear type state + that other MDP terms depend on. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the gear type randomization term. + + Args: + cfg: Event term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Extract gear types from config (required parameter) + if "gear_types" not in cfg.params: + raise ValueError("'gear_types' parameter is required in randomize_gear_type configuration") + self.gear_types: list[str] = cfg.params["gear_types"] + + # Create gear type mapping (shared across all terms) + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + + # Store current gear type for each environment (as list for easy access) + # Initialize all to first gear type in the list + self._current_gear_type = [self.gear_types[0]] * env.num_envs + + # Store current gear type indices as tensor for efficient vectorized access + # Initialize all to first gear type index + first_gear_idx = self.gear_type_map[self.gear_types[0]] + self._current_gear_type_indices = torch.full( + (env.num_envs,), first_gear_idx, device=env.device, dtype=torch.long + ) + + # Store reference on environment for other terms to access + env._gear_type_manager = self + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + gear_types: list[str] = ["gear_small", "gear_medium", "gear_large"], + ): + """Randomize the gear type for specified environments. + + Args: + env: The environment containing the assets + env_ids: Environment IDs to randomize + gear_types: List of available gear types to choose from + """ + # Randomly select gear type for each environment + # Use the parameter passed to __call__ (not self.gear_types) to allow runtime overrides + for env_id in env_ids.tolist(): + chosen_gear = random.choice(gear_types) + self._current_gear_type[env_id] = chosen_gear + self._current_gear_type_indices[env_id] = self.gear_type_map[chosen_gear] + + def get_gear_type(self, env_id: int) -> str: + """Get the current gear type for a specific environment.""" + return self._current_gear_type[env_id] + + def get_all_gear_types(self) -> list[str]: + """Get current gear types for all environments.""" + return self._current_gear_type + + def get_all_gear_type_indices(self) -> torch.Tensor: + """Get current gear type indices for all environments as a tensor. + + Returns: + Tensor of shape (num_envs,) with gear type indices (0=small, 1=medium, 2=large) + """ + return self._current_gear_type_indices + + +class set_robot_to_grasp_pose(ManagerTermBase): + """Set robot to grasp pose using IK with pre-cached tensors. + + This class-based term caches all required tensors and gear offsets during initialization, + avoiding repeated allocations and lookups during execution. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the set robot to grasp pose term. + + Args: + cfg: Event term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Get robot asset configuration + self.robot_asset_cfg: SceneEntityCfg = cfg.params.get("robot_asset_cfg", SceneEntityCfg("robot")) + self.robot_asset: Articulation = env.scene[self.robot_asset_cfg.name] + + # Get robot-specific parameters from environment config (all required) + # Validate required parameters + if "end_effector_body_name" not in cfg.params: + raise ValueError( + "'end_effector_body_name' parameter is required in set_robot_to_grasp_pose configuration. " + "Example: 'wrist_3_link'" + ) + if "num_arm_joints" not in cfg.params: + raise ValueError( + "'num_arm_joints' parameter is required in set_robot_to_grasp_pose configuration. Example: 6 for UR10e" + ) + if "grasp_rot_offset" not in cfg.params: + raise ValueError( + "'grasp_rot_offset' parameter is required in set_robot_to_grasp_pose configuration. " + "It should be a quaternion [w, x, y, z]. Example: [0.0, 0.707, 0.707, 0.0]" + ) + if "gripper_joint_setter_func" not in cfg.params: + raise ValueError( + "'gripper_joint_setter_func' parameter is required in set_robot_to_grasp_pose configuration. " + "It should be a function to set gripper joint positions." + ) + + self.end_effector_body_name = cfg.params["end_effector_body_name"] + self.num_arm_joints = cfg.params["num_arm_joints"] + self.gripper_joint_setter_func = cfg.params["gripper_joint_setter_func"] + + # Pre-cache gear grasp offsets as tensors (required parameter) + if "gear_offsets_grasp" not in cfg.params: + raise ValueError( + "'gear_offsets_grasp' parameter is required in set_robot_to_grasp_pose configuration. " + "It should be a dict with keys 'gear_small', 'gear_medium', 'gear_large' mapping to [x, y, z] offsets." + ) + gear_offsets_grasp = cfg.params["gear_offsets_grasp"] + if not isinstance(gear_offsets_grasp, dict): + raise TypeError( + f"'gear_offsets_grasp' parameter must be a dict, got {type(gear_offsets_grasp).__name__}. " + "It should have keys 'gear_small', 'gear_medium', 'gear_large' mapping to [x, y, z] offsets." + ) + + self.gear_grasp_offset_tensors = {} + for gear_type in ["gear_small", "gear_medium", "gear_large"]: + if gear_type not in gear_offsets_grasp: + raise ValueError( + f"'{gear_type}' offset is required in 'gear_offsets_grasp' parameter. " + f"Found keys: {list(gear_offsets_grasp.keys())}" + ) + self.gear_grasp_offset_tensors[gear_type] = torch.tensor( + gear_offsets_grasp[gear_type], device=env.device, dtype=torch.float32 + ) + + # Stack grasp offset tensors for vectorized indexing (shape: 3, 3) + # Index 0=small, 1=medium, 2=large + self.gear_grasp_offsets_stacked = torch.stack( + [ + self.gear_grasp_offset_tensors["gear_small"], + self.gear_grasp_offset_tensors["gear_medium"], + self.gear_grasp_offset_tensors["gear_large"], + ], + dim=0, + ) + + # Pre-cache grasp rotation offset tensor + grasp_rot_offset = cfg.params["grasp_rot_offset"] + self.grasp_rot_offset_tensor = ( + torch.tensor(grasp_rot_offset, device=env.device, dtype=torch.float32).unsqueeze(0).repeat(env.num_envs, 1) + ) + + # Pre-allocate buffers for batch operations + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + self.local_env_indices = torch.arange(env.num_envs, device=env.device) + self.gear_grasp_offsets_buffer = torch.zeros(env.num_envs, 3, device=env.device, dtype=torch.float32) + + # Cache hand grasp/close widths + self.hand_grasp_width = env.cfg.hand_grasp_width + self.hand_close_width = env.cfg.hand_close_width + + # Find end effector index once + eef_indices, _ = self.robot_asset.find_bodies([self.end_effector_body_name]) + if len(eef_indices) == 0: + raise ValueError(f"End effector body '{self.end_effector_body_name}' not found in robot") + self.eef_idx = eef_indices[0] + + # Find jacobian body index (for fixed-base robots, subtract 1) + self.jacobi_body_idx = self.eef_idx - 1 + + # Find all joints once + all_joints, all_joints_names = self.robot_asset.find_joints([".*"]) + self.all_joints = all_joints + self.finger_joints = all_joints[self.num_arm_joints :] + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + robot_asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + pos_threshold: float = 1e-6, + rot_threshold: float = 1e-6, + max_iterations: int = 10, + pos_randomization_range: dict | None = None, + gear_offsets_grasp: dict | None = None, + end_effector_body_name: str | None = None, + num_arm_joints: int | None = None, + grasp_rot_offset: list | None = None, + gripper_joint_setter_func: callable | None = None, + ): + """Set robot to grasp pose using IK. + + Args: + env: Environment instance + env_ids: Environment IDs to reset + robot_asset_cfg: Robot asset configuration (unused, kept for compatibility) + pos_threshold: Position convergence threshold + rot_threshold: Rotation convergence threshold + max_iterations: Maximum IK iterations + pos_randomization_range: Optional position randomization range + """ + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this event term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + + # Slice buffers for current batch size + num_reset_envs = len(env_ids) + gear_type_indices = self.gear_type_indices[:num_reset_envs] + local_env_indices = self.local_env_indices[:num_reset_envs] + gear_grasp_offsets = self.gear_grasp_offsets_buffer[:num_reset_envs] + grasp_rot_offset_tensor = self.grasp_rot_offset_tensor[env_ids] + + # IK loop + for i in range(max_iterations): + # Get current joint state + joint_pos = self.robot_asset.data.joint_pos[env_ids].clone() + joint_vel = self.robot_asset.data.joint_vel[env_ids].clone() + + # Stack all gear positions and quaternions + all_gear_pos = torch.stack( + [ + env.scene["factory_gear_small"].data.root_link_pos_w, + env.scene["factory_gear_medium"].data.root_link_pos_w, + env.scene["factory_gear_large"].data.root_link_pos_w, + ], + dim=1, + )[env_ids] + + all_gear_quat = torch.stack( + [ + env.scene["factory_gear_small"].data.root_link_quat_w, + env.scene["factory_gear_medium"].data.root_link_quat_w, + env.scene["factory_gear_large"].data.root_link_quat_w, + ], + dim=1, + )[env_ids] + + # Get gear type indices directly as tensor + all_gear_type_indices = gear_type_manager.get_all_gear_type_indices() + gear_type_indices[:] = all_gear_type_indices[env_ids] + + # Select gear data using advanced indexing + grasp_object_pos_world = all_gear_pos[local_env_indices, gear_type_indices] + grasp_object_quat = all_gear_quat[local_env_indices, gear_type_indices] + + # Apply rotation offset + grasp_object_quat = math_utils.quat_mul(grasp_object_quat, grasp_rot_offset_tensor) + + # Get grasp offsets (vectorized) + gear_grasp_offsets[:] = self.gear_grasp_offsets_stacked[gear_type_indices] + + # Add position randomization if specified + if pos_randomization_range is not None: + pos_keys = ["x", "y", "z"] + range_list_pos = [pos_randomization_range.get(key, (0.0, 0.0)) for key in pos_keys] + ranges_pos = torch.tensor(range_list_pos, device=env.device) + rand_pos_offsets = math_utils.sample_uniform( + ranges_pos[:, 0], ranges_pos[:, 1], (len(env_ids), 3), device=env.device + ) + gear_grasp_offsets = gear_grasp_offsets + rand_pos_offsets + + # Transform offsets from gear frame to world frame + grasp_object_pos_world = grasp_object_pos_world + math_utils.quat_apply( + grasp_object_quat, gear_grasp_offsets + ) + + # Get end effector pose + eef_pos = self.robot_asset.data.body_pos_w[env_ids, self.eef_idx] + eef_quat = self.robot_asset.data.body_quat_w[env_ids, self.eef_idx] + + # Compute pose error + pos_error, axis_angle_error = fc.get_pose_error( + fingertip_midpoint_pos=eef_pos, + fingertip_midpoint_quat=eef_quat, + ctrl_target_fingertip_midpoint_pos=grasp_object_pos_world, + ctrl_target_fingertip_midpoint_quat=grasp_object_quat, + jacobian_type="geometric", + rot_error_type="axis_angle", + ) + delta_hand_pose = torch.cat((pos_error, axis_angle_error), dim=-1) + + # Check convergence + pos_error_norm = torch.norm(pos_error, dim=-1) + rot_error_norm = torch.norm(axis_angle_error, dim=-1) + + if torch.all(pos_error_norm < pos_threshold) and torch.all(rot_error_norm < rot_threshold): + break + + # Solve IK using jacobian + jacobians = self.robot_asset.root_physx_view.get_jacobians().clone() + jacobian = jacobians[env_ids, self.jacobi_body_idx, :, :] + + delta_dof_pos = fc._get_delta_dof_pos( + delta_pose=delta_hand_pose, + ik_method="dls", + jacobian=jacobian, + device=env.device, + ) + + # Update joint positions + joint_pos = joint_pos + delta_dof_pos + joint_vel = torch.zeros_like(joint_pos) + + # Write to sim + self.robot_asset.set_joint_position_target(joint_pos, env_ids=env_ids) + self.robot_asset.set_joint_velocity_target(joint_vel, env_ids=env_ids) + self.robot_asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + + # Set gripper to grasp position + joint_pos = self.robot_asset.data.joint_pos[env_ids].clone() + + # Get gear types for all environments + all_gear_types = gear_type_manager.get_all_gear_types() + for row_idx, env_id in enumerate(env_ids.tolist()): + gear_key = all_gear_types[env_id] + hand_grasp_width = self.hand_grasp_width[gear_key] + self.gripper_joint_setter_func(joint_pos, [row_idx], self.finger_joints, hand_grasp_width) + + self.robot_asset.set_joint_position_target(joint_pos, joint_ids=self.all_joints, env_ids=env_ids) + self.robot_asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + + # Set gripper to closed position + for row_idx, env_id in enumerate(env_ids.tolist()): + gear_key = all_gear_types[env_id] + hand_close_width = self.hand_close_width[gear_key] + self.gripper_joint_setter_func(joint_pos, [row_idx], self.finger_joints, hand_close_width) + + self.robot_asset.set_joint_position_target(joint_pos, joint_ids=self.all_joints, env_ids=env_ids) + + +class randomize_gears_and_base_pose(ManagerTermBase): + """Randomize both the gear base pose and individual gear poses. + + This class-based term pre-caches all tensors needed for randomization. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the randomize gears and base pose term. + + Args: + cfg: Event term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Pre-allocate gear type mapping and indices + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + + # Cache asset names + self.gear_asset_names = ["factory_gear_small", "factory_gear_medium", "factory_gear_large"] + self.base_asset_name = "factory_gear_base" + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict = {}, + velocity_range: dict = {}, + gear_pos_range: dict = {}, + ): + """Randomize gear base and gear poses. + + Args: + env: Environment instance + env_ids: Environment IDs to randomize + pose_range: Pose randomization range for base and all gears + velocity_range: Velocity randomization range + gear_pos_range: Additional position randomization for selected gear only + """ + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this event term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + device = env.device + + # Shared pose samples for all assets + pose_keys = ["x", "y", "z", "roll", "pitch", "yaw"] + range_list_pose = [pose_range.get(key, (0.0, 0.0)) for key in pose_keys] + ranges_pose = torch.tensor(range_list_pose, device=device) + rand_pose_samples = math_utils.sample_uniform( + ranges_pose[:, 0], ranges_pose[:, 1], (len(env_ids), 6), device=device + ) + + orientations_delta = math_utils.quat_from_euler_xyz( + rand_pose_samples[:, 3], rand_pose_samples[:, 4], rand_pose_samples[:, 5] + ) + + # Shared velocity samples + range_list_vel = [velocity_range.get(key, (0.0, 0.0)) for key in pose_keys] + ranges_vel = torch.tensor(range_list_vel, device=device) + rand_vel_samples = math_utils.sample_uniform( + ranges_vel[:, 0], ranges_vel[:, 1], (len(env_ids), 6), device=device + ) + + # Prepare poses for all assets + positions_by_asset = {} + orientations_by_asset = {} + velocities_by_asset = {} + + asset_names_to_process = [self.base_asset_name] + self.gear_asset_names + for asset_name in asset_names_to_process: + asset: RigidObject | Articulation = env.scene[asset_name] + root_states = asset.data.default_root_state[env_ids].clone() + positions = root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_pose_samples[:, 0:3] + orientations = math_utils.quat_mul(root_states[:, 3:7], orientations_delta) + velocities = root_states[:, 7:13] + rand_vel_samples + positions_by_asset[asset_name] = positions + orientations_by_asset[asset_name] = orientations + velocities_by_asset[asset_name] = velocities + + # Per-env gear offset (gear_pos_range) applied only to selected gear + range_list_gear = [gear_pos_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges_gear = torch.tensor(range_list_gear, device=device) + rand_gear_offsets = math_utils.sample_uniform( + ranges_gear[:, 0], ranges_gear[:, 1], (len(env_ids), 3), device=device + ) + + # Get gear type indices directly as tensor + num_reset_envs = len(env_ids) + gear_type_indices = self.gear_type_indices[:num_reset_envs] + all_gear_type_indices = gear_type_manager.get_all_gear_type_indices() + gear_type_indices[:] = all_gear_type_indices[env_ids] + + # Apply offsets using vectorized operations with masks + for gear_idx, asset_name in enumerate(self.gear_asset_names): + if asset_name in positions_by_asset: + mask = gear_type_indices == gear_idx + positions_by_asset[asset_name][mask] = positions_by_asset[asset_name][mask] + rand_gear_offsets[mask] + + # Write to sim + for asset_name in positions_by_asset.keys(): + asset = env.scene[asset_name] + positions = positions_by_asset[asset_name] + orientations = orientations_by_asset[asset_name] + velocities = velocities_by_asset[asset_name] + 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) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/noise_models.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/noise_models.py new file mode 100644 index 000000000000..2d5411e96977 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/noise_models.py @@ -0,0 +1,109 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Noise models specific to deployment tasks.""" + +from __future__ import annotations + +__all__ = ["ResetSampledConstantNoiseModel", "ResetSampledConstantNoiseModelCfg"] + +from collections.abc import Sequence +from dataclasses import MISSING +from typing import TYPE_CHECKING + +import torch + +from isaaclab.utils import configclass +from isaaclab.utils.noise import NoiseModel, NoiseModelCfg + +if TYPE_CHECKING: + from isaaclab.utils.noise import NoiseCfg + + +class ResetSampledConstantNoiseModel(NoiseModel): + """Noise model that samples noise ONLY during reset and applies it consistently. + + The noise is sampled from the configured distribution ONLY during reset and applied consistently + until the next reset. Unlike regular noise that generates new random values every step, + this model maintains the same noise values throughout an episode. + + Note: + This noise model was used since the noise randimization should only be done at reset time. + Other noise models(Eg: GaussianNoise) were not used since this randomizes the noise at every time-step. + """ + + def __init__(self, noise_model_cfg: NoiseModelCfg, num_envs: int, device: str): + # initialize parent class + super().__init__(noise_model_cfg, num_envs, device) + # store the noise configuration + self._noise_cfg = noise_model_cfg.noise_cfg + self._sampled_noise = torch.zeros((num_envs, 1), device=self._device) + self._num_components: int | None = None + + def reset(self, env_ids: Sequence[int] | None = None): + """Reset the noise model by sampling NEW noise values. + + This method samples new noise for the specified environments using the configured noise function. + The sampled noise will remain constant until the next reset. + + 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) + + # Use the existing noise function to sample new noise + # Create dummy data to sample from the noise function + dummy_data = torch.zeros( + (env_ids.stop - env_ids.start if isinstance(env_ids, slice) else len(env_ids), 1), device=self._device + ) + + # Sample noise using the configured noise function + sampled_noise = self._noise_model_cfg.noise_cfg.func(dummy_data, self._noise_model_cfg.noise_cfg) + + self._sampled_noise[env_ids] = sampled_noise + + def __call__(self, data: torch.Tensor) -> torch.Tensor: + """Apply the pre-sampled noise to the data. + + This method applies the noise that was sampled during the last reset. + No new noise is generated - the same values are used consistently. + + 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. + """ + # on first apply, expand noise to match last dim of data + if self._num_components is None: + *_, self._num_components = data.shape + # expand noise from (num_envs,1) to (num_envs, num_components) + self._sampled_noise = self._sampled_noise.repeat(1, self._num_components) + + # apply the noise based on operation + if self._noise_cfg.operation == "add": + return data + self._sampled_noise + elif self._noise_cfg.operation == "scale": + return data * self._sampled_noise + elif self._noise_cfg.operation == "abs": + return self._sampled_noise + else: + raise ValueError(f"Unknown operation in noise: {self._noise_cfg.operation}") + + +@configclass +class ResetSampledConstantNoiseModelCfg(NoiseModelCfg): + """Configuration for a noise model that samples noise ONLY during reset.""" + + class_type: type = ResetSampledConstantNoiseModel + + noise_cfg: NoiseCfg = MISSING + """The noise configuration for the noise. + + Based on this configuration, the noise is sampled at every reset of the noise model. + """ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/observations.py new file mode 100644 index 000000000000..cf9ae56ee2da --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/observations.py @@ -0,0 +1,342 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Class-based observation terms for the gear assembly manipulation environment.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import RigidObject +from isaaclab.managers import ManagerTermBase, ObservationTermCfg, SceneEntityCfg +from isaaclab.utils.math import combine_frame_transforms + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + from .events import randomize_gear_type + + +class gear_shaft_pos_w(ManagerTermBase): + """Gear shaft position in world frame with offset applied. + + This class-based term caches gear offset tensors and identity quaternions for efficient computation + across all environments. It transforms the gear base position by the appropriate offset based on the + active gear type in each environment. + + Args: + asset_cfg: The asset configuration for the gear base. Defaults to SceneEntityCfg("factory_gear_base"). + gear_offsets: A dictionary mapping gear type names to their shaft offsets in the gear base frame. + Required keys are "gear_small", "gear_medium", and "gear_large", each mapping to a 3D offset + list [x, y, z]. This parameter is required and must be provided in the configuration. + + Returns: + Gear shaft position tensor in the environment frame with shape (num_envs, 3). + + Raises: + ValueError: If the 'gear_offsets' parameter is not provided in the configuration. + TypeError: If the 'gear_offsets' parameter is not a dictionary. + ValueError: If any of the required gear type keys are missing from 'gear_offsets'. + RuntimeError: If the gear type manager is not initialized in the environment. + """ + + def __init__(self, cfg: ObservationTermCfg, env: ManagerBasedRLEnv): + """Initialize the gear shaft position observation term. + + Args: + cfg: Observation term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Cache asset + self.asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("factory_gear_base")) + self.asset: RigidObject = env.scene[self.asset_cfg.name] + + # Pre-cache gear offset tensors (required parameter) + if "gear_offsets" not in cfg.params: + raise ValueError( + "'gear_offsets' parameter is required in gear_shaft_pos_w configuration. " + "It should be a dict with keys 'gear_small', 'gear_medium', 'gear_large' mapping to [x, y, z] offsets." + ) + gear_offsets = cfg.params["gear_offsets"] + if not isinstance(gear_offsets, dict): + raise TypeError( + f"'gear_offsets' parameter must be a dict, got {type(gear_offsets).__name__}. " + "It should have keys 'gear_small', 'gear_medium', 'gear_large' mapping to [x, y, z] offsets." + ) + + self.gear_offset_tensors = {} + for gear_type in ["gear_small", "gear_medium", "gear_large"]: + if gear_type not in gear_offsets: + raise ValueError( + f"'{gear_type}' offset is required in 'gear_offsets' parameter. " + f"Found keys: {list(gear_offsets.keys())}" + ) + self.gear_offset_tensors[gear_type] = torch.tensor( + gear_offsets[gear_type], device=env.device, dtype=torch.float32 + ) + + # Stack offset tensors for vectorized indexing (shape: 3, 3) + # Index 0=small, 1=medium, 2=large + self.gear_offsets_stacked = torch.stack( + [ + self.gear_offset_tensors["gear_small"], + self.gear_offset_tensors["gear_medium"], + self.gear_offset_tensors["gear_large"], + ], + dim=0, + ) + + # Pre-allocate buffers + self.offsets_buffer = torch.zeros(env.num_envs, 3, device=env.device, dtype=torch.float32) + self.env_indices = torch.arange(env.num_envs, device=env.device) + self.identity_quat = ( + torch.tensor([[1.0, 0.0, 0.0, 0.0]], device=env.device, dtype=torch.float32) + .repeat(env.num_envs, 1) + .contiguous() + ) + + def __call__( + self, + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("factory_gear_base"), + gear_offsets: dict | None = None, + ) -> torch.Tensor: + """Compute gear shaft position in world frame. + + Args: + env: Environment instance + asset_cfg: Configuration of the gear base asset (unused, kept for compatibility) + + Returns: + Gear shaft position tensor of shape (num_envs, 3) + """ + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this observation term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + # Get gear type indices directly as tensor (no Python loops!) + gear_type_indices = gear_type_manager.get_all_gear_type_indices() + + # Get base gear position and orientation + base_pos = self.asset.data.root_pos_w + base_quat = self.asset.data.root_quat_w + + # Update offsets using vectorized indexing + self.offsets_buffer = self.gear_offsets_stacked[gear_type_indices] + + # Transform offsets + shaft_pos, _ = combine_frame_transforms(base_pos, base_quat, self.offsets_buffer, self.identity_quat) + + return shaft_pos - env.scene.env_origins + + +class gear_shaft_quat_w(ManagerTermBase): + """Gear shaft orientation in world frame. + + This class-based term returns the orientation of the gear base (which is the same as the gear shaft + orientation). The quaternion is canonicalized to ensure the w component is positive, reducing + observation variation for the policy. + + Args: + asset_cfg: The asset configuration for the gear base. Defaults to SceneEntityCfg("factory_gear_base"). + + Returns: + Gear shaft orientation tensor as a quaternion (w, x, y, z) with shape (num_envs, 4). + """ + + def __init__(self, cfg: ObservationTermCfg, env: ManagerBasedRLEnv): + """Initialize the gear shaft orientation observation term. + + Args: + cfg: Observation term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Cache asset + self.asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("factory_gear_base")) + self.asset: RigidObject = env.scene[self.asset_cfg.name] + + def __call__( + self, + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("factory_gear_base"), + ) -> torch.Tensor: + """Compute gear shaft orientation in world frame. + + Args: + env: Environment instance + asset_cfg: Configuration of the gear base asset (unused, kept for compatibility) + + Returns: + Gear shaft orientation tensor of shape (num_envs, 4) + """ + # Get base quaternion + base_quat = self.asset.data.root_quat_w + + # Ensure w component is positive (q and -q represent the same rotation) + # Pick one canonical form to reduce observation variation seen by the policy + w_negative = base_quat[:, 0] < 0 + positive_quat = base_quat.clone() + positive_quat[w_negative] = -base_quat[w_negative] + + return positive_quat + + +class gear_pos_w(ManagerTermBase): + """Gear position in world frame. + + This class-based term returns the position of the active gear in each environment. It uses + vectorized indexing to efficiently select the correct gear position based on the gear type + (small, medium, or large) active in each environment. + + Returns: + Gear position tensor in the environment frame with shape (num_envs, 3). + + Raises: + RuntimeError: If the gear type manager is not initialized in the environment. + """ + + def __init__(self, cfg: ObservationTermCfg, env: ManagerBasedRLEnv): + """Initialize the gear position observation term. + + Args: + cfg: Observation term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Pre-allocate gear type mapping and indices + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + self.env_indices = torch.arange(env.num_envs, device=env.device) + + # Cache gear assets + self.gear_assets = { + "gear_small": env.scene["factory_gear_small"], + "gear_medium": env.scene["factory_gear_medium"], + "gear_large": env.scene["factory_gear_large"], + } + + def __call__(self, env: ManagerBasedRLEnv) -> torch.Tensor: + """Compute gear position in world frame. + + Args: + env: Environment instance + + Returns: + Gear position tensor of shape (num_envs, 3) + """ + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this observation term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + # Get gear type indices directly as tensor (no Python loops!) + self.gear_type_indices = gear_type_manager.get_all_gear_type_indices() + + # Stack all gear positions + all_gear_positions = torch.stack( + [ + self.gear_assets["gear_small"].data.root_pos_w, + self.gear_assets["gear_medium"].data.root_pos_w, + self.gear_assets["gear_large"].data.root_pos_w, + ], + dim=1, + ) + + # Select gear positions using advanced indexing + gear_positions = all_gear_positions[self.env_indices, self.gear_type_indices] + + return gear_positions - env.scene.env_origins + + +class gear_quat_w(ManagerTermBase): + """Gear orientation in world frame. + + This class-based term returns the orientation of the active gear in each environment. It uses + vectorized indexing to efficiently select the correct gear orientation based on the gear type + (small, medium, or large) active in each environment. The quaternion is canonicalized to ensure + the w component is positive, reducing observation variation for the policy. + + Returns: + Gear orientation tensor as a quaternion (w, x, y, z) with shape (num_envs, 4). + + Raises: + RuntimeError: If the gear type manager is not initialized in the environment. + """ + + def __init__(self, cfg: ObservationTermCfg, env: ManagerBasedRLEnv): + """Initialize the gear orientation observation term. + + Args: + cfg: Observation term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Pre-allocate gear type mapping and indices + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + self.env_indices = torch.arange(env.num_envs, device=env.device) + + # Cache gear assets + self.gear_assets = { + "gear_small": env.scene["factory_gear_small"], + "gear_medium": env.scene["factory_gear_medium"], + "gear_large": env.scene["factory_gear_large"], + } + + def __call__(self, env: ManagerBasedRLEnv) -> torch.Tensor: + """Compute gear orientation in world frame. + + Args: + env: Environment instance + + Returns: + Gear orientation tensor of shape (num_envs, 4) + """ + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this observation term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + # Get gear type indices directly as tensor (no Python loops!) + self.gear_type_indices = gear_type_manager.get_all_gear_type_indices() + + # Stack all gear quaternions + all_gear_quat = torch.stack( + [ + self.gear_assets["gear_small"].data.root_quat_w, + self.gear_assets["gear_medium"].data.root_quat_w, + self.gear_assets["gear_large"].data.root_quat_w, + ], + dim=1, + ) + + # Select gear quaternions using advanced indexing + gear_quat = all_gear_quat[self.env_indices, self.gear_type_indices] + + # Ensure w component is positive (q and -q represent the same rotation) + # Pick one canonical form to reduce observation variation seen by the policy + w_negative = gear_quat[:, 0] < 0 + gear_positive_quat = gear_quat.clone() + gear_positive_quat[w_negative] = -gear_quat[w_negative] + + return gear_positive_quat diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/rewards.py new file mode 100644 index 000000000000..482cab8f69b8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/rewards.py @@ -0,0 +1,513 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Class-based reward terms for the gear assembly manipulation environment.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import ManagerTermBase, RewardTermCfg, SceneEntityCfg +from isaaclab.sensors.frame_transformer.frame_transformer import FrameTransformer +from isaaclab.utils.math import combine_frame_transforms + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + from .events import randomize_gear_type + + +class keypoint_command_error(ManagerTermBase): + """Compute keypoint distance between current and desired poses from command. + + This class-based term uses _compute_keypoint_distance internally. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + """Initialize the keypoint command error term. + + Args: + cfg: Reward term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Cache asset configuration + self.asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("ee_frame")) + self.command_name: str = cfg.params.get("command_name", "ee_pose") + + # Create keypoint distance computer + self.keypoint_computer = _compute_keypoint_distance(cfg, env) + + def __call__( + self, + env: ManagerBasedRLEnv, + command_name: str, + asset_cfg: SceneEntityCfg, + keypoint_scale: float = 1.0, + add_cube_center_kp: bool = True, + ) -> torch.Tensor: + """Compute keypoint distance error. + + Args: + env: Environment instance + command_name: Name of the command containing desired pose + asset_cfg: Configuration of the asset to track + keypoint_scale: Scale factor for keypoint offsets + add_cube_center_kp: Whether to include center keypoint + + Returns: + Mean keypoint distance tensor of shape (num_envs,) + """ + # Extract frame transformer sensor + asset: FrameTransformer = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + + # Get desired pose from command + des_pos_w = command[:, :3] + des_quat_w = command[:, 3:7] + + # Get current pose from frame transformer + curr_pos_w = asset.data.target_pos_source[:, 0] + curr_quat_w = asset.data.target_quat_source[:, 0] + + # Compute keypoint distance + keypoint_dist_sep = self.keypoint_computer.compute( + current_pos=curr_pos_w, + current_quat=curr_quat_w, + target_pos=des_pos_w, + target_quat=des_quat_w, + keypoint_scale=keypoint_scale, + ) + + return keypoint_dist_sep.mean(-1) + + +class keypoint_command_error_exp(ManagerTermBase): + """Compute exponential keypoint reward between current and desired poses from command. + + This class-based term uses _compute_keypoint_distance internally and applies + exponential reward transformation. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + """Initialize the keypoint command error exponential term. + + Args: + cfg: Reward term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Cache asset configuration + self.asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("ee_frame")) + self.command_name: str = cfg.params.get("command_name", "ee_pose") + + # Create keypoint distance computer + self.keypoint_computer = _compute_keypoint_distance(cfg, env) + + def __call__( + self, + env: ManagerBasedRLEnv, + command_name: str, + asset_cfg: SceneEntityCfg, + kp_exp_coeffs: list[tuple[float, float]] = [(1.0, 0.1)], + kp_use_sum_of_exps: bool = True, + keypoint_scale: float = 1.0, + add_cube_center_kp: bool = True, + ) -> torch.Tensor: + """Compute exponential keypoint reward. + + Args: + env: Environment instance + command_name: Name of the command containing desired pose + asset_cfg: Configuration of the asset to track + kp_exp_coeffs: List of (a, b) coefficient pairs for exponential reward + kp_use_sum_of_exps: Whether to use sum of exponentials + keypoint_scale: Scale factor for keypoint offsets + add_cube_center_kp: Whether to include center keypoint + + Returns: + Exponential keypoint reward tensor of shape (num_envs,) + """ + # Extract frame transformer sensor + asset: FrameTransformer = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + + # Get desired pose from command + des_pos_w = command[:, :3] + des_quat_w = command[:, 3:7] + + # Get current pose from frame transformer + curr_pos_w = asset.data.target_pos_source[:, 0] + curr_quat_w = asset.data.target_quat_source[:, 0] + + # Compute keypoint distance + keypoint_dist_sep = self.keypoint_computer.compute( + current_pos=curr_pos_w, + current_quat=curr_quat_w, + target_pos=des_pos_w, + target_quat=des_quat_w, + keypoint_scale=keypoint_scale, + ) + + # Compute exponential reward + keypoint_reward_exp = torch.zeros_like(keypoint_dist_sep[:, 0]) + + if kp_use_sum_of_exps: + for coeff in kp_exp_coeffs: + a, b = coeff + keypoint_reward_exp += ( + 1.0 / (torch.exp(a * keypoint_dist_sep) + b + torch.exp(-a * keypoint_dist_sep)) + ).mean(-1) + else: + keypoint_dist = keypoint_dist_sep.mean(-1) + for coeff in kp_exp_coeffs: + a, b = coeff + keypoint_reward_exp += 1.0 / (torch.exp(a * keypoint_dist) + b + torch.exp(-a * keypoint_dist)) + + return keypoint_reward_exp + + +class keypoint_entity_error(ManagerTermBase): + """Compute keypoint distance between a RigidObject and the dynamically selected gear. + + This class-based term pre-caches gear type mapping and asset references. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + """Initialize the keypoint entity error term. + + Args: + cfg: Reward term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Cache asset configuration + self.asset_cfg_1: SceneEntityCfg = cfg.params.get("asset_cfg_1", SceneEntityCfg("factory_gear_base")) + self.asset_1 = env.scene[self.asset_cfg_1.name] + + # Pre-allocate gear type mapping and indices + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + self.env_indices = torch.arange(env.num_envs, device=env.device) + + # Cache gear assets + self.gear_assets = { + "gear_small": env.scene["factory_gear_small"], + "gear_medium": env.scene["factory_gear_medium"], + "gear_large": env.scene["factory_gear_large"], + } + + # Create keypoint distance computer + self.keypoint_computer = _compute_keypoint_distance(cfg, env) + + def __call__( + self, + env: ManagerBasedRLEnv, + asset_cfg_1: SceneEntityCfg, + keypoint_scale: float = 1.0, + add_cube_center_kp: bool = True, + ) -> torch.Tensor: + """Compute keypoint distance error. + + Args: + env: Environment instance + asset_cfg_1: Configuration of the first asset (RigidObject) + keypoint_scale: Scale factor for keypoint offsets + add_cube_center_kp: Whether to include center keypoint + + Returns: + Mean keypoint distance tensor of shape (num_envs,) + """ + # Get current pose of asset_1 (RigidObject) + curr_pos_1 = self.asset_1.data.body_pos_w[:, 0] + curr_quat_1 = self.asset_1.data.body_quat_w[:, 0] + + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this reward term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + # Get gear type indices directly as tensor + self.gear_type_indices = gear_type_manager.get_all_gear_type_indices() + + # Stack all gear positions and quaternions + all_gear_pos = torch.stack( + [ + self.gear_assets["gear_small"].data.body_pos_w[:, 0], + self.gear_assets["gear_medium"].data.body_pos_w[:, 0], + self.gear_assets["gear_large"].data.body_pos_w[:, 0], + ], + dim=1, + ) + + all_gear_quat = torch.stack( + [ + self.gear_assets["gear_small"].data.body_quat_w[:, 0], + self.gear_assets["gear_medium"].data.body_quat_w[:, 0], + self.gear_assets["gear_large"].data.body_quat_w[:, 0], + ], + dim=1, + ) + + # Select positions and quaternions using advanced indexing + curr_pos_2 = all_gear_pos[self.env_indices, self.gear_type_indices] + curr_quat_2 = all_gear_quat[self.env_indices, self.gear_type_indices] + + # Compute keypoint distance + keypoint_dist_sep = self.keypoint_computer.compute( + current_pos=curr_pos_1, + current_quat=curr_quat_1, + target_pos=curr_pos_2, + target_quat=curr_quat_2, + keypoint_scale=keypoint_scale, + ) + + return keypoint_dist_sep.mean(-1) + + +class keypoint_entity_error_exp(ManagerTermBase): + """Compute exponential keypoint reward between a RigidObject and the dynamically selected gear. + + This class-based term pre-caches gear type mapping and asset references. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + """Initialize the keypoint entity error exponential term. + + Args: + cfg: Reward term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Cache asset configuration + self.asset_cfg_1: SceneEntityCfg = cfg.params.get("asset_cfg_1", SceneEntityCfg("factory_gear_base")) + self.asset_1 = env.scene[self.asset_cfg_1.name] + + # Pre-allocate gear type mapping and indices + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + self.env_indices = torch.arange(env.num_envs, device=env.device) + + # Cache gear assets + self.gear_assets = { + "gear_small": env.scene["factory_gear_small"], + "gear_medium": env.scene["factory_gear_medium"], + "gear_large": env.scene["factory_gear_large"], + } + + # Create keypoint distance computer + self.keypoint_computer = _compute_keypoint_distance(cfg, env) + + def __call__( + self, + env: ManagerBasedRLEnv, + asset_cfg_1: SceneEntityCfg, + kp_exp_coeffs: list[tuple[float, float]] = [(1.0, 0.1)], + kp_use_sum_of_exps: bool = True, + keypoint_scale: float = 1.0, + add_cube_center_kp: bool = True, + ) -> torch.Tensor: + """Compute exponential keypoint reward. + + Args: + env: Environment instance + asset_cfg_1: Configuration of the first asset (RigidObject) + kp_exp_coeffs: List of (a, b) coefficient pairs for exponential reward + kp_use_sum_of_exps: Whether to use sum of exponentials + keypoint_scale: Scale factor for keypoint offsets + add_cube_center_kp: Whether to include center keypoint + + Returns: + Exponential keypoint reward tensor of shape (num_envs,) + """ + # Get current pose of asset_1 (RigidObject) + curr_pos_1 = self.asset_1.data.body_pos_w[:, 0] + curr_quat_1 = self.asset_1.data.body_quat_w[:, 0] + + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this reward term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + # Get gear type indices directly as tensor + self.gear_type_indices = gear_type_manager.get_all_gear_type_indices() + + # Stack all gear positions and quaternions + all_gear_pos = torch.stack( + [ + self.gear_assets["gear_small"].data.body_pos_w[:, 0], + self.gear_assets["gear_medium"].data.body_pos_w[:, 0], + self.gear_assets["gear_large"].data.body_pos_w[:, 0], + ], + dim=1, + ) + + all_gear_quat = torch.stack( + [ + self.gear_assets["gear_small"].data.body_quat_w[:, 0], + self.gear_assets["gear_medium"].data.body_quat_w[:, 0], + self.gear_assets["gear_large"].data.body_quat_w[:, 0], + ], + dim=1, + ) + + # Select positions and quaternions using advanced indexing + curr_pos_2 = all_gear_pos[self.env_indices, self.gear_type_indices] + curr_quat_2 = all_gear_quat[self.env_indices, self.gear_type_indices] + + # Compute keypoint distance + keypoint_dist_sep = self.keypoint_computer.compute( + current_pos=curr_pos_1, + current_quat=curr_quat_1, + target_pos=curr_pos_2, + target_quat=curr_quat_2, + keypoint_scale=keypoint_scale, + ) + + # Compute exponential reward + keypoint_reward_exp = torch.zeros_like(keypoint_dist_sep[:, 0]) + + if kp_use_sum_of_exps: + for coeff in kp_exp_coeffs: + a, b = coeff + keypoint_reward_exp += ( + 1.0 / (torch.exp(a * keypoint_dist_sep) + b + torch.exp(-a * keypoint_dist_sep)) + ).mean(-1) + else: + keypoint_dist = keypoint_dist_sep.mean(-1) + for coeff in kp_exp_coeffs: + a, b = coeff + keypoint_reward_exp += 1.0 / (torch.exp(a * keypoint_dist) + b + torch.exp(-a * keypoint_dist)) + + return keypoint_reward_exp + + +## +# Helper functions and classes +## + + +def _get_keypoint_offsets_full_6d(add_cube_center_kp: bool = False, device: torch.device | None = None) -> torch.Tensor: + """Get keypoints for pose alignment comparison. Pose is aligned if all axis are aligned. + + Args: + add_cube_center_kp: Whether to include the center keypoint (0, 0, 0) + device: Device to create the tensor on + + Returns: + Keypoint offsets tensor of shape (num_keypoints, 3) + """ + if add_cube_center_kp: + keypoint_corners = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]] + else: + keypoint_corners = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + + keypoint_corners = torch.tensor(keypoint_corners, device=device, dtype=torch.float32) + keypoint_corners = torch.cat((keypoint_corners, -keypoint_corners[-3:]), dim=0) + + return keypoint_corners + + +class _compute_keypoint_distance: + """Compute keypoint distance between current and target poses. + + This helper class pre-caches keypoint offsets and identity quaternions + to avoid repeated allocations during reward computation. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + """Initialize the compute keypoint distance helper. + + Args: + cfg: Reward term configuration + env: Environment instance + """ + # Get keypoint configuration + add_cube_center_kp = cfg.params.get("add_cube_center_kp", True) + + # Pre-compute base keypoint offsets (unscaled) + self.keypoint_offsets_base = _get_keypoint_offsets_full_6d( + add_cube_center_kp=add_cube_center_kp, device=env.device + ) + self.num_keypoints = self.keypoint_offsets_base.shape[0] + + # Pre-allocate identity quaternion for keypoint transforms + self.identity_quat_keypoints = ( + torch.tensor([[1.0, 0.0, 0.0, 0.0]], device=env.device, dtype=torch.float32) + .repeat(env.num_envs * self.num_keypoints, 1) + .contiguous() + ) + + # Pre-allocate buffer for batched keypoint offsets + self.keypoint_offsets_buffer = torch.zeros( + env.num_envs, self.num_keypoints, 3, device=env.device, dtype=torch.float32 + ) + + def compute( + self, + current_pos: torch.Tensor, + current_quat: torch.Tensor, + target_pos: torch.Tensor, + target_quat: torch.Tensor, + keypoint_scale: float = 1.0, + ) -> torch.Tensor: + """Compute keypoint distance between current and target poses. + + Args: + current_pos: Current position tensor of shape (num_envs, 3) + current_quat: Current quaternion tensor of shape (num_envs, 4) + target_pos: Target position tensor of shape (num_envs, 3) + target_quat: Target quaternion tensor of shape (num_envs, 4) + keypoint_scale: Scale factor for keypoint offsets + + Returns: + Keypoint distance tensor of shape (num_envs, num_keypoints) + """ + num_envs = current_pos.shape[0] + + # Scale keypoint offsets + keypoint_offsets = self.keypoint_offsets_base * keypoint_scale + + # Create batched keypoints (in-place operation) + self.keypoint_offsets_buffer[:num_envs] = keypoint_offsets.unsqueeze(0) + + # Flatten for batch processing + keypoint_offsets_flat = self.keypoint_offsets_buffer[:num_envs].reshape(-1, 3) + identity_quat = self.identity_quat_keypoints[: num_envs * self.num_keypoints] + + # Expand quaternions and positions for all keypoints + current_quat_expanded = current_quat.unsqueeze(1).expand(-1, self.num_keypoints, -1).reshape(-1, 4) + current_pos_expanded = current_pos.unsqueeze(1).expand(-1, self.num_keypoints, -1).reshape(-1, 3) + target_quat_expanded = target_quat.unsqueeze(1).expand(-1, self.num_keypoints, -1).reshape(-1, 4) + target_pos_expanded = target_pos.unsqueeze(1).expand(-1, self.num_keypoints, -1).reshape(-1, 3) + + # Transform all keypoints at once + keypoints_current_flat, _ = combine_frame_transforms( + current_pos_expanded, current_quat_expanded, keypoint_offsets_flat, identity_quat + ) + keypoints_target_flat, _ = combine_frame_transforms( + target_pos_expanded, target_quat_expanded, keypoint_offsets_flat, identity_quat + ) + + # Reshape back + keypoints_current = keypoints_current_flat.reshape(num_envs, self.num_keypoints, 3) + keypoints_target = keypoints_target_flat.reshape(num_envs, self.num_keypoints, 3) + + # Calculate L2 norm distance + keypoint_dist_sep = torch.norm(keypoints_target - keypoints_current, p=2, dim=-1) + + return keypoint_dist_sep diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/terminations.py new file mode 100644 index 000000000000..b623148c5b3b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/terminations.py @@ -0,0 +1,331 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Class-based termination terms specific to the gear assembly manipulation environments.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import carb + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation +from isaaclab.managers import ManagerTermBase, SceneEntityCfg, TerminationTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .events import randomize_gear_type + + +class reset_when_gear_dropped(ManagerTermBase): + """Check if the gear has fallen out of the gripper and return reset flags. + + This class-based term pre-caches all required tensors and gear offsets. + """ + + def __init__(self, cfg: TerminationTermCfg, env: ManagerBasedEnv): + """Initialize the reset when gear dropped term. + + Args: + cfg: Termination term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Get robot asset configuration + self.robot_asset_cfg: SceneEntityCfg = cfg.params.get("robot_asset_cfg", SceneEntityCfg("robot")) + self.robot_asset: Articulation = env.scene[self.robot_asset_cfg.name] + + # Validate required parameters + if "end_effector_body_name" not in cfg.params: + raise ValueError( + "'end_effector_body_name' parameter is required in reset_when_gear_dropped configuration. " + "Example: 'wrist_3_link'" + ) + if "grasp_rot_offset" not in cfg.params: + raise ValueError( + "'grasp_rot_offset' parameter is required in reset_when_gear_dropped configuration. " + "It should be a quaternion [w, x, y, z]. Example: [0.0, 0.707, 0.707, 0.0]" + ) + + self.end_effector_body_name = cfg.params["end_effector_body_name"] + + # Pre-cache gear grasp offsets as tensors (required parameter) + if "gear_offsets_grasp" not in cfg.params: + raise ValueError( + "'gear_offsets_grasp' parameter is required in reset_when_gear_dropped configuration. " + "It should be a dict with keys 'gear_small', 'gear_medium', 'gear_large' mapping to [x, y, z] offsets." + ) + gear_offsets_grasp = cfg.params["gear_offsets_grasp"] + if not isinstance(gear_offsets_grasp, dict): + raise TypeError( + f"'gear_offsets_grasp' parameter must be a dict, got {type(gear_offsets_grasp).__name__}. " + "It should have keys 'gear_small', 'gear_medium', 'gear_large' mapping to [x, y, z] offsets." + ) + + self.gear_grasp_offset_tensors = {} + for gear_type in ["gear_small", "gear_medium", "gear_large"]: + if gear_type not in gear_offsets_grasp: + raise ValueError( + f"'{gear_type}' offset is required in 'gear_offsets_grasp' parameter. " + f"Found keys: {list(gear_offsets_grasp.keys())}" + ) + self.gear_grasp_offset_tensors[gear_type] = torch.tensor( + gear_offsets_grasp[gear_type], device=env.device, dtype=torch.float32 + ) + + # Stack grasp offset tensors for vectorized indexing (shape: 3, 3) + # Index 0=small, 1=medium, 2=large + self.gear_grasp_offsets_stacked = torch.stack( + [ + self.gear_grasp_offset_tensors["gear_small"], + self.gear_grasp_offset_tensors["gear_medium"], + self.gear_grasp_offset_tensors["gear_large"], + ], + dim=0, + ) + + # Pre-cache grasp rotation offset tensor + grasp_rot_offset = cfg.params["grasp_rot_offset"] + self.grasp_rot_offset_tensor = ( + torch.tensor(grasp_rot_offset, device=env.device, dtype=torch.float32).unsqueeze(0).repeat(env.num_envs, 1) + ) + + # Pre-allocate buffers + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + self.env_indices = torch.arange(env.num_envs, device=env.device) + self.gear_grasp_offsets_buffer = torch.zeros(env.num_envs, 3, device=env.device, dtype=torch.float32) + self.all_gear_pos_buffer = torch.zeros(env.num_envs, 3, 3, device=env.device, dtype=torch.float32) + self.all_gear_quat_buffer = torch.zeros(env.num_envs, 3, 4, device=env.device, dtype=torch.float32) + self.reset_flags = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + + # Cache gear assets + self.gear_assets = { + "gear_small": env.scene["factory_gear_small"], + "gear_medium": env.scene["factory_gear_medium"], + "gear_large": env.scene["factory_gear_large"], + } + + # Find end effector index once + eef_indices, _ = self.robot_asset.find_bodies([self.end_effector_body_name]) + if len(eef_indices) == 0: + carb.log_warn( + f"{self.end_effector_body_name} not found in robot body names. Cannot check gear drop condition." + ) + self.eef_idx = None + else: + self.eef_idx = eef_indices[0] + + def __call__( + self, + env: ManagerBasedEnv, + distance_threshold: float = 0.1, + robot_asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + gear_offsets_grasp: dict | None = None, + end_effector_body_name: str | None = None, + grasp_rot_offset: list | None = None, + ) -> torch.Tensor: + """Check if gear has dropped and return reset flags. + + Args: + env: Environment instance + distance_threshold: Maximum allowed distance between gear grasp point and gripper + robot_asset_cfg: Configuration for the robot asset (unused, kept for compatibility) + + Returns: + Boolean tensor indicating which environments should be reset + """ + # Reset flags + self.reset_flags.fill_(False) + + if self.eef_idx is None: + return self.reset_flags + + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this termination term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + # Get gear type indices directly as tensor (no Python loops!) + self.gear_type_indices = gear_type_manager.get_all_gear_type_indices() + + # Get end effector position + eef_pos_world = self.robot_asset.data.body_link_pos_w[:, self.eef_idx] + + # Update gear positions and quaternions in buffers + self.all_gear_pos_buffer[:, 0, :] = self.gear_assets["gear_small"].data.root_link_pos_w + self.all_gear_pos_buffer[:, 1, :] = self.gear_assets["gear_medium"].data.root_link_pos_w + self.all_gear_pos_buffer[:, 2, :] = self.gear_assets["gear_large"].data.root_link_pos_w + + self.all_gear_quat_buffer[:, 0, :] = self.gear_assets["gear_small"].data.root_link_quat_w + self.all_gear_quat_buffer[:, 1, :] = self.gear_assets["gear_medium"].data.root_link_quat_w + self.all_gear_quat_buffer[:, 2, :] = self.gear_assets["gear_large"].data.root_link_quat_w + + # Select gear data using advanced indexing + gear_pos_world = self.all_gear_pos_buffer[self.env_indices, self.gear_type_indices] + gear_quat_world = self.all_gear_quat_buffer[self.env_indices, self.gear_type_indices] + + # Apply rotation offset + gear_quat_world = math_utils.quat_mul(gear_quat_world, self.grasp_rot_offset_tensor) + + # Get grasp offsets (vectorized) + self.gear_grasp_offsets_buffer = self.gear_grasp_offsets_stacked[self.gear_type_indices] + + # Transform grasp offsets to world frame + gear_grasp_pos_world = gear_pos_world + math_utils.quat_apply(gear_quat_world, self.gear_grasp_offsets_buffer) + + # Compute distances + distances = torch.norm(gear_grasp_pos_world - eef_pos_world, dim=-1) + + # Check distance threshold + self.reset_flags[:] = distances > distance_threshold + + return self.reset_flags + + +class reset_when_gear_orientation_exceeds_threshold(ManagerTermBase): + """Check if the gear's orientation relative to the gripper exceeds thresholds. + + This class-based term pre-caches all required tensors and thresholds. + """ + + def __init__(self, cfg: TerminationTermCfg, env: ManagerBasedEnv): + """Initialize the reset when gear orientation exceeds threshold term. + + Args: + cfg: Termination term configuration + env: Environment instance + """ + super().__init__(cfg, env) + + # Get robot asset configuration + self.robot_asset_cfg: SceneEntityCfg = cfg.params.get("robot_asset_cfg", SceneEntityCfg("robot")) + self.robot_asset: Articulation = env.scene[self.robot_asset_cfg.name] + + # Validate required parameters + if "end_effector_body_name" not in cfg.params: + raise ValueError( + "'end_effector_body_name' parameter is required in reset_when_gear_orientation_exceeds_threshold" + " configuration. Example: 'wrist_3_link'" + ) + if "grasp_rot_offset" not in cfg.params: + raise ValueError( + "'grasp_rot_offset' parameter is required in reset_when_gear_orientation_exceeds_threshold" + " configuration. It should be a quaternion [w, x, y, z]. Example: [0.0, 0.707, 0.707, 0.0]" + ) + + self.end_effector_body_name = cfg.params["end_effector_body_name"] + + # Pre-cache grasp rotation offset tensor + grasp_rot_offset = cfg.params["grasp_rot_offset"] + self.grasp_rot_offset_tensor = ( + torch.tensor(grasp_rot_offset, device=env.device, dtype=torch.float32).unsqueeze(0).repeat(env.num_envs, 1) + ) + + # Pre-allocate buffers + self.gear_type_map = {"gear_small": 0, "gear_medium": 1, "gear_large": 2} + self.gear_type_indices = torch.zeros(env.num_envs, device=env.device, dtype=torch.long) + self.env_indices = torch.arange(env.num_envs, device=env.device) + self.all_gear_quat_buffer = torch.zeros(env.num_envs, 3, 4, device=env.device, dtype=torch.float32) + self.reset_flags = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + + # Cache gear assets + self.gear_assets = { + "gear_small": env.scene["factory_gear_small"], + "gear_medium": env.scene["factory_gear_medium"], + "gear_large": env.scene["factory_gear_large"], + } + + # Find end effector index once + eef_indices, _ = self.robot_asset.find_bodies([self.end_effector_body_name]) + if len(eef_indices) == 0: + carb.log_warn( + f"{self.end_effector_body_name} not found in robot body names. Cannot check gear orientation condition." + ) + self.eef_idx = None + else: + self.eef_idx = eef_indices[0] + + def __call__( + self, + env: ManagerBasedEnv, + roll_threshold_deg: float = 30.0, + pitch_threshold_deg: float = 30.0, + yaw_threshold_deg: float = 180.0, + robot_asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + end_effector_body_name: str | None = None, + grasp_rot_offset: list | None = None, + ) -> torch.Tensor: + """Check if gear orientation exceeds thresholds and return reset flags. + + Args: + env: Environment instance + roll_threshold_deg: Maximum allowed roll angle deviation in degrees + pitch_threshold_deg: Maximum allowed pitch angle deviation in degrees + yaw_threshold_deg: Maximum allowed yaw angle deviation in degrees + robot_asset_cfg: Configuration for the robot asset (unused, kept for compatibility) + + Returns: + Boolean tensor indicating which environments should be reset + """ + # Reset flags + self.reset_flags.fill_(False) + + if self.eef_idx is None: + return self.reset_flags + + # Check if gear type manager exists + if not hasattr(env, "_gear_type_manager"): + raise RuntimeError( + "Gear type manager not initialized. Ensure randomize_gear_type event is configured " + "in your environment's event configuration before this termination term is used." + ) + + gear_type_manager: randomize_gear_type = env._gear_type_manager + # Get gear type indices directly as tensor (no Python loops!) + self.gear_type_indices = gear_type_manager.get_all_gear_type_indices() + + # Convert thresholds to radians + roll_threshold_rad = torch.deg2rad(torch.tensor(roll_threshold_deg, device=env.device)) + pitch_threshold_rad = torch.deg2rad(torch.tensor(pitch_threshold_deg, device=env.device)) + yaw_threshold_rad = torch.deg2rad(torch.tensor(yaw_threshold_deg, device=env.device)) + + # Get end effector orientation + eef_quat_world = self.robot_asset.data.body_link_quat_w[:, self.eef_idx] + + # Update gear quaternions in buffer + self.all_gear_quat_buffer[:, 0, :] = self.gear_assets["gear_small"].data.root_link_quat_w + self.all_gear_quat_buffer[:, 1, :] = self.gear_assets["gear_medium"].data.root_link_quat_w + self.all_gear_quat_buffer[:, 2, :] = self.gear_assets["gear_large"].data.root_link_quat_w + + # Select gear data using advanced indexing + gear_quat_world = self.all_gear_quat_buffer[self.env_indices, self.gear_type_indices] + + # Apply rotation offset + gear_quat_world = math_utils.quat_mul(gear_quat_world, self.grasp_rot_offset_tensor) + + # Compute relative orientation: q_rel = q_gear * q_eef^-1 + eef_quat_inv = math_utils.quat_conjugate(eef_quat_world) + relative_quat = math_utils.quat_mul(gear_quat_world, eef_quat_inv) + + # Convert relative quaternion to Euler angles + roll, pitch, yaw = math_utils.euler_xyz_from_quat(relative_quat) + + # Check if any angle exceeds its threshold + self.reset_flags[:] = ( + (torch.abs(roll) > roll_threshold_rad) + | (torch.abs(pitch) > pitch_threshold_rad) + | (torch.abs(yaw) > yaw_threshold_rad) + ) + + return self.reset_flags diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/__init__.py new file mode 100644 index 000000000000..69fa0010cd00 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""end-effector pose tracking tasks that have been deployed on a real robot.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/__init__.py new file mode 100644 index 000000000000..05f9849b508a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration package for manipulation tasks that have been deployed on a real robot.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/__init__.py new file mode 100644 index 000000000000..b1626c782186 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/__init__.py @@ -0,0 +1,42 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Deploy-Reach-UR10e-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10eReachEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:URReachPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Deploy-Reach-UR10e-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10eReachEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:URReachPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Deploy-Reach-UR10e-ROS-Inference-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.ros_inference_env_cfg:UR10eReachROSInferenceEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:URReachPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/__init__.py new file mode 100644 index 000000000000..cf59b16a1e2e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..02af65eec9a0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,38 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class URReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 512 + max_iterations = 1500 + save_interval = 50 + experiment_name = "reach_ur10e" + empirical_normalization = True + obs_groups = {"policy": ["policy"], "critic": ["policy"]} + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/joint_pos_env_cfg.py new file mode 100644 index 000000000000..4abcf4369764 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/joint_pos_env_cfg.py @@ -0,0 +1,112 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +from isaaclab.managers import SceneEntityCfg +from isaaclab.markers.config import FRAME_MARKER_CFG +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.deploy.mdp as mdp +from isaaclab_tasks.manager_based.manipulation.deploy.reach.reach_env_cfg import ReachEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets import UR10e_CFG # isort: skip + + +## +# Environment configuration +## + + +@configclass +class UR10eReachEnvCfg(ReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.events.robot_joint_stiffness_and_damping.params["asset_cfg"].joint_names = [ + "shoulder_.*", + "elbow_.*", + "wrist_.*", + ] + self.events.joint_friction.params["asset_cfg"].joint_names = ["shoulder_.*", "elbow_.*", "wrist_.*"] + + # switch robot to ur10e + self.scene.robot = UR10e_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # The real UR10e robots polyscore software uses the "base" frame for reference + # But the USD model and UR10e ROS interface uses the "base_link" frame + # We are training this policy to track the end-effector pose in the "base" frame + # The base frame is 180 offset from the base_link frame + # And hence the source_frame_offset is set to 180 degrees around the z-axis + self.rewards.end_effector_keypoint_tracking.params["asset_cfg"] = SceneEntityCfg("ee_frame_wrt_base_frame") + self.rewards.end_effector_keypoint_tracking_exp.params["asset_cfg"] = SceneEntityCfg("ee_frame_wrt_base_frame") + self.scene.ee_frame_wrt_base_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + visualizer_cfg=FRAME_MARKER_CFG.replace(prim_path="/Visuals/FrameTransformer"), + source_frame_offset=OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.0, 0.0, 0.0, 1.0)), + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/wrist_3_link", + name="end_effector", + ), + ], + ) + # Disable visualization for the goal pose because the commands are generated wrt to the base frame + # But the visualization will visualizing it wrt to the base_link frame + self.commands.ee_pose.debug_vis = False + + # Incremental joint position action configuration + self.actions.arm_action = mdp.RelativeJointPositionActionCfg( + asset_name="robot", joint_names=[".*"], scale=0.0625, use_zero_offset=True + ) + # override command generator body + # end-effector is along x-direction + self.target_pos_centre = (0.8875, -0.225, 0.2) + self.target_pos_range = (0.25, 0.125, 0.1) + self.commands.ee_pose.body_name = "wrist_3_link" + self.commands.ee_pose.ranges.pos_x = ( + self.target_pos_centre[0] - self.target_pos_range[0], + self.target_pos_centre[0] + self.target_pos_range[0], + ) + self.commands.ee_pose.ranges.pos_y = ( + self.target_pos_centre[1] - self.target_pos_range[1], + self.target_pos_centre[1] + self.target_pos_range[1], + ) + self.commands.ee_pose.ranges.pos_z = ( + self.target_pos_centre[2] - self.target_pos_range[2], + self.target_pos_centre[2] + self.target_pos_range[2], + ) + + self.target_rot_centre = (math.pi, 0.0, -math.pi / 2) # end-effector facing down + self.target_rot_range = (math.pi / 6, math.pi / 6, math.pi * 2 / 3) + self.commands.ee_pose.ranges.roll = ( + self.target_rot_centre[0] - self.target_rot_range[0], + self.target_rot_centre[0] + self.target_rot_range[0], + ) + self.commands.ee_pose.ranges.pitch = ( + self.target_rot_centre[1] - self.target_rot_range[1], + self.target_rot_centre[1] + self.target_rot_range[1], + ) + self.commands.ee_pose.ranges.yaw = ( + self.target_rot_centre[2] - self.target_rot_range[2], + self.target_rot_centre[2] + self.target_rot_range[2], + ) + + +@configclass +class UR10eReachEnvCfg_PLAY(UR10eReachEnvCfg): + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/ros_inference_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/ros_inference_env_cfg.py new file mode 100644 index 000000000000..2324b32b001c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/ros_inference_env_cfg.py @@ -0,0 +1,46 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .joint_pos_env_cfg import UR10eReachEnvCfg + + +@configclass +class UR10eReachROSInferenceEnvCfg(UR10eReachEnvCfg): + """Exposing variables for ROS inferences""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Variables used by Isaac Manipuulator for on robot inference + # TODO: @ashwinvk: Remove these from env cfg once the generic inference node has been implemented + self.obs_order = ["arm_dof_pos", "arm_dof_vel", "target_pos", "target_quat"] + self.policy_action_space = "joint" + self.arm_joint_names = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ] + self.policy_action_space = "joint" + self.action_space = 6 + self.state_space = 19 + self.observation_space = 19 + + # Set joint_action_scale from the existing arm_action.scale + self.joint_action_scale = self.actions.arm_action.scale + + self.action_scale_joint_space = [ + self.joint_action_scale, + self.joint_action_scale, + self.joint_action_scale, + self.joint_action_scale, + self.joint_action_scale, + self.joint_action_scale, + ] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/reach_env_cfg.py new file mode 100644 index 000000000000..90b65a0f96c2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/reach_env_cfg.py @@ -0,0 +1,215 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 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.deploy.mdp as mdp + +## +# Scene definition +## + + +@configclass +class SceneCfg(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)), + ) + + # 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), + ) + + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.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, 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)) + pose_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose"}) + + 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_offset, + mode="reset", + params={ + "position_range": (-0.125, 0.125), + "velocity_range": (0.0, 0.0), + }, + ) + + robot_joint_stiffness_and_damping = EventTerm( + func=mdp.randomize_actuator_gains, + min_step_count_between_reset=200, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot"), + "stiffness_distribution_params": (0.9, 1.1), + "damping_distribution_params": (0.75, 1.5), + "operation": "scale", + "distribution": "uniform", + }, + ) + + joint_friction = EventTerm( + func=mdp.randomize_joint_parameters, + min_step_count_between_reset=200, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot"), + "friction_distribution_params": (0.0, 0.1), + "operation": "add", + "distribution": "uniform", + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + end_effector_keypoint_tracking = RewTerm( + func=mdp.keypoint_command_error, + weight=-1.5, + params={ + "asset_cfg": SceneEntityCfg("ee_frame"), + "command_name": "ee_pose", + "keypoint_scale": 0.45, + }, + ) + end_effector_keypoint_tracking_exp = RewTerm( + func=mdp.keypoint_command_error_exp, + weight=1.5, + params={ + "asset_cfg": SceneEntityCfg("ee_frame"), + "command_name": "ee_pose", + "kp_exp_coeffs": [(50, 0.0001), (300, 0.0001), (5000, 0.0001)], + "kp_use_sum_of_exps": False, + "keypoint_scale": 0.45, + }, + ) + + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.005) + action = RewTerm(func=mdp.action_l2, weight=-0.005) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +## +# Environment configuration +## + + +@configclass +class ReachEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the end-effector pose tracking environment that has been deployed on a real robot.""" + + # Scene settings + scene: SceneCfg = SceneCfg(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() + + 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 / 120.0 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 000000000000..cb7aaceec9c2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/__init__.py @@ -0,0 +1,26 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 000000000000..de3aca917f75 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/adr_curriculum.py @@ -0,0 +1,122 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 000000000000..d6250c019573 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 000000000000..8c9e9617fce3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/__init__.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +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/dexsuite/config/kuka_allegro/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause 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 000000000000..3a9e96eaeb05 --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..9bc92bd8f69b --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 000000000000..6b7f82fde06e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 isaaclab_assets.robots import KUKA_ALLEGRO_CFG + +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 000000000000..9ee00105e57a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py @@ -0,0 +1,467 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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): + """Observations for perception group.""" + + 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 000000000000..a6537b1a5e19 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 000000000000..83f55101029b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..ade464360a07 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/pose_commands.py @@ -0,0 +1,180 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Sub-module containing command generators for pose tracking.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +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 000000000000..e3c83882a3f5 --- /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-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from 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 000000000000..148046f012c7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/curriculums.py @@ -0,0 +1,114 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +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 000000000000..604c74320b01 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/observations.py @@ -0,0 +1,198 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +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 000000000000..a6ddab0f9081 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py @@ -0,0 +1,127 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +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 000000000000..91bf2d0e3aaf --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/terminations.py @@ -0,0 +1,50 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 + +from typing import TYPE_CHECKING + +import torch + +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 000000000000..409abc5931d8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py @@ -0,0 +1,249 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import hashlib +import logging + +import numpy as np +import torch +import trimesh +from trimesh.sample import sample_surface + +from pxr import UsdGeom + +import isaaclab.sim as sim_utils + +# ---- 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() + # Obtain stage handle + stage = sim_utils.get_current_stage() + + for i in range(num_envs): + # Resolve prim path + obj_path = prim_path.replace(".*", str(i)) + + # Gather prims + prims = sim_utils.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 = stage.GetPrimAtPath(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/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/__init__.py index ec0d90ba1efd..5dfbb3b40111 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/__init__.py index f757d501acc1..c586540a04d2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/__init__.py index e63efd67eac3..9f53828ec4b6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rl_games_ppo_cfg.yaml index 24b4540060e5..2fa70902ab61 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py index 4a25aea7fc4c..b1d3d4be1756 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class AllegroCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 5000 save_interval = 50 experiment_name = "allegro_cube" - empirical_normalization = True 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", 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 a7763370650e..f61e7f50132d 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -23,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/inhand/config/allegro_hand/allegro_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/allegro_env_cfg.py index d55e595ea377..7223ce0234fe 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/allegro_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/allegro_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -21,6 +21,8 @@ def __post_init__(self): # switch robot to allegro hand self.scene.robot = ALLEGRO_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # enable clone in fabric + self.scene.clone_in_fabric = True @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py index ca9980142d16..71594ae210d8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/__init__.py index 2dbbc78d9565..0fafe4500367 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/__init__.py index d7f54652a30a..ab3a9cf3e11c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/commands_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/commands_cfg.py index 0969448bfe59..8c020137c0c9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/commands_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/commands_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py index 72cad391f186..3f116a48c497 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,10 +7,11 @@ from __future__ import annotations -import torch from collections.abc import Sequence from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils from isaaclab.assets import RigidObject from isaaclab.managers import CommandTerm diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/events.py index 8c52d80951d4..dad2e88107e8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/events.py @@ -1,16 +1,16 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Functions specific to the in-hand dexterous manipulation environments.""" - from __future__ import annotations -import torch from typing import TYPE_CHECKING, Literal +import torch + from isaaclab.assets import Articulation from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg from isaaclab.utils.math import sample_uniform diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/observations.py index eca47de0bbed..e90597925634 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/observations.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Functions specific to the in-hand dexterous manipulation environments.""" -import torch from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils from isaaclab.assets import RigidObject from isaaclab.envs import ManagerBasedRLEnv diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/rewards.py index 012e09546e31..f928b92fb367 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Functions specific to the in-hand dexterous manipulation environments.""" -import torch from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils from isaaclab.assets import RigidObject from isaaclab.envs import ManagerBasedRLEnv diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/terminations.py index f50ffec0ccd3..1d4f36f1e62b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/mdp/terminations.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Functions specific to the in-hand dexterous manipulation environments.""" -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.envs import ManagerBasedRLEnv from isaaclab.managers import SceneEntityCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/__init__.py index fd33aacd3a5b..1211ee056649 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/__init__.py index fd33aacd3a5b..1211ee056649 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/__init__.py index 6335e2e1ccbe..d72fd6ebb596 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/__init__.py @@ -1,9 +1,8 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import gymnasium as gym -import os from . import agents @@ -72,7 +71,7 @@ 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"), + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc.json", }, disable_env_checker=True, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rl_games_ppo_cfg.yaml index 6ed68d88912c..77360c1b91bb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py index e3fb13e1d1f7..7a94614d8c97 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class LiftCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "franka_lift" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[256, 128, 64], critic_hidden_dims=[256, 128, 64], activation="elu", diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/sb3_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/sb3_ppo_cfg.yaml index 6d6f15781acc..593de544a83a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/sb3_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/sb3_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 seed: 42 @@ -14,10 +19,11 @@ 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]) - )" +policy_kwargs: + activation_fn: 'nn.ELU' + net_arch: + pi: [256, 128, 64] + vf: [256, 128, 64] target_kl: 0.01 max_grad_norm: 1.0 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 94337d46bac6..3f39d0c4afc4 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -23,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/lift/config/franka/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_abs_env_cfg.py index 073f079f326c..9b9441a22341 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_abs_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_abs_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -80,7 +80,7 @@ def __post_init__(self): ) # Make the end effector less stiff to not hurt the poor teddy bear - self.scene.robot.actuators["panda_hand"].effort_limit = 50.0 + 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 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_rel_env_cfg.py index f5ccdd79fd45..5e5c95e7d472 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_rel_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/joint_pos_env_cfg.py index e9f0ab0d9080..5c5754c53e43 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/joint_pos_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/__init__.py new file mode 100644 index 000000000000..40ce61a45f83 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/__init__.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Lift-Cube-OpenArm-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmCubeLiftEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:OpenArmLiftCubePPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Lift-Cube-OpenArm-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmCubeLiftEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:OpenArmLiftCubePPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 000000000000..3363b1ea7348 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,85 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +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: openarm_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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..f079552f1f4a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class OpenArmLiftCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 2000 + save_interval = 50 + experiment_name = "openarm_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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/joint_pos_env_cfg.py new file mode 100644 index 000000000000..b5f29f1fc32f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/joint_pos_env_cfg.py @@ -0,0 +1,101 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +import math + +from isaaclab.assets import RigidObjectCfg +from isaaclab.sensors import FrameTransformerCfg +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.config.openarm.lift_openarm_env_cfg import LiftEnvCfg + +from isaaclab_assets.robots.openarm import OPENARM_UNI_CFG + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip + + +@configclass +class OpenArmCubeLiftEnvCfg(LiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set OpenArm as robot + self.scene.robot = OPENARM_UNI_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (OpenArm) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "openarm_joint.*", + ], + scale=0.5, + use_default_offset=True, + ) + + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["openarm_finger_joint.*"], + open_command_expr={"openarm_finger_joint.*": 0.044}, + close_command_expr={"openarm_finger_joint.*": 0.0}, + ) + + # Set the body name for the end effector + self.commands.object_pose.body_name = "openarm_hand" + self.commands.object_pose.ranges.pitch = (math.pi / 2, math.pi / 2) + + # Set Cube as object + self.scene.object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.4, 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/openarm_link0", + debug_vis=False, + visualizer_cfg=marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_ee_tcp", + name="end_effector", + ), + ], + ) + + +@configclass +class OpenArmCubeLiftEnvCfg_PLAY(OpenArmCubeLiftEnvCfg): + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py new file mode 100644 index 000000000000..491b713c14f9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py @@ -0,0 +1,240 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +We modified parts of the environment, such as the target's position and orientation, +as well as certain object properties, to better suit the smaller robot. +""" + +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.2, 0.4), + pos_y=(-0.2, 0.2), + pos_z=(0.15, 0.4), + 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, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["openarm_joint.*", "openarm_finger_joint.*"])}, + ) + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["openarm_joint.*", "openarm_finger_joint.*"])}, + ) + 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.1) + + 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", joint_names=["openarm_joint.*", "openarm_finger_joint.*"])}, + ) + + +@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.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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py index fbc0698b11ec..272661bda61d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/__init__.py index f53d3692aa6d..f3dd0fecdf8e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/observations.py index f41de8d9b905..8654933a9aae 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/observations.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import RigidObject from isaaclab.managers import SceneEntityCfg from isaaclab.utils.math import subtract_frame_transforms @@ -25,7 +26,5 @@ def object_position_in_robot_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_state_w[:, :3], robot.data.root_state_w[:, 3:7], object_pos_w - ) + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/rewards.py index 232156a1b6d0..34e60773a068 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import RigidObject from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import FrameTransformer @@ -60,8 +61,8 @@ def object_goal_distance( 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_state_w[:, :3], robot.data.root_state_w[:, 3:7], des_pos_b) + 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) + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/terminations.py index 8ce9f6a7d93b..68fe0e011b85 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/mdp/terminations.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,9 +11,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import RigidObject from isaaclab.managers import SceneEntityCfg from isaaclab.utils.math import combine_frame_transforms @@ -45,7 +46,7 @@ def object_reached_goal( 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_state_w[:, :3], robot.data.root_state_w[:, 3:7], des_pos_b) + 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) 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 new file mode 100644 index 000000000000..7f2bd7d0f707 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py @@ -0,0 +1,58 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +gym.register( + id="Isaac-PickPlace-GR1T2-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.pickplace_gr1t2_env_cfg:PickPlaceGR1T2EnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-NutPour-GR1T2-Pink-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.nutpour_gr1t2_pink_ik_env_cfg:NutPourGR1T2PinkIKEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_image_nut_pouring.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-ExhaustPipe-GR1T2-Pink-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.exhaustpipe_gr1t2_pink_ik_env_cfg:ExhaustPipeGR1T2PinkIKEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_image_exhaust_pipe.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.pickplace_gr1t2_waist_enabled_env_cfg:PickPlaceGR1T2WaistEnabledEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-PickPlace-G1-InspireFTP-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.pickplace_unitree_g1_inspire_hand_env_cfg:PickPlaceG1InspireFTPEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_image_exhaust_pipe.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_image_exhaust_pipe.json new file mode 100644 index 000000000000..5af2a9f4a4f8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_image_exhaust_pipe.json @@ -0,0 +1,220 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_image_gr1_exhaust_pipe", + "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": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state" + ], + "rgb": [ + "robot_pov_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": 144, + "crop_width": 236, + "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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_image_nut_pouring.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_image_nut_pouring.json new file mode 100644 index 000000000000..dbe527d72dde --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_image_nut_pouring.json @@ -0,0 +1,220 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_image_gr1_nut_pouring", + "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": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state" + ], + "rgb": [ + "robot_pov_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": 144, + "crop_width": 236, + "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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json new file mode 100644 index 000000000000..d2e0f8fc6d94 --- /dev/null +++ b/source/isaaclab_tasks/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/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 new file mode 100644 index 000000000000..6e14d2e1fdd2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -0,0 +1,336 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import tempfile +from dataclasses import MISSING + +import torch + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg, SceneEntityCfg +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 TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg + +# from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the GR1T2 Exhaust Pipe Base Scene.""" + + # Table + table = AssetBaseCfg( + prim_path="/World/envs/env_.*/Table", + 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"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/table.usd", + scale=(1.0, 1.0, 1.3), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + blue_exhaust_pipe = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlueExhaustPipe", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.04904, 0.31, 1.2590], rot=[0, 0, 1.0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/blue_exhaust_pipe.usd", + scale=(0.5, 0.5, 1.5), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + blue_sorting_bin = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlueSortingBin", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.16605, 0.39, 0.98634], rot=[1.0, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/blue_sorting_bin.usd", + scale=(1.0, 1.7, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + black_sorting_bin = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlackSortingBin", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.40132, 0.39, 0.98634], rot=[1.0, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/black_sorting_bin.usd", + scale=(1.0, 1.7, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # 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.10933163, + "left_shoulder_roll_joint": 0.43292055, + "left_shoulder_yaw_joint": -0.15983289, + "left_elbow_pitch_joint": -1.48233023, + "left_wrist_yaw_joint": 0.2359135, + "left_wrist_roll_joint": 0.26184522, + "left_wrist_pitch_joint": 0.00830735, + # right hand + "R_index_intermediate_joint": 0.0, + "R_index_proximal_joint": 0.0, + "R_middle_intermediate_joint": 0.0, + "R_middle_proximal_joint": 0.0, + "R_pinky_intermediate_joint": 0.0, + "R_pinky_proximal_joint": 0.0, + "R_ring_intermediate_joint": 0.0, + "R_ring_proximal_joint": 0.0, + "R_thumb_distal_joint": 0.0, + "R_thumb_proximal_pitch_joint": 0.0, + "R_thumb_proximal_yaw_joint": -1.57, + # left hand + "L_index_intermediate_joint": 0.0, + "L_index_proximal_joint": 0.0, + "L_middle_intermediate_joint": 0.0, + "L_middle_proximal_joint": 0.0, + "L_pinky_intermediate_joint": 0.0, + "L_pinky_proximal_joint": 0.0, + "L_ring_intermediate_joint": 0.0, + "L_ring_proximal_joint": 0.0, + "L_thumb_distal_joint": 0.0, + "L_thumb_proximal_pitch_joint": 0.0, + "L_thumb_proximal_yaw_joint": -1.57, + # -- + "head_.*": 0.0, + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Set table view camera + robot_pov_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/RobotPOVCam", + update_period=0.0, + height=160, + width=256, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(focal_length=18.15, clipping_range=(0.1, 2)), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.12, 1.85418), rot=(-0.17246, 0.98502, 0.0, 0.0), convention="ros"), + ) + + # 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.""" + + gr1_action: ActionTermCfg = 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) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + 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, + params={"sensor_cfg": SceneEntityCfg("robot_pov_cam"), "data_type": "rgb", "normalize": False}, + ) + + 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) + + blue_exhaust_pipe_dropped = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("blue_exhaust_pipe")}, + ) + + success = DoneTerm(func=mdp.task_done_exhaust_pipe) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_blue_exhaust_pipe = 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("blue_exhaust_pipe"), + }, + ) + + +@configclass +class ExhaustPipeGR1T2BaseEnvCfg(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), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # 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/right hand joint pos (22)] + idle_action = torch.tensor( + [ + [ + -0.2909, + 0.2778, + 1.1247, + 0.5253, + 0.5747, + -0.4160, + 0.4699, + 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 / 100 + self.sim.render_interval = 2 + + # Set settings for camera rendering + self.num_rerenders_on_reset = 3 + self.sim.render.antialiasing_mode = "DLAA" # Use DLAA for higher quality rendering + + # List of image observations in policy observations + self.image_obs_list = ["robot_pov_cam"] 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 new file mode 100644 index 000000000000..66ebfcad8a18 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -0,0 +1,155 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from pink.tasks import DampingTask, FrameTask + +import carb + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.pick_place.exhaustpipe_gr1t2_base_env_cfg import ( + ExhaustPipeGR1T2BaseEnvCfg, +) + + +@configclass +class ExhaustPipeGR1T2PinkIKEnvCfg(ExhaustPipeGR1T2BaseEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.actions.gr1_action = 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", + ], + 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", + ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_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=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=22, + show_ik_warnings=False, + # Determines whether Pink IK solver will fail due to a joint limit violation + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "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 + 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 + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.2, + lm_damping=1, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + ) + # 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.gr1_action.controller.urdf_path = temp_urdf_output_path + self.actions.gr1_action.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # 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.gr1_action.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.py new file mode 100644 index 000000000000..555bfb7cbe8f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 .pick_place_events import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 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 new file mode 100644 index 000000000000..01e52e73f242 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py @@ -0,0 +1,86 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_obs( + env: ManagerBasedRLEnv, + left_eef_link_name: str, + right_eef_link_name: str, +) -> 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_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 + + 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_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(link_name) + left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins + + return left_eef_pos + + +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(link_name) + left_eef_quat = body_quat_w[:, left_eef_idx] + + return left_eef_quat + + +def get_robot_joint_state( + env: ManagerBasedRLEnv, + joint_names: list[str], +) -> torch.Tensor: + # 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 robot_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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/pick_place_events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/pick_place_events.py new file mode 100644 index 000000000000..ca1fd940fea8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/pick_place_events.py @@ -0,0 +1,96 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def reset_object_poses_nut_pour( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict[str, tuple[float, float]], + sorting_beaker_cfg: SceneEntityCfg = SceneEntityCfg("sorting_beaker"), + factory_nut_cfg: SceneEntityCfg = SceneEntityCfg("factory_nut"), + sorting_bowl_cfg: SceneEntityCfg = SceneEntityCfg("sorting_bowl"), + sorting_scale_cfg: SceneEntityCfg = SceneEntityCfg("sorting_scale"), +): + """Reset the asset root states to a random position and orientation uniformly within the given ranges. + + Args: + env: The RL environment instance. + env_ids: The environment IDs to reset the object poses for. + sorting_beaker_cfg: The configuration for the sorting beaker asset. + factory_nut_cfg: The configuration for the factory nut asset. + sorting_bowl_cfg: The configuration for the sorting bowl asset. + sorting_scale_cfg: The configuration for the sorting scale asset. + pose_range: The dictionary of pose ranges for the objects. Keys are + ``x``, ``y``, ``z``, ``roll``, ``pitch``, and ``yaw``. + """ + # extract the used quantities (to enable type-hinting) + sorting_beaker = env.scene[sorting_beaker_cfg.name] + factory_nut = env.scene[factory_nut_cfg.name] + sorting_bowl = env.scene[sorting_bowl_cfg.name] + sorting_scale = env.scene[sorting_scale_cfg.name] + + # get default root state + sorting_beaker_root_states = sorting_beaker.data.default_root_state[env_ids].clone() + factory_nut_root_states = factory_nut.data.default_root_state[env_ids].clone() + sorting_bowl_root_states = sorting_bowl.data.default_root_state[env_ids].clone() + sorting_scale_root_states = sorting_scale.data.default_root_state[env_ids].clone() + + # get pose ranges + 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=sorting_beaker.device) + + # randomize sorting beaker and factory nut together + rand_samples = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=sorting_beaker.device + ) + orientations_delta = math_utils.quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5]) + positions_sorting_beaker = ( + sorting_beaker_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + ) + positions_factory_nut = factory_nut_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + orientations_sorting_beaker = math_utils.quat_mul(sorting_beaker_root_states[:, 3:7], orientations_delta) + orientations_factory_nut = math_utils.quat_mul(factory_nut_root_states[:, 3:7], orientations_delta) + + # randomize sorting bowl + rand_samples = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=sorting_beaker.device + ) + orientations_delta = math_utils.quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5]) + positions_sorting_bowl = sorting_bowl_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + orientations_sorting_bowl = math_utils.quat_mul(sorting_bowl_root_states[:, 3:7], orientations_delta) + + # randomize scorting scale + rand_samples = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=sorting_beaker.device + ) + orientations_delta = math_utils.quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5]) + positions_sorting_scale = sorting_scale_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + orientations_sorting_scale = math_utils.quat_mul(sorting_scale_root_states[:, 3:7], orientations_delta) + + # set into the physics simulation + sorting_beaker.write_root_pose_to_sim( + torch.cat([positions_sorting_beaker, orientations_sorting_beaker], dim=-1), env_ids=env_ids + ) + factory_nut.write_root_pose_to_sim( + torch.cat([positions_factory_nut, orientations_factory_nut], dim=-1), env_ids=env_ids + ) + sorting_bowl.write_root_pose_to_sim( + torch.cat([positions_sorting_bowl, orientations_sorting_bowl], dim=-1), env_ids=env_ids + ) + sorting_scale.write_root_pose_to_sim( + torch.cat([positions_sorting_scale, orientations_sorting_scale], dim=-1), env_ids=env_ids + ) 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 new file mode 100644 index 000000000000..6252b9c67a21 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py @@ -0,0 +1,222 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +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, + max_x: float = 0.85, + min_y: float = 0.35, + max_y: float = 0.60, + max_height: float = 1.10, + 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. + max_height: Maximum 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. + """ + 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] + + # Extract wheel position relative to environment origin + object_x = object.data.root_pos_w[:, 0] - env.scene.env_origins[:, 0] + object_y = object.data.root_pos_w[:, 1] - env.scene.env_origins[:, 1] + object_height = object.data.root_pos_w[:, 2] - env.scene.env_origins[:, 2] + object_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(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 + done = object_x < max_x + done = torch.logical_and(done, object_x > min_x) + done = torch.logical_and(done, object_y < max_y) + done = torch.logical_and(done, object_y > min_y) + done = torch.logical_and(done, object_height < max_height) + done = torch.logical_and(done, right_wrist_x < right_wrist_max_x) + done = torch.logical_and(done, object_vel[:, 0] < min_vel) + done = torch.logical_and(done, object_vel[:, 1] < min_vel) + done = torch.logical_and(done, object_vel[:, 2] < min_vel) + + return done + + +def task_done_nut_pour( + env: ManagerBasedRLEnv, + sorting_scale_cfg: SceneEntityCfg = SceneEntityCfg("sorting_scale"), + sorting_bowl_cfg: SceneEntityCfg = SceneEntityCfg("sorting_bowl"), + sorting_beaker_cfg: SceneEntityCfg = SceneEntityCfg("sorting_beaker"), + factory_nut_cfg: SceneEntityCfg = SceneEntityCfg("factory_nut"), + sorting_bin_cfg: SceneEntityCfg = SceneEntityCfg("black_sorting_bin"), + max_bowl_to_scale_x: float = 0.055, + max_bowl_to_scale_y: float = 0.055, + max_bowl_to_scale_z: float = 0.025, + max_nut_to_bowl_x: float = 0.050, + max_nut_to_bowl_y: float = 0.050, + max_nut_to_bowl_z: float = 0.019, + max_beaker_to_bin_x: float = 0.08, + max_beaker_to_bin_y: float = 0.12, + max_beaker_to_bin_z: float = 0.07, +) -> torch.Tensor: + """Determine if the nut pouring task is complete. + + This function checks whether all success conditions for the task have been met: + 1. The factory nut is in the sorting bowl + 2. The sorting beaker is in the sorting bin + 3. The sorting bowl is placed on the sorting scale + + Args: + env: The RL environment instance. + sorting_scale_cfg: Configuration for the sorting scale entity. + sorting_bowl_cfg: Configuration for the sorting bowl entity. + sorting_beaker_cfg: Configuration for the sorting beaker entity. + factory_nut_cfg: Configuration for the factory nut entity. + sorting_bin_cfg: Configuration for the sorting bin entity. + max_bowl_to_scale_x: Maximum x position of the sorting bowl relative to the sorting scale for task completion. + max_bowl_to_scale_y: Maximum y position of the sorting bowl relative to the sorting scale for task completion. + max_bowl_to_scale_z: Maximum z position of the sorting bowl relative to the sorting scale for task completion. + max_nut_to_bowl_x: Maximum x position of the factory nut relative to the sorting bowl for task completion. + max_nut_to_bowl_y: Maximum y position of the factory nut relative to the sorting bowl for task completion. + max_nut_to_bowl_z: Maximum z position of the factory nut relative to the sorting bowl for task completion. + max_beaker_to_bin_x: Maximum x position of the sorting beaker relative to the sorting bin for task completion. + max_beaker_to_bin_y: Maximum y position of the sorting beaker relative to the sorting bin for task completion. + max_beaker_to_bin_z: Maximum z position of the sorting beaker relative to the sorting bin for task completion. + + Returns: + Boolean tensor indicating which environments have completed the task. + """ + # Get object entities from the scene + sorting_scale: RigidObject = env.scene[sorting_scale_cfg.name] + sorting_bowl: RigidObject = env.scene[sorting_bowl_cfg.name] + factory_nut: RigidObject = env.scene[factory_nut_cfg.name] + sorting_beaker: RigidObject = env.scene[sorting_beaker_cfg.name] + sorting_bin: RigidObject = env.scene[sorting_bin_cfg.name] + + # Get positions relative to environment origin + scale_pos = sorting_scale.data.root_pos_w - env.scene.env_origins + bowl_pos = sorting_bowl.data.root_pos_w - env.scene.env_origins + sorting_beaker_pos = sorting_beaker.data.root_pos_w - env.scene.env_origins + nut_pos = factory_nut.data.root_pos_w - env.scene.env_origins + bin_pos = sorting_bin.data.root_pos_w - env.scene.env_origins + + # nut to bowl + nut_to_bowl_x = torch.abs(nut_pos[:, 0] - bowl_pos[:, 0]) + nut_to_bowl_y = torch.abs(nut_pos[:, 1] - bowl_pos[:, 1]) + nut_to_bowl_z = nut_pos[:, 2] - bowl_pos[:, 2] + + # bowl to scale + bowl_to_scale_x = torch.abs(bowl_pos[:, 0] - scale_pos[:, 0]) + bowl_to_scale_y = torch.abs(bowl_pos[:, 1] - scale_pos[:, 1]) + bowl_to_scale_z = bowl_pos[:, 2] - scale_pos[:, 2] + + # beaker to bin + beaker_to_bin_x = torch.abs(sorting_beaker_pos[:, 0] - bin_pos[:, 0]) + beaker_to_bin_y = torch.abs(sorting_beaker_pos[:, 1] - bin_pos[:, 1]) + beaker_to_bin_z = sorting_beaker_pos[:, 2] - bin_pos[:, 2] + + done = nut_to_bowl_x < max_nut_to_bowl_x + done = torch.logical_and(done, nut_to_bowl_y < max_nut_to_bowl_y) + done = torch.logical_and(done, nut_to_bowl_z < max_nut_to_bowl_z) + done = torch.logical_and(done, bowl_to_scale_x < max_bowl_to_scale_x) + done = torch.logical_and(done, bowl_to_scale_y < max_bowl_to_scale_y) + done = torch.logical_and(done, bowl_to_scale_z < max_bowl_to_scale_z) + done = torch.logical_and(done, beaker_to_bin_x < max_beaker_to_bin_x) + done = torch.logical_and(done, beaker_to_bin_y < max_beaker_to_bin_y) + done = torch.logical_and(done, beaker_to_bin_z < max_beaker_to_bin_z) + + return done + + +def task_done_exhaust_pipe( + env: ManagerBasedRLEnv, + blue_exhaust_pipe_cfg: SceneEntityCfg = SceneEntityCfg("blue_exhaust_pipe"), + blue_sorting_bin_cfg: SceneEntityCfg = SceneEntityCfg("blue_sorting_bin"), + max_blue_exhaust_to_bin_x: float = 0.085, + max_blue_exhaust_to_bin_y: float = 0.200, + min_blue_exhaust_to_bin_y: float = -0.090, + max_blue_exhaust_to_bin_z: float = 0.070, +) -> torch.Tensor: + """Determine if the exhaust pipe task is complete. + + This function checks whether all success conditions for the task have been met: + 1. The blue exhaust pipe is placed in the correct position + + Args: + env: The RL environment instance. + blue_exhaust_pipe_cfg: Configuration for the blue exhaust pipe entity. + blue_sorting_bin_cfg: Configuration for the blue sorting bin entity. + max_blue_exhaust_to_bin_x: Maximum x position of the blue exhaust pipe + relative to the blue sorting bin for task completion. + max_blue_exhaust_to_bin_y: Maximum y position of the blue exhaust pipe + relative to the blue sorting bin for task completion. + max_blue_exhaust_to_bin_z: Maximum z position of the blue exhaust pipe + relative to the blue sorting bin for task completion. + + Returns: + Boolean tensor indicating which environments have completed the task. + """ + # Get object entities from the scene + blue_exhaust_pipe: RigidObject = env.scene[blue_exhaust_pipe_cfg.name] + blue_sorting_bin: RigidObject = env.scene[blue_sorting_bin_cfg.name] + + # Get positions relative to environment origin + blue_exhaust_pipe_pos = blue_exhaust_pipe.data.root_pos_w - env.scene.env_origins + blue_sorting_bin_pos = blue_sorting_bin.data.root_pos_w - env.scene.env_origins + + # blue exhaust to bin + blue_exhaust_to_bin_x = torch.abs(blue_exhaust_pipe_pos[:, 0] - blue_sorting_bin_pos[:, 0]) + blue_exhaust_to_bin_y = blue_exhaust_pipe_pos[:, 1] - blue_sorting_bin_pos[:, 1] + blue_exhaust_to_bin_z = blue_exhaust_pipe_pos[:, 2] - blue_sorting_bin_pos[:, 2] + + done = blue_exhaust_to_bin_x < max_blue_exhaust_to_bin_x + done = torch.logical_and(done, blue_exhaust_to_bin_y < max_blue_exhaust_to_bin_y) + done = torch.logical_and(done, blue_exhaust_to_bin_y > min_blue_exhaust_to_bin_y) + done = torch.logical_and(done, blue_exhaust_to_bin_z < max_blue_exhaust_to_bin_z) + + return done 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 new file mode 100644 index 000000000000..01caf58a8af2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -0,0 +1,371 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import tempfile +from dataclasses import MISSING + +import torch + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg, SceneEntityCfg +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 TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg + +# from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the GR1T2 Nut Pour Base Scene.""" + + # Table + table = AssetBaseCfg( + prim_path="/World/envs/env_.*/Table", + 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"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/table.usd", + scale=(1.0, 1.0, 1.3), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + sorting_scale = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/SortingScale", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.22236, 0.56, 0.9859], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_scale.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + sorting_bowl = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/SortingBowl", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.02779, 0.43007, 0.9860], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_bowl_yellow.usd", + scale=(1.0, 1.0, 1.5), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005), + ), + ) + + sorting_beaker = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/SortingBeaker", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.13739, 0.45793, 0.9861], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_beaker_red.usd", + scale=(0.45, 0.45, 1.3), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + factory_nut = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/FactoryNut", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.13739, 0.45793, 0.9995], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/factory_m16_nut_green.usd", + scale=(0.5, 0.5, 0.5), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005), + ), + ) + + black_sorting_bin = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlackSortingBin", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.32688, 0.46793, 0.98634], rot=[1.0, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_bin_blue.usd", + scale=(0.75, 1.0, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + 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, + # right hand + "R_index_intermediate_joint": 0.0, + "R_index_proximal_joint": 0.0, + "R_middle_intermediate_joint": 0.0, + "R_middle_proximal_joint": 0.0, + "R_pinky_intermediate_joint": 0.0, + "R_pinky_proximal_joint": 0.0, + "R_ring_intermediate_joint": 0.0, + "R_ring_proximal_joint": 0.0, + "R_thumb_distal_joint": 0.0, + "R_thumb_proximal_pitch_joint": 0.0, + "R_thumb_proximal_yaw_joint": -1.57, + # left hand + "L_index_intermediate_joint": 0.0, + "L_index_proximal_joint": 0.0, + "L_middle_intermediate_joint": 0.0, + "L_middle_proximal_joint": 0.0, + "L_pinky_intermediate_joint": 0.0, + "L_pinky_proximal_joint": 0.0, + "L_ring_intermediate_joint": 0.0, + "L_ring_proximal_joint": 0.0, + "L_thumb_distal_joint": 0.0, + "L_thumb_proximal_pitch_joint": 0.0, + "L_thumb_proximal_yaw_joint": -1.57, + # -- + "head_.*": 0.0, + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Set table view camera + robot_pov_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/RobotPOVCam", + update_period=0.0, + height=160, + width=256, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(focal_length=18.15, clipping_range=(0.1, 2)), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.12, 1.67675), rot=(-0.19848, 0.9801, 0.0, 0.0), convention="ros"), + ) + + # 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.""" + + gr1_action: ActionTermCfg = 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) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + 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, + params={"sensor_cfg": SceneEntityCfg("robot_pov_cam"), "data_type": "rgb", "normalize": False}, + ) + + 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) + + sorting_bowl_dropped = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("sorting_bowl")} + ) + sorting_beaker_dropped = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("sorting_beaker")}, + ) + factory_nut_dropped = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("factory_nut")} + ) + + success = DoneTerm(func=mdp.task_done_nut_pour) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + set_factory_nut_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("factory_nut"), + "mass_distribution_params": (0.2, 0.2), + "operation": "abs", + }, + ) + + reset_object = EventTerm( + func=mdp.reset_object_poses_nut_pour, + mode="reset", + params={ + "pose_range": { + "x": [-0.01, 0.01], + "y": [-0.01, 0.01], + }, + }, + ) + + +@configclass +class NutPourGR1T2BaseEnvCfg(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), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # 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/right hand joint pos (22)] + 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 / 100 + self.sim.render_interval = 2 + + # Set settings for camera rendering + self.num_rerenders_on_reset = 3 + self.sim.render.antialiasing_mode = "DLAA" # Use DLAA for higher quality rendering + + # List of image observations in policy observations + self.image_obs_list = ["robot_pov_cam"] 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 new file mode 100644 index 000000000000..818fba1fc805 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -0,0 +1,153 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from pink.tasks import DampingTask, FrameTask + +import carb + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.pick_place.nutpour_gr1t2_base_env_cfg import NutPourGR1T2BaseEnvCfg + + +@configclass +class NutPourGR1T2PinkIKEnvCfg(NutPourGR1T2BaseEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.actions.gr1_action = 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", + ], + 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", + ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_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=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=22, + show_ik_warnings=False, + # Determines whether Pink IK solver will fail due to a joint limit violation + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "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 + 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 + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.2, + lm_damping=1, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + ) + # 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.gr1_action.controller.urdf_path = temp_urdf_output_path + self.actions.gr1_action.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # 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.gr1_action.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) 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 new file mode 100644 index 000000000000..ba6c5d385135 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -0,0 +1,420 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import tempfile + +import torch +from pink.tasks import DampingTask, FrameTask + +import carb + +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.fourier.gr1t2_retargeter import GR1T2RetargeterCfg +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, ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the GR1T2 Pick Place Base Scene.""" + + # 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.45, 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(), + ), + ) + + # Humanoid robot configured for pick-place manipulation tasks + robot: ArticulationCfg = GR1T2_HIGH_PD_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.""" + + upper_body_ik = 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", + ], + 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", + ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_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=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=22, + show_ik_warnings=False, + # Determines whether Pink IK solver will fail due to a joint limit violation + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + 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=12, # dampening for solver for step jumps + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + ) + + +@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_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"]}, + ) + + 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 + 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_hand_roll_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 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), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # 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 = 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.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={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # 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.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "manusvive": ManusViveCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + 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/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 new file mode 100644 index 000000000000..23ed8d984bcd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -0,0 +1,87 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import tempfile + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.utils import configclass + +from .pickplace_gr1t2_env_cfg import ActionsCfg, EventCfg, ObjectTableSceneCfg, ObservationsCfg, TerminationsCfg + + +@configclass +class PickPlaceGR1T2WaistEnabledEnvCfg(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), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + 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 + + # 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.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 + ) + + # Set the URDF and mesh paths for the IK controller + 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={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # 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.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/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 000000000000..85af79e7fb19 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py @@ -0,0 +1,415 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +import tempfile + +import torch +from pink.tasks import FrameTask + +import carb + +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): + """Configuration for the Unitree G1 Inspire Hand Pick Place Base Scene.""" + + # 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 000000000000..696e138c3e42 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 000000000000..696e138c3e42 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 000000000000..7b99bd4a5d34 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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 000000000000..ffe842b0202a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py @@ -0,0 +1,347 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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): + """Configuration for the Agibot Place Toy2Box RMP Rel Environment.""" + + 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 000000000000..14841036d66e --- /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,283 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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): + """Configuration for the Agibot Place Upright Mug RMP Rel Environment.""" + + 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 000000000000..41f76fcdb1bb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 000000000000..b0a9107beca1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/observations.py @@ -0,0 +1,119 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING, Literal + +import torch + +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 + else: + 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 000000000000..cf7248d29348 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/terminations.py @@ -0,0 +1,123 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg + +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/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/__init__.py index 6bda0ef8cbb6..be11b529e2c2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/__init__.py index f9fc872af39c..acf853fcf647 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/__init__.py index 8b39fbaf1233..47158f64a650 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rl_games_ppo_cfg.yaml index b568d40f26af..09e4f9d48b59 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py index ed378be2b674..ede70559fd56 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -15,10 +15,10 @@ class FrankaReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): save_interval = 50 experiment_name = "franka_reach" run_name = "" - resume = False - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[64, 64], critic_hidden_dims=[64, 64], activation="elu", 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 ef0232ccace8..986b35fff6b3 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ACTIONS @@ -23,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/franka/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py index e1c58d7b92d2..b090e568965e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py index b868ac25be74..024a42270d85 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py index 3f29bb009c82..a848ddb87667 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py index 2cc3f1002d91..e612439fda70 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/__init__.py new file mode 100644 index 000000000000..6cd284b1838e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/__init__.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Reach-OpenArm-Bi-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmReachEnvCfg", + "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:OpenArmReachPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Reach-OpenArm-Bi-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmReachEnvCfg_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:OpenArmReachPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 000000000000..71526744500e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,83 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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: openarm_bi_reach + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..d1dd736a2ed7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class OpenArmReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 550 + save_interval = 50 + experiment_name = "openarm_bi_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-2, + 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/reach/config/openarm/bimanual/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/joint_pos_env_cfg.py new file mode 100644 index 000000000000..6b17b4174cb0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/joint_pos_env_cfg.py @@ -0,0 +1,80 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +## +# Pre-defined configs +## + +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp +from isaaclab_tasks.manager_based.manipulation.reach.config.openarm.bimanual.reach_openarm_bi_env_cfg import ReachEnvCfg + +from isaaclab_assets.robots.openarm import OPENARM_BI_HIGH_PD_CFG + +## +# Environment configuration +## + + +@configclass +class OpenArmReachEnvCfg(ReachEnvCfg): + """Configuration for the Bimanual OpenArm Reach Environment.""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch robot to OpenArm + self.scene.robot = OPENARM_BI_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # override rewards + self.rewards.left_end_effector_position_tracking.params["asset_cfg"].body_names = ["openarm_left_hand"] + self.rewards.left_end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "openarm_left_hand" + ] + self.rewards.left_end_effector_orientation_tracking.params["asset_cfg"].body_names = ["openarm_left_hand"] + + self.rewards.right_end_effector_position_tracking.params["asset_cfg"].body_names = ["openarm_right_hand"] + self.rewards.right_end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "openarm_right_hand" + ] + self.rewards.right_end_effector_orientation_tracking.params["asset_cfg"].body_names = ["openarm_right_hand"] + + # override actions + self.actions.left_arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "openarm_left_joint.*", + ], + scale=0.5, + use_default_offset=True, + ) + + self.actions.right_arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "openarm_right_joint.*", + ], + scale=0.5, + use_default_offset=True, + ) + + # override command generator body + # end-effector is along z-direction + self.commands.left_ee_pose.body_name = "openarm_left_hand" + self.commands.right_ee_pose.body_name = "openarm_right_hand" + + +@configclass +class OpenArmReachEnvCfg_PLAY(OpenArmReachEnvCfg): + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/reach_openarm_bi_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/reach_openarm_bi_env_cfg.py new file mode 100644 index 000000000000..7ccdfa0f851b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/reach_openarm_bi_env_cfg.py @@ -0,0 +1,335 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +We modified parts of the environmentโ€”such as the targetโ€™s position and orientationโ€”to better suit the smaller robot. +""" + +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 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.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, 0)), + ) + + # 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.""" + + left_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.15, 0.3), + pos_y=(0.15, 0.25), + pos_z=(0.3, 0.5), + roll=(-math.pi / 6, math.pi / 6), + pitch=(3 * math.pi / 2, 3 * math.pi / 2), + yaw=(8 * math.pi / 9, 10 * math.pi / 9), + ), + ) + + right_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.15, 0.3), + pos_y=(-0.25, -0.15), + pos_z=(0.3, 0.5), + roll=(-math.pi / 6, math.pi / 6), + pitch=(3 * math.pi / 2, 3 * math.pi / 2), + yaw=(8 * math.pi / 9, 10 * math.pi / 9), + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + left_arm_action: ActionTerm = MISSING + right_arm_action: ActionTerm = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + left_joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + "openarm_left_joint.*", + ], + ) + }, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + + right_joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + "openarm_right_joint.*", + ], + ) + }, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + + left_joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + "openarm_left_joint.*", + ], + ) + }, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + right_joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + "openarm_right_joint.*", + ], + ) + }, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + left_pose_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "left_ee_pose"}) + right_pose_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "right_ee_pose"}) + left_actions = ObsTerm(func=mdp.last_action, params={"action_name": "left_arm_action"}) + right_actions = ObsTerm(func=mdp.last_action, params={"action_name": "right_arm_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 + left_end_effector_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=MISSING), + "command_name": "left_ee_pose", + }, + ) + + right_end_effector_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.25, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=MISSING), + "command_name": "right_ee_pose", + }, + ) + + left_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": "left_ee_pose", + }, + ) + + right_end_effector_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.2, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=MISSING), + "std": 0.1, + "command_name": "right_ee_pose", + }, + ) + + left_end_effector_orientation_tracking = RewTerm( + func=mdp.orientation_command_error, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=MISSING), + "command_name": "left_ee_pose", + }, + ) + + right_end_effector_orientation_tracking = RewTerm( + func=mdp.orientation_command_error, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=MISSING), + "command_name": "right_ee_pose", + }, + ) + + # action penalty + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.0001) + left_joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.0001, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + "openarm_left_joint.*", + ], + ) + }, + ) + right_joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.0001, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + "openarm_right_joint.*", + ], + ) + }, + ) + + +@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}, + ) + + left_joint_vel = CurrTerm( + func=mdp.modify_reward_weight, + params={"term_name": "left_joint_vel", "weight": -0.001, "num_steps": 4500}, + ) + + right_joint_vel = CurrTerm( + func=mdp.modify_reward_weight, + params={"term_name": "right_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 = 24.0 + self.viewer.eye = (3.5, 3.5, 3.5) + # simulation settings + self.sim.dt = 1.0 / 60.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/__init__.py new file mode 100644 index 000000000000..d0003b608094 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/__init__.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Reach-OpenArm-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmReachEnvCfg", + "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:OpenArmReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Reach-OpenArm-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:OpenArmReachEnvCfg_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:OpenArmReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 000000000000..29349e1e389f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +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: openarm_reach + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..4d43c3574195 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class OpenArmReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1000 + save_interval = 50 + experiment_name = "openarm_reach" + run_name = "" + resume = False + empirical_normalization = True + 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-2, + 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/reach/config/openarm/unimanual/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/skrl_ppo_cfg.yaml new file mode 100644 index 000000000000..93bd0e506bb4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,85 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +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: "openarm_reach" + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/joint_pos_env_cfg.py new file mode 100644 index 000000000000..2bfd6e326a5a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/joint_pos_env_cfg.py @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp +from isaaclab_tasks.manager_based.manipulation.reach.config.openarm.unimanual.reach_openarm_uni_env_cfg import ( + ReachEnvCfg, +) + +## +# Pre-defined configs +## +from isaaclab_assets.robots.openarm import OPENARM_UNI_CFG + +## +# Environment configuration +## + + +@configclass +class OpenArmReachEnvCfg(ReachEnvCfg): + """Configuration for the single-arm OpenArm Reach Environment.""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch robot to OpenArm + self.scene.robot = OPENARM_UNI_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "openarm_joint1": 1.57, + "openarm_joint2": 0.0, + "openarm_joint3": -1.57, + "openarm_joint4": 1.57, + "openarm_joint5": 0.0, + "openarm_joint6": 0.0, + "openarm_joint7": 0.0, + "openarm_finger_joint.*": 0.0, + }, # Close the gripper + ), + ) + + # override rewards + self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["openarm_hand"] + self.rewards.end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = ["openarm_hand"] + self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["openarm_hand"] + + # override actions + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "openarm_joint.*", + ], + scale=0.5, + use_default_offset=True, + ) + + # override command generator body + # end-effector is along z-direction + self.commands.ee_pose.body_name = "openarm_hand" + + +@configclass +class OpenArmReachEnvCfg_PLAY(OpenArmReachEnvCfg): + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/reach_openarm_uni_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/reach_openarm_uni_env_cfg.py new file mode 100644 index 000000000000..ed9bcbfc08be --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/reach_openarm_uni_env_cfg.py @@ -0,0 +1,248 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +We modified parts of the environmentโ€”such as the targetโ€™s position and orientationโ€”to better suit the smaller robot. +""" + +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 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.25, 0.35), + pos_y=(-0.2, 0.2), + pos_z=(0.3, 0.4), + roll=(-math.pi / 6, math.pi / 6), + pitch=(math.pi / 2, math.pi / 2), + yaw=(-math.pi / 9, math.pi / 9), + ), + ) + + +@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, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=["openarm_joint.*"], + ) + }, + noise=Unoise(n_min=-0.01, n_max=0.01), + ) + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=["openarm_joint.*"], + ) + }, + 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", + joint_names=["openarm_joint.*"], + ) + }, + ) + + +@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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/__init__.py index 2eefd4312a24..fafe5b75820c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rl_games_ppo_cfg.yaml index ab0b67fc7f44..06a3c70554e3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rl_games_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rl_games_ppo_cfg.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + params: seed: 42 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py index 7e712b7a2a3a..c445786c44c7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -15,10 +15,10 @@ class UR10ReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): save_interval = 50 experiment_name = "reach_ur10" run_name = "" - resume = False - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[64, 64], critic_hidden_dims=[64, 64], activation="elu", 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 5ffaffd11758..327a9b9ca80e 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ACTIONS @@ -23,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/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/joint_pos_env_cfg.py index 8f4c1f261b62..6ddf935768b8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/joint_pos_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/__init__.py index a759b12415a1..3fec83fe70af 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/rewards.py index 892f5a540192..76f2fe36db41 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import RigidObject from isaaclab.managers import SceneEntityCfg from isaaclab.utils.math import combine_frame_transforms, quat_error_magnitude, quat_mul @@ -28,8 +29,8 @@ def position_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: 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_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) - curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + 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) @@ -46,8 +47,8 @@ def position_command_error_tanh( 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_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) - curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + 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) @@ -64,6 +65,6 @@ def orientation_command_error(env: ManagerBasedRLEnv, command_name: str, asset_c 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_state_w[:, 3:7], des_quat_b) - curr_quat_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], 3:7] # type: ignore + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py index c648675b4433..bad88b401c7c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,6 +7,10 @@ import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.gamepad import Se3GamepadCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import ActionTermCfg as ActionTerm from isaaclab.managers import CurriculumTermCfg as CurrTerm @@ -206,3 +210,20 @@ def __post_init__(self): self.viewer.eye = (3.5, 3.5, 3.5) # simulation settings self.sim.dt = 1.0 / 60.0 + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + gripper_term=False, + sim_device=self.sim.device, + ), + "gamepad": Se3GamepadCfg( + gripper_term=False, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + gripper_term=False, + sim_device=self.sim.device, + ), + }, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/__init__.py index 350778be73d1..9e0ebd843653 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/__init__.py index 350778be73d1..9e0ebd843653 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/__init__.py index 41746a6f48a9..0c93d83ff1f1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/__init__.py @@ -1,18 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import gymnasium as gym -import os -from . import ( - agents, - stack_ik_rel_blueprint_env_cfg, - stack_ik_rel_env_cfg, - stack_ik_rel_instance_randomize_env_cfg, - stack_joint_pos_env_cfg, - stack_joint_pos_instance_randomize_env_cfg, -) +from . import agents ## # Register Gym environments. @@ -26,7 +18,7 @@ id="Isaac-Stack-Cube-Franka-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg, + "env_cfg_entry_point": f"{__name__}.stack_joint_pos_env_cfg:FrankaCubeStackEnvCfg", }, disable_env_checker=True, ) @@ -35,7 +27,9 @@ 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, + "env_cfg_entry_point": ( + f"{__name__}.stack_joint_pos_instance_randomize_env_cfg:FrankaCubeStackInstanceRandomizeEnvCfg" + ), }, disable_env_checker=True, ) @@ -49,8 +43,40 @@ 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"), + "env_cfg_entry_point": f"{__name__}.stack_ik_rel_env_cfg:FrankaCubeStackEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}: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": f"{__name__}.stack_ik_rel_visuomotor_env_cfg:FrankaCubeStackVisuomotorEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_image_200.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.stack_ik_rel_visuomotor_cosmos_env_cfg:FrankaCubeStackVisuomotorCosmosEnvCfg" + ), + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_image_cosmos.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": f"{__name__}.stack_ik_abs_env_cfg:FrankaCubeStackEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", }, disable_env_checker=True, ) @@ -59,7 +85,9 @@ 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, + "env_cfg_entry_point": ( + f"{__name__}.stack_ik_rel_instance_randomize_env_cfg:FrankaCubeStackInstanceRandomizeEnvCfg" + ), }, disable_env_checker=True, ) @@ -68,7 +96,27 @@ 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, + "env_cfg_entry_point": f"{__name__}.stack_ik_rel_blueprint_env_cfg:FrankaCubeStackBlueprintEnvCfg", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.stack_ik_rel_env_cfg_skillgen:FrankaCubeStackSkillgenEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Bin-Franka-IK-Rel-Mimic-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.bin_stack_ik_rel_env_cfg:FrankaBinStackEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", }, disable_env_checker=True, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_200.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_200.json new file mode 100644 index 000000000000..33117b90e3f3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_200.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": 181, + "crop_width": 181, + "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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_cosmos.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_cosmos.json new file mode 100644 index 000000000000..5f68551765b9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_cosmos.json @@ -0,0 +1,218 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_image_franka_stack_cosmos", + "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" + ], + "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": 180, + "crop_width": 180, + "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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json index 24d807ac342d..23e490971f39 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json @@ -1,7 +1,7 @@ { "algo_name": "bc", "experiment": { - "name": "bc", + "name": "bc_rnn_low_dim_franka_stack", "validate": false, "logging": { "terminal_output_to_txt": true, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_ik_rel_env_cfg.py new file mode 100644 index 000000000000..91ddcbb851cc --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_ik_rel_env_cfg.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import bin_stack_joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaBinStackEnvCfg(bin_stack_joint_pos_env_cfg.FrankaBinStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + 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.0]), + ) 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 new file mode 100644 index 000000000000..4121c1bb2e20 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_joint_pos_env_cfg.py @@ -0,0 +1,207 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +import isaaclab.sim as sim_utils +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, ISAACLAB_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", + mode="reset", + 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"), + }, + ) + + # Reset blue bin position + reset_blue_bin_pose = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + # Keep bin at fixed position - no randomization + "pose_range": {"x": (0.4, 0.4), "y": (0.0, 0.0), "z": (0.0203, 0.0203), "yaw": (0.0, 0.0)}, + "min_separation": 0.0, + "asset_cfgs": [SceneEntityCfg("blue_sorting_bin")], + }, + ) + + # Reset cube 1 to initial position (inside the bin) + reset_cube_1_pose = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + "pose_range": {"x": (0.4, 0.4), "y": (0.0, 0.0), "z": (0.0203, 0.0203), "yaw": (0.0, 0.0)}, + "min_separation": 0.0, + "asset_cfgs": [SceneEntityCfg("cube_1")], + }, + ) + + # Reset cube 2 and 3 to initial position (outside the bin, to the left and right) + reset_cube_pose = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + "pose_range": {"x": (0.65, 0.70), "y": (-0.18, 0.18), "z": (0.0203, 0.0203), "yaw": (-1.0, 1.0, 0)}, + "min_separation": 0.1, + "asset_cfgs": [SceneEntityCfg("cube_2"), SceneEntityCfg("cube_3")], + }, + ) + + +@configclass +class FrankaBinStackEnvCfg(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}, + ) + # 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( + solver_position_iteration_count=40, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ) + + # Blue sorting bin positioned at table center + self.scene.blue_sorting_bin = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlueSortingBin", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.4, 0.0, 0.0203), rot=(1.0, 0.0, 0.0, 0.0)), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_bin_blue.usd", + scale=(1.1, 1.6, 3.3), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Cube 1 positioned at the bottom center of the blue bin + # The bin is at (0.4, 0.0, 0.0203), so cube_1 should be slightly above it + self.scene.cube_1 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_1", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.4, 0.0, 0.025), rot=(1.0, 0.0, 0.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, + ), + ) + + # Cube 2 positioned outside the bin (to the right) + self.scene.cube_2 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_2", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.85, 0.25, 0.0203), rot=(1.0, 0.0, 0.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 3 positioned outside the bin (to the left) + self.scene.cube_3 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_3", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.85, -0.25, 0.0203), rot=(1.0, 0.0, 0.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, + ), + ) + + # 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.0), + ), + ), + 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/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py new file mode 100644 index 000000000000..8d9b18bcd95f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py @@ -0,0 +1,59 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_abs_retargeter import Se3AbsRetargeterCfg +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"), + ) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3AbsRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.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/franka/stack_ik_rel_blueprint_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_blueprint_env_cfg.py index 69c544083be1..3586508df2dc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_blueprint_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_blueprint_env_cfg.py @@ -1,9 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import os + import torch from torchvision.utils import save_image @@ -84,7 +85,9 @@ def image( if images.dtype == torch.uint8: images = images.float() / 255.0 # Get total successful episodes - total_successes = sum(env.recorder_manager._exported_successful_episode_count.values()) + 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) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py index cfd33246a036..16eb9b9f087e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py @@ -1,9 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeterCfg from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg from isaaclab.utils import configclass @@ -34,3 +39,31 @@ def __post_init__(self): scale=0.5, body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), ) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=DeviceBase.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=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg_skillgen.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg_skillgen.py new file mode 100644 index 000000000000..d2a2bd62100a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg_skillgen.py @@ -0,0 +1,167 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeterCfg +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.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.""" + + 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"), + }, + ) + stack_2 = ObsTerm( + func=mdp.object_stacked, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "upper_object_cfg": SceneEntityCfg("cube_3"), + "lower_object_cfg": SceneEntityCfg("cube_2"), + }, + ) + + 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 FrankaCubeStackSkillgenEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Override observations with SkillGen-specific config + self.observations = ObservationsCfg() + + # 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.0]), + ) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=DeviceBase.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=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + } + ) + + # Apply skillgen-specific cube position randomization + self.events.randomize_cube_positions.params["pose_range"] = { + "x": (0.45, 0.6), + "y": (-0.23, 0.23), + "z": (0.0203, 0.0203), + "yaw": (-1.0, 1, 0), + } + + # Set the offset for the end effector to be 0.0 + for f in self.scene.ee_frame.target_frames: + if f.name == "end_effector": + f.offset.pos = [0.0, 0.0, 0.0] + break diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_instance_randomize_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_instance_randomize_env_cfg.py index 84759a50236d..4f31184585dd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_instance_randomize_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_instance_randomize_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -27,6 +27,9 @@ def __post_init__(self): # 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", 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 new file mode 100644 index 000000000000..2cdd8ef39ee4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_cosmos_env_cfg.py @@ -0,0 +1,160 @@ +# Copyright (c) 2025-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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 CameraCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.stack import mdp + +from . import stack_ik_rel_visuomotor_env_cfg + + +@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} + ) + table_cam_segmentation = ObsTerm( + func=mdp.image, + params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "semantic_segmentation", "normalize": True}, + ) + table_cam_normals = ObsTerm( + func=mdp.image, + params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "normals", "normalize": True}, + ) + table_cam_depth = ObsTerm( + func=mdp.image, + params={ + "sensor_cfg": SceneEntityCfg("table_cam"), + "data_type": "distance_to_image_plane", + "normalize": True, + }, + ) + + 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 FrankaCubeStackVisuomotorCosmosEnvCfg(stack_ik_rel_visuomotor_env_cfg.FrankaCubeStackVisuomotorEnvCfg): + observations: ObservationsCfg = ObservationsCfg() + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # 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), + "class:cube_2": (255, 36, 66, 255), + "class:cube_3": (55, 255, 139, 255), + "class:table": (255, 237, 218, 255), + "class:ground": (100, 100, 100, 255), + "class:robot": (204, 110, 248, 255), + "class:UNLABELLED": (150, 150, 150, 255), + "class:BACKGROUND": (200, 200, 200, 255), + } + + # 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=200, + width=200, + 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=200, + width=200, + data_types=["rgb", "semantic_segmentation", "normals", "distance_to_image_plane"], + colorize_semantic_segmentation=True, + semantic_segmentation_mapping=SEMANTIC_MAPPING, + 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.num_rerenders_on_reset = 1 + self.sim.render.antialiasing_mode = "OFF" + + # List of image observations in policy observations + self.image_obs_list = ["table_cam", "wrist_cam"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_env_cfg.py new file mode 100644 index 000000000000..13040620b788 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_env_cfg.py @@ -0,0 +1,238 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import 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 EventTermCfg as EventTerm +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 isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, NVIDIA_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.stack import mdp +from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events + +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 EventCfg(stack_joint_pos_env_cfg.EventCfg): + """Configuration for events.""" + + randomize_light = EventTerm( + func=franka_stack_events.randomize_scene_lighting_domelight, + mode="reset", + params={ + "intensity_range": (1500.0, 10000.0), + "color_variation": 0.4, + "textures": [ + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Cloudy/abandoned_parking_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Cloudy/evening_road_01_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Cloudy/lakeside_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Indoor/autoshop_01_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Indoor/carpentry_shop_01_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Indoor/hospital_room_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Indoor/hotel_room_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Indoor/old_bus_depot_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Indoor/small_empty_house_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Indoor/surgery_4k.hdr", + f"{NVIDIA_NUCLEUS_DIR}/Assets/Skies/Studio/photo_studio_01_4k.hdr", + ], + "default_intensity": 3000.0, + "default_color": (0.75, 0.75, 0.75), + "default_texture": "", + }, + ) + + randomize_table_visual_material = EventTerm( + func=franka_stack_events.randomize_visual_texture_material, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("table"), + "textures": [ + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Ash/Ash_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Bamboo_Planks/Bamboo_Planks_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Birch/Birch_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Cherry/Cherry_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Mahogany_Planks/Mahogany_Planks_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Oak/Oak_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Plywood/Plywood_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Timber/Timber_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Timber_Cladding/Timber_Cladding_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Wood/Walnut_Planks/Walnut_Planks_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Stone/Marble/Marble_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Steel_Stainless/Steel_Stainless_BaseColor.png", + ], + "default_texture": ( + f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/Materials/Textures/DemoTable_TableBase_BaseColor.png" + ), + }, + ) + + randomize_robot_arm_visual_texture = EventTerm( + func=franka_stack_events.randomize_visual_texture_material, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot"), + "textures": [ + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Aluminum_Cast/Aluminum_Cast_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Aluminum_Polished/Aluminum_Polished_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Brass/Brass_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Bronze/Bronze_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Brushed_Antique_Copper/Brushed_Antique_Copper_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Cast_Metal_Silver_Vein/Cast_Metal_Silver_Vein_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Copper/Copper_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Gold/Gold_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Iron/Iron_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/RustedMetal/RustedMetal_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Silver/Silver_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Steel_Carbon/Steel_Carbon_BaseColor.png", + f"{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Steel_Stainless/Steel_Stainless_BaseColor.png", + ], + }, + ) + + +@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() + + # Evaluation settings + eval_mode = False + eval_type = None + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set events + self.events = EventCfg() + + # 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]), + ) + + # 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=200, + width=200, + 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=200, + width=200, + 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.num_rerenders_on_reset = 3 + self.sim.render.antialiasing_mode = "DLAA" # Use DLAA for higher quality rendering + + # List of image observations in policy observations + self.image_obs_list = ["table_cam", "wrist_cam"] 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 782fe29e906d..e344f0021db2 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 @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -30,7 +30,7 @@ class EventCfg: init_franka_arm_pose = EventTerm( func=franka_stack_events.set_default_joint_pose, - mode="startup", + mode="reset", params={ "default_pose": [0.0444, -0.1894, -0.1107, -2.5148, 0.0044, 2.3775, 0.6952, 0.0400, 0.0400], }, @@ -59,6 +59,8 @@ class EventCfg: @configclass class FrankaCubeStackEnvCfg(StackEnvCfg): + """Configuration for the Franka Cube Stack Environment.""" + def __post_init__(self): # post init of parent super().__post_init__() @@ -86,6 +88,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_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 5a244e5e4218..3e14199a2630 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 @@ -1,15 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import 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 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 @@ -75,6 +74,9 @@ def __post_init__(self): # 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 @@ -85,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( @@ -164,32 +170,6 @@ def __post_init__(self): 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) 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 new file mode 100644 index 000000000000..760669cca629 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/__init__.py @@ -0,0 +1,73 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +import gymnasium as gym + +## +# Register Gym environments. +## + +## +# RMPFlow (with Joint Limit Constraint and Obstacle Avoidance) for Galbot Single Arm Cube Stack Task +# you can use for both absolute and relative mode, by given the USE_RELATIVE_MODE environment variable +## +gym.register( + id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.stack_rmp_rel_env_cfg:RmpFlowGalbotLeftArmCubeStackEnvCfg", + }, + disable_env_checker=True, +) + + +gym.register( + id="Isaac-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.stack_rmp_rel_env_cfg:RmpFlowGalbotRightArmCubeStackEnvCfg", + }, + disable_env_checker=True, +) + + +## +# Visuomotor Task for Galbot Left ArmCube Stack Task +## +gym.register( + id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.stack_rmp_rel_env_cfg:RmpFlowGalbotLeftArmCubeStackVisuomotorEnvCfg", + }, + disable_env_checker=True, +) + +## +# Policy Close-loop Evaluation Task for Galbot Left Arm Cube Stack Task (in Joint Space) +## +gym.register( + id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Joint-Position-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.stack_rmp_rel_env_cfg:GalbotLeftArmJointPositionCubeStackVisuomotorEnvCfg_PLAY" + ), + }, + disable_env_checker=True, +) + +## +# Policy Close-loop Evaluation Task for Galbot Left Arm Cube Stack Task (in Task Space) +## +gym.register( + id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-RmpFlow-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "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 new file mode 100644 index 000000000000..4506a95eaba9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py @@ -0,0 +1,325 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.assets import RigidObjectCfg, SurfaceGripperCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.openxr.openxr_device import 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 +from isaaclab.managers import ObservationTermCfg as ObsTerm +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 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 + +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 ObservationsCfg, StackEnvCfg + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip +from isaaclab_assets.robots.galbot import GALBOT_ONE_CHARLIE_CFG # isort: skip + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True}) + + randomize_cube_positions = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + "pose_range": { + "x": (-0.2, 0.0), + "y": (0.20, 0.40), + "z": (0.0203, 0.0203), + "yaw": (-1.0, 1.0, 0.0), + }, + "min_separation": 0.1, + "asset_cfgs": [SceneEntityCfg("cube_1"), SceneEntityCfg("cube_2"), SceneEntityCfg("cube_3")], + }, + ) + + +@configclass +class ObservationGalbotLeftArmGripperCfg: + """Observations for the Galbot Left Arm Gripper.""" + + @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_abs_obs_in_base_frame, + params={ + "robot_cfg": SceneEntityCfg("robot"), + }, + ) + cube_positions = ObsTerm( + func=mdp.cube_poses_in_base_frame, params={"robot_cfg": SceneEntityCfg("robot"), "return_key": "pos"} + ) + cube_orientations = ObsTerm( + func=mdp.cube_poses_in_base_frame, params={"robot_cfg": SceneEntityCfg("robot"), "return_key": "quat"} + ) + + eef_pos = ObsTerm( + func=mdp.ee_frame_pose_in_base_frame, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "return_key": "pos", + }, + ) + eef_quat = ObsTerm( + func=mdp.ee_frame_pose_in_base_frame, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "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(ObservationsCfg.SubtaskCfg): + """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): + super().__post_init__() + + @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 + + subtask_terms: SubtaskCfg = SubtaskCfg() + policy: PolicyCfg = PolicyCfg() + rgb_camera: RGBCameraPolicyCfg = RGBCameraPolicyCfg() + + +@configclass +class GalbotLeftArmCubeStackEnvCfg(StackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # MDP settings + + # Set events + self.events = EventCfg() + self.observations.policy = ObservationGalbotLeftArmGripperCfg().PolicyCfg() + self.observations.subtask_terms = ObservationGalbotLeftArmGripperCfg().SubtaskCfg() + + # Set galbot as robot + self.scene.robot = GALBOT_ONE_CHARLIE_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (galbot) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["left_arm_joint.*"], scale=0.5, use_default_offset=True + ) + # Enable Parallel Gripper + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["left_gripper_.*_joint"], + open_command_expr={"left_gripper_.*_joint": 0.035}, + close_command_expr={"left_gripper_.*_joint": 0.0}, + ) + self.gripper_joint_names = ["left_gripper_.*_joint"] + self.gripper_open_val = 0.035 + self.gripper_threshold = 0.010 + + # 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, + ) + cube_collision_properties = CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0) + + # 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, + collision_props=cube_collision_properties, + ), + ) + 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, + collision_props=cube_collision_properties, + ), + ) + 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, + collision_props=cube_collision_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/left_gripper_tcp_link", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.0], + ), + ), + ], + ) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3AbsRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) + + +@configclass +class GalbotRightArmCubeStackEnvCfg(GalbotLeftArmCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Move to area below right hand (invert y-axis) + left, right = self.events.randomize_cube_positions.params["pose_range"]["y"] + self.events.randomize_cube_positions.params["pose_range"]["y"] = (-right, -left) + + # Set actions for the specific robot type (galbot) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["right_arm_joint.*"], scale=0.5, use_default_offset=True + ) + + # 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.0075, + shear_force_limit=5000.0, + coaxial_force_limit=5000.0, + retry_interval=0.05, + ) + + # Set surface gripper action + self.actions.gripper_action = SurfaceGripperBinaryActionCfg( + asset_name="surface_gripper", + open_command=-1.0, + close_command=1.0, + ) + + 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=DeviceBase.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.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 new file mode 100644 index 000000000000..eebb79e1d315 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py @@ -0,0 +1,324 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +import os + +import isaaclab.sim as sim_utils +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.openxr.openxr_device import 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 +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.stack import mdp + +from . import stack_joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab.controllers.config.rmp_flow import ( # isort: skip + GALBOT_LEFT_ARM_RMPFLOW_CFG, + GALBOT_RIGHT_ARM_RMPFLOW_CFG, +) +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip + + +## +# RmpFlow Controller for Galbot Left Arm Cube Stack Task (with Parallel Gripper) +## +@configclass +class RmpFlowGalbotLeftArmCubeStackEnvCfg(stack_joint_pos_env_cfg.GalbotLeftArmCubeStackEnvCfg): + """Configuration for the Galbot Left Arm Cube Stack Environment.""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # read use_relative_mode from environment variable + # True for record_demos, and False for replay_demos, annotate_demos and generate_demos + 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 (Galbot) + self.actions.arm_action = RMPFlowActionCfg( + asset_name="robot", + joint_names=["left_arm_joint.*"], + body_name="left_gripper_tcp_link", + controller=GALBOT_LEFT_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, + ) + + # Set the simulation parameters + self.sim.dt = 1 / 60 + self.sim.render_interval = 6 + + self.decimation = 3 + self.episode_length_s = 30.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, + ), + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=DeviceBase.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=DeviceBase.TrackingTarget.HAND_LEFT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) + + +## +# RmpFlow Controller for Galbot Right Arm Cube Stack Task (with Surface Gripper) +## +@configclass +class RmpFlowGalbotRightArmCubeStackEnvCfg(stack_joint_pos_env_cfg.GalbotRightArmCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # read use_relative_mode from environment variable + # True for record_demos, and False for replay_demos, annotate_demos and generate_demos + 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 (Galbot) + self.actions.arm_action = RMPFlowActionCfg( + asset_name="robot", + joint_names=["right_arm_joint.*"], + body_name="right_suction_cup_tcp_link", + controller=GALBOT_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, + ) + # Set the simulation parameters + self.sim.dt = 1 / 120 + self.sim.render_interval = 6 + + 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( + 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, + ), + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=DeviceBase.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=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) + + +## +# Visuomotor Env for Record, Generate and Replay (in Task Space) +## +@configclass +class RmpFlowGalbotLeftArmCubeStackVisuomotorEnvCfg(RmpFlowGalbotLeftArmCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set left and right wrist cameras for VLA policy training + self.scene.right_wrist_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/right_arm_camera_sim_view_frame/right_camera", + update_period=0.0333, + height=256, + width=256, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=18.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"), + ) + + self.scene.left_wrist_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/left_arm_camera_sim_view_frame/left_camera", + update_period=0.0333, + height=256, + width=256, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=18.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"), + ) + + # Set ego view camera + self.scene.ego_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/head_camera_sim_view_frame/head_camera", + update_period=0.0333, + height=256, + width=256, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=18.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"), + ) + + # Set front view camera + self.scene.front_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/front_camera", + update_period=0.0333, + height=256, + width=256, + 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.6), rot=(-0.3799, 0.5963, 0.5963, -0.3799), convention="ros"), + ) + + marker_right_camera_cfg = FRAME_MARKER_CFG.copy() + marker_right_camera_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_right_camera_cfg.prim_path = "/Visuals/FrameTransformerRightCamera" + + self.scene.right_arm_camera_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + debug_vis=False, + visualizer_cfg=marker_right_camera_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/right_arm_camera_sim_view_frame", + name="right_camera", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.0], + rot=(0.5, -0.5, 0.5, -0.5), + ), + ), + ], + ) + + marker_left_camera_cfg = FRAME_MARKER_CFG.copy() + marker_left_camera_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_left_camera_cfg.prim_path = "/Visuals/FrameTransformerLeftCamera" + + self.scene.left_arm_camera_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + debug_vis=False, + visualizer_cfg=marker_left_camera_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/left_arm_camera_sim_view_frame", + name="left_camera", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.0], + rot=(0.5, -0.5, 0.5, -0.5), + ), + ), + ], + ) + + # Set settings for camera rendering + self.num_rerenders_on_reset = 3 + self.sim.render.antialiasing_mode = "DLAA" # Use DLAA for higher quality rendering + + # List of image observations in policy observations + self.image_obs_list = ["ego_cam", "left_wrist_cam", "right_wrist_cam"] + + +## +# Task Env for VLA Policy Close-loop Evaluation (in Joint Space) +## + + +@configclass +class GalbotLeftArmJointPositionCubeStackVisuomotorEnvCfg_PLAY(RmpFlowGalbotLeftArmCubeStackVisuomotorEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["left_arm_joint.*"], scale=1.0, use_default_offset=False + ) + # Enable Parallel Gripper with AbsBinaryJointPosition Control + self.actions.gripper_action = mdp.AbsBinaryJointPositionActionCfg( + asset_name="robot", + threshold=0.030, + joint_names=["left_gripper_.*_joint"], + open_command_expr={"left_gripper_.*_joint": 0.035}, + close_command_expr={"left_gripper_.*_joint": 0.023}, + # real gripper close data is 0.0235, close to it to meet data distribution, + # but smaller to ensure robust grasping. + # during VLA inference, we set the close command to '0.023' since the VLA + # has never seen the gripper fully closed. + ) + + +## +# Task Envs for VLA Policy Close-loop Evaluation (in Task Space) +## +@configclass +class GalbotLeftArmRmpFlowCubeStackVisuomotorEnvCfg_PLAY(RmpFlowGalbotLeftArmCubeStackVisuomotorEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Enable Parallel Gripper with AbsBinaryJointPosition Control + self.actions.gripper_action = mdp.AbsBinaryJointPositionActionCfg( + asset_name="robot", + threshold=0.030, + joint_names=["left_gripper_.*_joint"], + open_command_expr={"left_gripper_.*_joint": 0.035}, + close_command_expr={"left_gripper_.*_joint": 0.023}, + ) 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 new file mode 100644 index 000000000000..81165e4a28a4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/__init__.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +## +# Register Gym environments. +## + + +## +# Inverse Kinematics - Relative Pose Control +## + +gym.register( + id="Isaac-Stack-Cube-UR10-Long-Suction-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.stack_ik_rel_env_cfg:UR10LongSuctionCubeStackEnvCfg", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-UR10-Short-Suction-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "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_ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_ik_rel_env_cfg.py new file mode 100644 index 000000000000..0e6b2df08b29 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_ik_rel_env_cfg.py @@ -0,0 +1,80 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import stack_joint_pos_env_cfg + + +@configclass +class UR10LongSuctionCubeStackEnvCfg(stack_joint_pos_env_cfg.UR10LongSuctionCubeStackEnvCfg): + """Configuration for the UR10 Long Suction Cube Stack Environment.""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set actions for the specific robot type (UR10 LONG SUCTION) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=[".*_joint"], + body_name="ee_link", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=1.0, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, -0.22]), + ) + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.02, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + } + ) + + +@configclass +class UR10ShortSuctionCubeStackEnvCfg(stack_joint_pos_env_cfg.UR10ShortSuctionCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set actions for the specific robot type (UR10 SHORT SUCTION) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=[".*_joint"], + body_name="ee_link", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=1.0, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, -0.159]), + ) + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.02, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + } + ) 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 new file mode 100644 index 000000000000..c3d73a3fb3c9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py @@ -0,0 +1,212 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.assets import RigidObjectCfg, SurfaceGripperCfg +from isaaclab.envs.mdp.actions.actions_cfg import SurfaceGripperBinaryActionCfg +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 + +from isaaclab_assets.robots.universal_robots import ( # isort: skip + UR10_LONG_SUCTION_CFG, + UR10_SHORT_SUCTION_CFG, +) + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip + + +@configclass +class EventCfgLongSuction: + """Configuration for events.""" + + init_franka_arm_pose = EventTerm( + func=franka_stack_events.set_default_joint_pose, + mode="reset", + params={ + "default_pose": [0.0, -1.5707, 1.5707, -1.5707, -1.5707, 0.0], + }, + ) + + 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, 0)}, + "min_separation": 0.1, + "asset_cfgs": [SceneEntityCfg("cube_1"), SceneEntityCfg("cube_2"), SceneEntityCfg("cube_3")], + }, + ) + + +@configclass +class UR10CubeStackEnvCfg(StackEnvCfg): + # 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, + ) + cube_scale = (1.0, 1.0, 1.0) + # 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" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set events + self.events = EventCfgLongSuction() + + # Set actions for the specific robot type (ur10) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=[".*_joint"], scale=0.5, use_default_offset=True + ) + # Set surface gripper action + self.actions.gripper_action = SurfaceGripperBinaryActionCfg( + asset_name="surface_gripper", + open_command=-1.0, + close_command=1.0, + ) + + # 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=self.cube_scale, + rigid_props=self.cube_properties, + ), + ) + 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=self.cube_scale, + rigid_props=self.cube_properties, + ), + ) + 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=self.cube_scale, + rigid_props=self.cube_properties, + ), + ) + + self.decimation = 5 + self.episode_length_s = 30.0 + # simulation settings + self.sim.dt = 0.01 # 100Hz + self.sim.render_interval = 5 + + +@configclass +class UR10LongSuctionCubeStackEnvCfg(UR10CubeStackEnvCfg): + """Configuration for the UR10 Long Suction Cube Stack Environment.""" + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Suction grippers currently require CPU simulation + self.device = "cpu" + + # Set events + self.events = EventCfgLongSuction() + + # Set UR10 as robot + self.scene.robot = UR10_LONG_SUCTION_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # 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.0075, + shear_force_limit=5000.0, + coaxial_force_limit=5000.0, + retry_interval=0.05, + ) + + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + debug_vis=True, + visualizer_cfg=self.marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/ee_link", + name="end_effector", + offset=OffsetCfg( + pos=[0.22, 0.0, 0.0], + ), + ), + ], + ) + + +@configclass +class UR10ShortSuctionCubeStackEnvCfg(UR10CubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Suction grippers currently require CPU simulation + self.device = "cpu" + + # Set UR10 as robot + self.scene.robot = UR10_SHORT_SUCTION_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # 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.0075, + shear_force_limit=5000.0, + coaxial_force_limit=5000.0, + retry_interval=0.05, + ) + + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + debug_vis=True, + visualizer_cfg=self.marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/ee_link", + name="end_effector", + offset=OffsetCfg( + pos=[0.1585, 0.0, 0.0], + ), + ), + ], + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py index 292b12a91c78..ea04fcc468e9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py index 9c10ee0c5996..3d9e1db48629 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -8,9 +8,12 @@ import math import random -import torch from typing import TYPE_CHECKING +import torch + +from isaacsim.core.utils.extensions import enable_extension + import isaaclab.utils.math as math_utils from isaaclab.assets import Articulation, AssetBase from isaaclab.managers import SceneEntityCfg @@ -57,21 +60,75 @@ def randomize_joint_by_gaussian_offset( asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) +def sample_random_color(base=(0.75, 0.75, 0.75), variation=0.1): + """ + Generates a randomized color that stays close to the base color while preserving overall brightness. + The relative balance between the R, G, and B components is maintained by ensuring that + the sum of random offsets is zero. + + Parameters: + base (tuple): The base RGB color with each component between 0 and 1. + variation (float): Maximum deviation to sample for each channel before balancing. + + Returns: + tuple: A new RGB color with balanced random variation. + """ + # Generate random offsets for each channel in the range [-variation, variation] + offsets = [random.uniform(-variation, variation) for _ in range(3)] + # Compute the average offset + avg_offset = sum(offsets) / 3 + # Adjust offsets so their sum is zero (maintaining brightness) + balanced_offsets = [offset - avg_offset for offset in offsets] + + # Apply the balanced offsets to the base color and clamp each channel between 0 and 1 + new_color = tuple(max(0, min(1, base_component + offset)) for base_component, offset in zip(base, balanced_offsets)) + + return new_color + + def randomize_scene_lighting_domelight( env: ManagerBasedEnv, env_ids: torch.Tensor, intensity_range: tuple[float, float], + color_variation: float, + textures: list[str], + default_intensity: float = 3000.0, + default_color: tuple[float, float, float] = (0.75, 0.75, 0.75), + default_texture: str = "", 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) + intensity_attr.Set(default_intensity) + + color_attr = light_prim.GetAttribute("inputs:color") + color_attr.Set(default_color) + + texture_file_attr = light_prim.GetAttribute("inputs:texture:file") + texture_file_attr.Set(default_texture) + + if not hasattr(env.cfg, "eval_mode") or not env.cfg.eval_mode: + return + + if env.cfg.eval_type in ["light_intensity", "all"]: + # Sample new light intensity + new_intensity = random.uniform(intensity_range[0], intensity_range[1]) + # Set light intensity to light prim + intensity_attr.Set(new_intensity) + + if env.cfg.eval_type in ["light_color", "all"]: + # Sample new light color + new_color = sample_random_color(base=default_color, variation=color_variation) + # Set light color to light prim + color_attr.Set(new_color) + + if env.cfg.eval_type in ["light_texture", "all"]: + # Sample new light texture (background) + new_texture = random.sample(textures, 1)[0] + # Set light texture to light prim + texture_file_attr.Set(new_texture) def sample_object_poses( @@ -184,3 +241,75 @@ def randomize_rigid_objects_in_focus( ) env.rigid_objects_in_focus.append(selected_ids) + + +def randomize_visual_texture_material( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + asset_cfg: SceneEntityCfg, + textures: list[str], + default_texture: str = "", + texture_rotation: tuple[float, float] = (0.0, 0.0), +): + """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. + """ + if hasattr(env.cfg, "eval_mode") and ( + not env.cfg.eval_mode or env.cfg.eval_type not in [f"{asset_cfg.name}_texture", "all"] + ): + return + # textures = [default_texture] + + # 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 + + # 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 = ".*" + + if not hasattr(asset, "cfg"): + prims_group = rep.get.prims(path_pattern=f"{asset.prim_paths[0]}/visuals") + else: + prims_group = rep.get.prims(path_pattern=f"{asset.cfg.prim_path}/{body_names_regex}/visuals") + + with prims_group: + rep.randomizer.texture( + textures=textures, project_uvw=True, texture_rotate=rep.distribution.uniform(*texture_rotation) + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/observations.py index 1adb7a2e9a60..31123e71a308 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/observations.py @@ -1,13 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations +from typing import TYPE_CHECKING, Literal + import torch -from typing import TYPE_CHECKING +import isaaclab.utils.math as math_utils from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import FrameTransformer @@ -256,12 +258,35 @@ def ee_frame_quat(env: ManagerBasedRLEnv, ee_frame_cfg: SceneEntityCfg = SceneEn return ee_frame_quat -def gripper_pos(env: ManagerBasedRLEnv, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: +def gripper_pos( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """ + Obtain the versatile gripper position of both Gripper and Suction Cup. + """ 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) + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + # Handle multiple surface grippers by concatenating their states + gripper_states = [] + for gripper_name, surface_gripper in env.scene.surface_grippers.items(): + gripper_states.append(surface_gripper.state.view(-1, 1)) + + if len(gripper_states) == 1: + return gripper_states[0] + else: + return torch.cat(gripper_states, dim=1) + + 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, "Observation gripper_pos only support parallel gripper for now" + finger_joint_1 = robot.data.joint_pos[:, gripper_joint_ids[0]].clone().unsqueeze(1) + finger_joint_2 = -1 * robot.data.joint_pos[:, gripper_joint_ids[1]].clone().unsqueeze(1) + return torch.cat((finger_joint_1, finger_joint_2), dim=1) + else: + raise NotImplementedError("[Error] Cannot find gripper_joint_names in the environment config") def object_grasped( @@ -270,8 +295,6 @@ def object_grasped( 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.""" @@ -283,13 +306,33 @@ def object_grasped( 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 - ) + 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) + assert len(gripper_joint_ids) == 2, "Observations only support parallel gripper for now" + + grasped = torch.logical_and( + pose_diff < diff_threshold, + torch.abs( + robot.data.joint_pos[:, gripper_joint_ids[0]] + - torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device) + ) + > env.cfg.gripper_threshold, + ) + grasped = torch.logical_and( + grasped, + torch.abs( + robot.data.joint_pos[:, gripper_joint_ids[1]] + - torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device) + ) + > env.cfg.gripper_threshold, + ) return grasped @@ -302,7 +345,6 @@ def object_stacked( 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.""" @@ -316,11 +358,177 @@ def object_stacked( 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 + 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) + stacked = torch.logical_and(suction_cup_is_open, stacked) + + 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, "Observations only support parallel gripper for now" + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[0]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=1e-4, + rtol=1e-4, + ), + stacked, + ) + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[1]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=1e-4, + rtol=1e-4, + ), + stacked, + ) + else: + raise ValueError("No gripper_joint_names found in environment config") + + return stacked + + +def cube_poses_in_base_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + return_key: Literal["pos", "quat", None] = None, +) -> torch.Tensor: + """The position and orientation of the cubes in the robot base 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] + + pos_cube_1_world = cube_1.data.root_pos_w + pos_cube_2_world = cube_2.data.root_pos_w + pos_cube_3_world = cube_3.data.root_pos_w + + quat_cube_1_world = cube_1.data.root_quat_w + quat_cube_2_world = cube_2.data.root_quat_w + quat_cube_3_world = cube_3.data.root_quat_w + + robot: Articulation = env.scene[robot_cfg.name] + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + + pos_cube_1_base, quat_cube_1_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_cube_1_world, quat_cube_1_world + ) + pos_cube_2_base, quat_cube_2_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_cube_2_world, quat_cube_2_world ) - stacked = torch.logical_and( - torch.isclose(robot.data.joint_pos[:, -2], gripper_open_val.to(env.device), atol=1e-4, rtol=1e-4), stacked + pos_cube_3_base, quat_cube_3_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_cube_3_world, quat_cube_3_world ) - return stacked + pos_cubes_base = torch.cat((pos_cube_1_base, pos_cube_2_base, pos_cube_3_base), dim=1) + quat_cubes_base = torch.cat((quat_cube_1_base, quat_cube_2_base, quat_cube_3_base), dim=1) + + if return_key == "pos": + return pos_cubes_base + elif return_key == "quat": + return quat_cubes_base + else: + return torch.cat((pos_cubes_base, quat_cubes_base), dim=1) + + +def object_abs_obs_in_base_frame( + 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"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """ + Object Abs observations (in base frame): remove the relative observations, + and add abs gripper pos and quat in robot base frame + cube_1 pos, + cube_1 quat, + cube_2 pos, + cube_2 quat, + cube_3 pos, + cube_3 quat, + gripper pos, + gripper quat, + """ + 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] + robot: Articulation = env.scene[robot_cfg.name] + + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + + 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 + + pos_cube_1_base, quat_cube_1_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, cube_1_pos_w, cube_1_quat_w + ) + pos_cube_2_base, quat_cube_2_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, cube_2_pos_w, cube_2_quat_w + ) + pos_cube_3_base, quat_cube_3_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, cube_3_pos_w, cube_3_quat_w + ) + + ee_pos_w = ee_frame.data.target_pos_w[:, 0, :] + ee_quat_w = ee_frame.data.target_quat_w[:, 0, :] + ee_pos_base, ee_quat_base = math_utils.subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w) + + return torch.cat( + ( + pos_cube_1_base, + quat_cube_1_base, + pos_cube_2_base, + quat_cube_2_base, + pos_cube_3_base, + quat_cube_3_base, + ee_pos_base, + ee_quat_base, + ), + dim=1, + ) + + +def ee_frame_pose_in_base_frame( + env: ManagerBasedRLEnv, + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + return_key: Literal["pos", "quat", None] = None, +) -> torch.Tensor: + """ + The end effector pose in the robot base frame. + """ + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + ee_frame_pos_w = ee_frame.data.target_pos_w[:, 0, :] + ee_frame_quat_w = ee_frame.data.target_quat_w[:, 0, :] + + robot: Articulation = env.scene[robot_cfg.name] + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + ee_pos_in_base, ee_quat_in_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, ee_frame_pos_w, ee_frame_quat_w + ) + + if return_key == "pos": + return ee_pos_in_base + elif return_key == "quat": + return ee_quat_in_base + else: + return torch.cat((ee_pos_in_base, ee_quat_in_base), dim=1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/terminations.py index 8e41574ef877..2e4c14afcea1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/terminations.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,9 +11,10 @@ from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation, RigidObject from isaaclab.managers import SceneEntityCfg @@ -27,10 +28,9 @@ def cubes_stacked( 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, + xy_threshold: float = 0.04, 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, ): @@ -53,14 +53,41 @@ def cubes_stacked( # 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(pos_diff_c12[:, 2] < 0.0, stacked) stacked = torch.logical_and(h_dist_c23 - height_diff < height_threshold, stacked) + stacked = torch.logical_and(pos_diff_c23[:, 2] < 0.0, 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 - ) + 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) + stacked = torch.logical_and(suction_cup_is_open, stacked) + + 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" + + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[0]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=atol, + rtol=rtol, + ), + stacked, + ) + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[1]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=atol, + rtol=rtol, + ), + stacked, + ) + else: + raise ValueError("No gripper_joint_names found in environment config") return stacked diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py index a2f8ec1be1a3..5c772a11760e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,6 +7,7 @@ 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 @@ -177,6 +178,11 @@ class StackEnvCfg(ManagerBasedRLEnvCfg): 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 @@ -184,7 +190,7 @@ def __post_init__(self): self.episode_length_s = 30.0 # simulation settings self.sim.dt = 0.01 # 100Hz - self.sim.render_interval = self.decimation + self.sim.render_interval = 2 self.sim.physx.bounce_threshold_velocity = 0.2 self.sim.physx.bounce_threshold_velocity = 0.01 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py index 316d9db3ce20..526297b95617 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -10,10 +10,8 @@ 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 @@ -37,10 +35,6 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): # 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", @@ -96,24 +90,8 @@ 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 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/__init__.py index 00a568dfdfb9..f611f9e8eb4d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/__init__.py index a9e5c523d7b5..99d0035ef264 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/__init__.py index c03491851dd2..bcedc58f3f0a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py index 043e187af985..93ec98732f8c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -14,9 +14,10 @@ class NavigationEnvPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_c_navigation" - empirical_normalization = False policy = RslRlPpoActorCriticCfg( init_noise_std=0.5, + actor_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[128, 128], critic_hidden_dims=[128, 128], activation="elu", 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 9b98aa7910c4..3eba30e7fc18 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 @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + seed: 42 @@ -14,7 +19,7 @@ models: initial_log_std: -0.6931471805599453 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128] activations: elu output: ACTIONS @@ -23,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/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py index c55a77fcfb26..96b60705bb50 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/__init__.py index 80ceadd3de26..213391d362b1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py index 940b2c541d8a..c25558c78846 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py @@ -1,14 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from dataclasses import MISSING from typing import TYPE_CHECKING +import torch + import isaaclab.utils.math as math_utils from isaaclab.assets import Articulation from isaaclab.managers import ActionTerm, ActionTermCfg, ObservationGroupCfg, ObservationManager @@ -107,7 +108,7 @@ 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 + # create markers if necessary for the first time if not hasattr(self, "base_vel_goal_visualizer"): # -- goal marker_cfg = GREEN_ARROW_X_MARKER_CFG.copy() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/rewards.py index 1f0211b93e2c..ccaad755d087 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + if TYPE_CHECKING: from isaaclab.envs import ManagerBasedRLEnv diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/utils/__init__.py index 5fe8a8c0193c..495b207c3194 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/hydra.py b/source/isaaclab_tasks/isaaclab_tasks/utils/hydra.py index 1341ce9f9909..525b425917fa 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/hydra.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/hydra.py @@ -1,11 +1,10 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Sub-module with utilities for the hydra configuration system.""" - import functools from collections.abc import Callable @@ -78,10 +77,10 @@ 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, agent_cfg_entry_point) + 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, version_base="1.3") + @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) diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/importer.py b/source/isaaclab_tasks/isaaclab_tasks/utils/importer.py index 1d3e97137aa5..ddbab7ede412 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/importer.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/importer.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -41,6 +41,11 @@ def import_packages(package_name: str, blacklist_pkgs: list[str] | None = None): pass +""" +Internal helpers. +""" + + def _walk_packages( path: str | None = None, prefix: str = "", @@ -51,16 +56,20 @@ def _walk_packages( 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 + the ``blacklist_pkgs`` argument to skip blacklisted packages. Please refer to the original ``pkgutil.walk_packages`` function for more details. + """ + # Default blacklist if blacklist_pkgs is None: blacklist_pkgs = [] - def seen(p, m={}): + def seen(p: str, m: dict[str, bool] = {}) -> bool: + """Check if a package has been seen before.""" if p in m: return True - m[p] = True # noqa: R503 + m[p] = True + return False for info in pkgutil.iter_modules(path, prefix): # check blacklisted @@ -79,7 +88,7 @@ def seen(p, m={}): else: raise else: - path = getattr(sys.modules[info.name], "__path__", None) or [] + path: list = getattr(sys.modules[info.name], "__path__", []) # don't traverse path items we've seen before path = [p for p in path if not seen(p)] diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py index 93fa7547b1fb..0002c5d58d9a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py @@ -1,16 +1,17 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Sub-module with utilities for parsing and loading configurations.""" - -import gymnasium as gym +import collections import importlib import inspect import os import re + +import gymnasium as gym import yaml from isaaclab.envs import DirectRLEnvCfg, ManagerBasedRLEnvCfg @@ -32,6 +33,7 @@ def load_cfg_from_registry(task_name: str, entry_point_key: str) -> dict | objec kwargs={"env_entry_point_cfg": "path.to.config:ConfigClass"}, ) + The parsed configuration object for above example can be obtained as: .. code-block:: python @@ -40,6 +42,7 @@ def load_cfg_from_registry(task_name: str, entry_point_key: str) -> dict | objec 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. @@ -52,12 +55,30 @@ def load_cfg_from_registry(task_name: str, entry_point_key: str) -> dict | objec 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).kwargs.get(entry_point_key) + 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: + # get existing agents and algorithms + agents = collections.defaultdict(list) + for k in gym.spec(task_name.split(":")[-1]).kwargs: + if k.endswith("_cfg_entry_point") and k != "env_cfg_entry_point": + spec = ( + k.replace("_cfg_entry_point", "") + .replace("rl_games", "rl-games") + .replace("rsl_rl", "rsl-rl") + .split("_") + ) + agent = spec[0].replace("-", "_") + algorithms = [item.upper() for item in (spec[1:] if len(spec) > 1 else ["PPO"])] + agents[agent].extend(algorithms) + msg = "\nExisting RL library (and algorithms) config entry points: " + for agent, algorithms in agents.items(): + msg += f"\n |-- {agent}: {', '.join(algorithms)}" + # raise error 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}'." + f"\nPlease check that the gym registry has the entry point: '{entry_point_key}'." + f"{msg if agents else ''}" ) # parse the default config file if isinstance(cfg_entry_point, str) and cfg_entry_point.endswith(".yaml"): @@ -117,7 +138,7 @@ def parse_env_cfg( environment configuration. """ # load the default configuration - cfg = load_cfg_from_registry(task_name, "env_cfg_entry_point") + 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 diff --git a/source/isaaclab_tasks/setup.py b/source/isaaclab_tasks/setup.py index 5139281b6ec1..455c56689ce0 100644 --- a/source/isaaclab_tasks/setup.py +++ b/source/isaaclab_tasks/setup.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,8 +6,8 @@ """Installation script for the 'isaaclab_tasks' python package.""" import os -import toml +import toml from setuptools import setup # Obtain the extension data from the extension.toml file @@ -18,17 +18,16 @@ # Minimum dependencies required prior to installation INSTALL_REQUIRES = [ # generic - "numpy", - "torch==2.5.1", + "numpy<2", + "torch>=2.7", "torchvision>=0.14.1", # ensure compatibility with torch 1.13.1 - # 5.26.0 introduced a breaking change, so we restricted it for now. - # See issue https://github.com/tensorflow/tensorboard/issues/6808 for details. - "protobuf>=3.20.2, < 5.0.0", + "protobuf>=4.25.8,!=5.26.0", # basic logger "tensorboard", + "numba", ] -PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu118"] +PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu128"] # Installation operation setup( @@ -47,7 +46,10 @@ classifiers=[ "Natural Language :: English", "Programming Language :: Python :: 3.10", + "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/benchmarking/configs.yaml b/source/isaaclab_tasks/test/benchmarking/configs.yaml new file mode 100644 index 000000000000..d5df21551b73 --- /dev/null +++ b/source/isaaclab_tasks/test/benchmarking/configs.yaml @@ -0,0 +1,303 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# mode for very simple functional testing without checking thresholds +fast_test: + rl_games:Isaac-Ant-v0: + max_iterations: 10 + lower_thresholds: + reward: -99999 + episode_length: -99999 + upper_thresholds: + duration: 99999 + +# mode for capturing KPIs across all environments without checking thresholds +full_test: + Isaac-*: + max_iterations: 500 + lower_thresholds: + reward: -99999 + episode_length: -99999 + upper_thresholds: + duration: 99999 + +# mode for PR tests (default mode) +fast: + rl_games:Isaac-Ant-v0: + max_iterations: 200 + lower_thresholds: + reward: 45 + episode_length: 900 + upper_thresholds: + duration: 750 + skrl:Isaac-Cartpole-RGB-Camera-Direct-v0: + max_iterations: 50 + lower_thresholds: + reward: 190 + episode_length: 230 + upper_thresholds: + duration: 450 + rsl_rl:Isaac-Humanoid-v0: + max_iterations: 200 + lower_thresholds: + reward: 50 + episode_length: 750 + upper_thresholds: + duration: 500 + rl_games:Isaac-Quadcopter-Direct-v0: + max_iterations: 200 + lower_thresholds: + reward: 100 + episode_length: 400 + upper_thresholds: + duration: 250 + skrl:Isaac-Shadow-Hand-Over-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 30 + episode_length: 250 + upper_thresholds: + duration: 600 + rsl_rl:Isaac-Velocity-Rough-Anymal-C-v0: + max_iterations: 300 + lower_thresholds: + reward: 7 + episode_length: 900 + upper_thresholds: + duration: 1800 + +# mode for weekly CI +full: + Isaac-Ant-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 7000 + episode_length: 700 + upper_thresholds: + duration: 500 + Isaac-Ant-v0: + max_iterations: 1000 + lower_thresholds: + reward: 100 + episode_length: 700 + upper_thresholds: + duration: 800 + Isaac-Cart-Double-Pendulum-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 400 + episode_length: 150 + upper_thresholds: + duration: 500 + Isaac-Cartpole-Depth-Camera-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 200 + episode_length: 150 + upper_thresholds: + duration: 3000 + Isaac-Cartpole-Depth-v0: + max_iterations: 300 + lower_thresholds: + reward: 1 + episode_length: 150 + upper_thresholds: + duration: 3000 + Isaac-Cartpole-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 200 + episode_length: 150 + upper_thresholds: + duration: 500 + Isaac-Cartpole-RGB-Camera-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 200 + episode_length: 150 + upper_thresholds: + duration: 3000 + Isaac-Cartpole-RGB-ResNet18-v0: + max_iterations: 300 + lower_thresholds: + reward: 1 + episode_length: 100 + upper_thresholds: + duration: 4000 + Isaac-Cartpole-RGB-TheiaTiny-v0: + max_iterations: 300 + lower_thresholds: + reward: 1 + episode_length: 150 + upper_thresholds: + duration: 4000 + Isaac-Cartpole-RGB-v0: + max_iterations: 300 + lower_thresholds: + reward: -2 + episode_length: 150 + upper_thresholds: + duration: 4000 + Isaac-Cartpole-v0: + max_iterations: 1000 + lower_thresholds: + reward: 3 + episode_length: 150 + upper_thresholds: + duration: 1500 + Isaac-Factory-GearMesh-Direct-v0: + max_iterations: 100 + lower_thresholds: + reward: 200 + episode_length: 250 + upper_thresholds: + duration: 6000 + Isaac-Factory-NutThread-Direct-v0: + max_iterations: 100 + lower_thresholds: + reward: 400 + episode_length: 400 + upper_thresholds: + duration: 5000 + Isaac-Factory-PegInsert-Direct-v0: + max_iterations: 100 + lower_thresholds: + reward: 125 + episode_length: 130 + upper_thresholds: + duration: 4000 + Isaac-Franka-Cabinet-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 2000 + episode_length: 400 + upper_thresholds: + duration: 1000 + Isaac-Humanoid-Direct-v0: + max_iterations: 300 + lower_thresholds: + reward: 2000 + episode_length: 600 + upper_thresholds: + duration: 1000 + Isaac-Humanoid-v0: + max_iterations: 1000 + lower_thresholds: + reward: 100 + episode_length: 600 + upper_thresholds: + duration: 2500 + Isaac-Lift-Cube-Franka-v0: + max_iterations: 300 + lower_thresholds: + reward: 90 + episode_length: 100 + upper_thresholds: + duration: 1000 + Isaac-Navigation-Flat-Anymal-C-v0: + max_iterations: 300 + lower_thresholds: + reward: 0.5 + episode_length: 20 + upper_thresholds: + duration: 2000 + Isaac-Open-Drawer-Franka-v0: + max_iterations: 200 + lower_thresholds: + reward: 60 + episode_length: 150 + upper_thresholds: + duration: 3000 + Isaac-Quadcopter-Direct-v0: + max_iterations: 500 + lower_thresholds: + reward: 90 + episode_length: 300 + upper_thresholds: + duration: 500 + Isaac-Reach-Franka-*: + max_iterations: 1000 + lower_thresholds: + reward: 0.25 + episode_length: 150 + upper_thresholds: + duration: 1500 + Isaac-Reach-UR10-v0: + max_iterations: 1000 + lower_thresholds: + reward: 0.25 + episode_length: 150 + upper_thresholds: + duration: 1500 + Isaac-Repose-Cube-Allegro-Direct-v0: + max_iterations: 500 + lower_thresholds: + reward: 200 + episode_length: 150 + upper_thresholds: + duration: 1500 + Isaac-Repose-Cube-Allegro-*: + max_iterations: 500 + lower_thresholds: + reward: 15 + episode_length: 300 + upper_thresholds: + duration: 1500 + Isaac-Repose-Cube-Shadow-Direct-v0: + max_iterations: 3000 + lower_thresholds: + reward: 1000 + episode_length: 300 + upper_thresholds: + duration: 10000 + Isaac-Repose-Cube-Shadow-OpenAI-FF-Direct-v0: + max_iterations: 3000 + lower_thresholds: + reward: 1000 + episode_length: 50 + upper_thresholds: + duration: 15000 + Isaac-Repose-Cube-Shadow-OpenAI-LSTM-Direct-v0: + max_iterations: 3000 + lower_thresholds: + reward: 1000 + episode_length: 100 + upper_thresholds: + duration: 30000 + Isaac-Repose-Cube-Shadow-Vision-Direct-v0: + max_iterations: 3000 + lower_thresholds: + reward: 1000 + episode_length: 400 + upper_thresholds: + duration: 40000 + Isaac-Shadow-Hand-Over-Direct-v0: + max_iterations: 3000 + lower_thresholds: + reward: 1000 + episode_length: 150 + upper_thresholds: + duration: 10000 + Isaac-Velocity-Flat-*: + max_iterations: 1000 + lower_thresholds: + reward: 15 + episode_length: 700 + upper_thresholds: + duration: 3000 + Isaac-Velocity-Flat-Spot-v0: + max_iterations: 1000 + lower_thresholds: + reward: 150 + episode_length: 700 + upper_thresholds: + duration: 6000 + Isaac-Velocity-Rough-*: + max_iterations: 1000 + lower_thresholds: + reward: 7 + episode_length: 700 + upper_thresholds: + duration: 6000 diff --git a/source/isaaclab_tasks/test/benchmarking/conftest.py b/source/isaaclab_tasks/test/benchmarking/conftest.py new file mode 100644 index 000000000000..6a13b1898a52 --- /dev/null +++ b/source/isaaclab_tasks/test/benchmarking/conftest.py @@ -0,0 +1,105 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import json + +import pytest + +# Local imports should be imported last +import env_benchmark_test_utils as utils # isort: skip + +# Global variable for storing KPI data +GLOBAL_KPI_STORE = {} + + +def pytest_addoption(parser): + parser.addoption( + "--workflows", + action="store", + nargs="+", + default=["rl_games", "rsl_rl", "sb3", "skrl"], + help="List of workflows. Must be equal to or a subset of the default list.", + ) + parser.addoption( + "--config_path", + action="store", + default="configs.yaml", + help="Path to config file for environment training and evaluation.", + ) + parser.addoption( + "--mode", + action="store", + default="fast", + help="Coverage mode defined in the config file.", + ) + parser.addoption("--num_gpus", action="store", type=int, default=1, help="Number of GPUs for distributed training.") + parser.addoption( + "--save_kpi_payload", + action="store_true", + help="To collect output metrics into a KPI payload that can be uploaded to a dashboard.", + ) + parser.addoption( + "--tag", + action="store", + default="", + help="Optional tag to add to the KPI payload for filtering on the Grafana dashboard.", + ) + + +@pytest.fixture +def workflows(request): + return request.config.getoption("--workflows") + + +@pytest.fixture +def config_path(request): + return request.config.getoption("--config_path") + + +@pytest.fixture +def mode(request): + return request.config.getoption("--mode") + + +@pytest.fixture +def num_gpus(request): + return request.config.getoption("--num_gpus") + + +@pytest.fixture +def save_kpi_payload(request): + return request.config.getoption("--save_kpi_payload") + + +@pytest.fixture +def tag(request): + return request.config.getoption("--tag") + + +# Fixture for storing KPI data in a global variable +@pytest.fixture(scope="session") +def kpi_store(): + return GLOBAL_KPI_STORE # Using global variable for storing KPI data + + +# This hook dynamically generates test cases based on the --workflows option. +# For any test that includes a 'workflow' fixture, this will parametrize it +# with all values passed via the command line option --workflows. +def pytest_generate_tests(metafunc): + if "workflow" in metafunc.fixturenames: + workflows = metafunc.config.getoption("workflows") + metafunc.parametrize("workflow", workflows) + + +# The pytest session finish hook +def pytest_sessionfinish(session, exitstatus): + # Access global variable instead of fixture + tag = session.config.getoption("--tag") + utils.process_kpi_data(GLOBAL_KPI_STORE, tag=tag) + print(json.dumps(GLOBAL_KPI_STORE, indent=2)) + save_kpi_payload = session.config.getoption("--save_kpi_payload") + if save_kpi_payload: + print("Saving KPI data...") + utils.output_payloads(GLOBAL_KPI_STORE) diff --git a/source/isaaclab_tasks/test/benchmarking/env_benchmark_test_utils.py b/source/isaaclab_tasks/test/benchmarking/env_benchmark_test_utils.py new file mode 100644 index 000000000000..52dbeadda3a0 --- /dev/null +++ b/source/isaaclab_tasks/test/benchmarking/env_benchmark_test_utils.py @@ -0,0 +1,239 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import glob +import json +import math +import os +import re +from datetime import datetime + +import numpy as np +import yaml +from tensorboard.backend.event_processing import event_accumulator + +import carb + + +def get_env_configs(configs_path): + """Get environment configurations from yaml filepath.""" + with open(configs_path) as env_configs_file: + env_configs = yaml.safe_load(env_configs_file) + return env_configs + + +def get_env_config(env_configs, mode, workflow, task): + """Get the environment configuration.""" + if mode not in env_configs: + raise ValueError(f"Mode {mode} is not supported in the config file.") + + extended_task = f"{workflow}:{task}" + # return a direct match with extended task name + if extended_task in env_configs[mode]: + return env_configs[mode][extended_task] + + # else, return a direct match with task name + if task in env_configs[mode]: + return env_configs[mode][task] + + # else, return a regex match with extended task name + for env_config_key in env_configs[mode].keys(): + if re.match(env_config_key, extended_task): + return env_configs[mode][env_config_key] + + # else, return a regex match with task name + for env_config_key in env_configs[mode].keys(): + if re.match(env_config_key, task): + return env_configs[mode][env_config_key] + + # if no match is found, return None + return None + + +def evaluate_job(workflow, task, env_config, duration): + """Evaluate the job.""" + log_data = _retrieve_logs(workflow, task) + + kpi_payload = {"success": True, "msg": ""} + + # handle case where no log files are found + if not log_data: + kpi_payload["success"] = False + kpi_payload["msg"] = "error: training did not finish!" + return kpi_payload + + thresholds = {**env_config.get("lower_thresholds", {}), **env_config.get("upper_thresholds", {})} + + # evaluate all thresholds from the config + for threshold_name, threshold_val in thresholds.items(): + uses_lower_threshold = threshold_name in env_config.get("lower_thresholds", {}) + if threshold_name == "duration": + val = duration + else: + val = _extract_log_val(threshold_name, log_data, uses_lower_threshold, workflow) + # skip non-numeric values + if val is None or not isinstance(val, (int, float)) or (isinstance(val, float) and math.isnan(val)): + continue + val = round(val, 4) + if uses_lower_threshold: + # print(f"{threshold_name}: {val} > {round(threshold_val, 4)}") + if val < threshold_val: + kpi_payload["success"] = False + else: + # print(f"{threshold_name}: {val} < {round(threshold_val, 4)}") + if val > threshold_val: + kpi_payload["success"] = False + kpi_payload[threshold_name] = val + if threshold_name == "reward": + normalized_reward = val / threshold_val + kpi_payload[f"{threshold_name}_normalized"] = normalized_reward + kpi_payload[f"{threshold_name}_threshold"] = threshold_val + + # add max iterations to the payload + max_iterations = env_config.get("max_iterations") + if max_iterations is not None: + kpi_payload["max_iterations"] = max_iterations + + return kpi_payload + + +def process_kpi_data(kpi_payloads, tag=""): + """Combine and augment the KPI payloads.""" + # accumulate workflow outcomes + totals = {} + successes = {} + failures_did_not_finish = {} + failures_did_not_pass_thresholds = {} + for job_id, kpi_payload in kpi_payloads.items(): + workflow = job_id.split(":")[0] + if workflow not in totals: + totals[workflow] = 0 + successes[workflow] = 0 + failures_did_not_finish[workflow] = 0 + failures_did_not_pass_thresholds[workflow] = 0 + totals[workflow] += 1 + if kpi_payload["success"]: + successes[workflow] += 1 + else: + if kpi_payload["msg"] == "error: training did not finish!": + failures_did_not_finish[workflow] += 1 + else: + failures_did_not_pass_thresholds[workflow] += 1 + + kpi_payloads["overall"] = { + "totals": totals, + "successes": successes, + "failures_did_not_finish": failures_did_not_finish, + "failures_did_not_pass_thresholds": failures_did_not_pass_thresholds, + "timestamp": datetime.now().isoformat(), + "tag": tag, + } + + return kpi_payloads + + +def output_payloads(payloads): + """Output the KPI payloads to a json file.""" + # first grab all log files + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + output_path = os.path.join(repo_path, "logs/kpi.json") + # create directory if it doesn't exist + if not os.path.exists(os.path.dirname(output_path)): + os.makedirs(os.path.dirname(output_path)) + # save file + with open(output_path, "w") as payload_file: + json.dump(payloads, payload_file, indent=4) + + +def _retrieve_logs(workflow, task): + """Retrieve training logs.""" + # first grab all log files + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + from isaaclab.utils.version import get_isaac_sim_version + + if get_isaac_sim_version().major < 5: + repo_path = os.path.join(repo_path, "..") + if workflow == "rl_games": + log_files_path = os.path.join(repo_path, f"logs/{workflow}/{task}/*/summaries/*") + else: + log_files_path = os.path.join(repo_path, f"logs/{workflow}/{task}/*/*.tfevents.*") + log_files = glob.glob(log_files_path) + # handle case where no log files are found + if not log_files: + return None + # find most recent + latest_log_file = max(log_files, key=os.path.getctime) + # parse tf file into a dictionary + log_data = _parse_tf_logs(latest_log_file) + return log_data + + +def _parse_tf_logs(log): + """Parse the tensorflow filepath into a dictionary.""" + log_data = {} + ea = event_accumulator.EventAccumulator(log) + ea.Reload() + tags = ea.Tags()["scalars"] + for tag in tags: + log_data[tag] = [] + for event in ea.Scalars(tag): + log_data[tag].append((event.step, event.value)) + return log_data + + +def _extract_log_val(name, log_data, uses_lower_threshold, workflow): + """Extract the value from the log data.""" + try: + if name == "reward": + reward_tags = { + "rl_games": "rewards/iter", + "rsl_rl": "Train/mean_reward", + "sb3": None, # TODO: complete when sb3 is fixed + "skrl": "Reward / Total reward (mean)", + } + tag = reward_tags.get(workflow) + if tag: + return _extract_reward(log_data, tag) + + elif name == "episode_length": + episode_tags = { + "rl_games": "episode_lengths/iter", + "rsl_rl": "Train/mean_episode_length", + "sb3": None, # TODO: complete when sb3 is fixed + "skrl": "Episode / Total timesteps (mean)", + } + tag = episode_tags.get(workflow) + if tag: + return _extract_feature(log_data, tag, uses_lower_threshold) + + elif name == "training_time": + return {"rl_games": log_data["rewards/time"][-1][0], "rsl_rl": None, "sb3": None, "skrl": None}.get( + workflow + ) + except Exception: + return None + + raise ValueError(f"Env Config name {name} is not supported.") + + +def _extract_feature(log_data, feature, uses_lower_threshold): + """Extract the feature from the log data.""" + log_data = np.array(log_data[feature])[:, 1] + + if uses_lower_threshold: + return max(log_data) + else: + return min(log_data) + + +def _extract_reward(log_data, feature, k=8): + """Extract the averaged max reward from the log data.""" + log_data = np.array(log_data[feature])[:, 1] + + # find avg of k max values + k = min(len(log_data), k) + averaged_reward = np.mean(np.partition(log_data, -k)[-k:]) + + return averaged_reward diff --git a/source/isaaclab_tasks/test/benchmarking/test_environments_training.py b/source/isaaclab_tasks/test/benchmarking/test_environments_training.py new file mode 100644 index 000000000000..70fd562089a1 --- /dev/null +++ b/source/isaaclab_tasks/test/benchmarking/test_environments_training.py @@ -0,0 +1,118 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# Launch omniverse app +app_launcher = AppLauncher(headless=True, enable_cameras=True) +simulation_app = app_launcher.app + +import os +import subprocess +import sys +import time + +import env_benchmark_test_utils as utils +import gymnasium as gym +import pytest + +import carb + +from isaaclab_rl.utils.pretrained_checkpoint import WORKFLOW_EXPERIMENT_NAME_VARIABLE, WORKFLOW_TRAINER + + +def setup_environment(): + """Setup environment for testing.""" + # Acquire all Isaac environments names + registered_task_specs = [] + for task_spec in gym.registry.values(): + if "Isaac" in task_spec.id and not task_spec.id.endswith("Play-v0"): + registered_task_specs.append(task_spec) + + # Sort environments by name + registered_task_specs.sort(key=lambda x: x.id) + + # This flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + return registered_task_specs + + +def train_job(workflow, task, env_config, num_gpus): + """Train a single job for a given workflow, task, and configuration, and return the duration.""" + cmd = [ + sys.executable, + WORKFLOW_TRAINER[workflow], + "--task", + task, + "--enable_cameras", + "--headless", + ] + + # Add max iterations if specified + max_iterations = env_config.get("max_iterations") + if max_iterations is not None: + cmd.extend(["--max_iterations", str(max_iterations)]) + + if num_gpus > 1: + cmd.append(f"--nnprod_per_node={num_gpus}") + cmd.append("--distributed") + + # Add experiment name variable + cmd.append(f"{WORKFLOW_EXPERIMENT_NAME_VARIABLE[workflow]}={task}") + + print("Running : " + " ".join(cmd)) + + start_time = time.time() + subprocess.run(cmd) + duration = time.time() - start_time + + return duration + + +@pytest.mark.parametrize("task_spec", setup_environment()) +def test_train_environments(workflow, task_spec, config_path, mode, num_gpus, kpi_store): + """Train environments provided in the config file, save KPIs, and evaluate against thresholds""" + # Skip if workflow not supported for this task + if workflow + "_cfg_entry_point" not in task_spec.kwargs: + pytest.skip(f"Workflow {workflow} not supported for task {task_spec.id}") + + # Load environment config + task = task_spec.id + if config_path.startswith("/"): + full_config_path = config_path + else: + full_config_path = os.path.join(os.path.dirname(__file__), config_path) + env_configs = utils.get_env_configs(full_config_path) + env_config = utils.get_env_config(env_configs, mode, workflow, task) + + # Skip if config not found + if env_config is None: + pytest.skip(f"No config found for task {task} in {mode} mode") + + job_name = f"{workflow}:{task}" + print(f">>> Training: {job_name}") + + # Train and capture duration + duration = train_job(workflow, task, env_config, num_gpus) + + print(f">>> Evaluating trained: {job_name}") + # Check if training logs were output and all thresholds passed + kpi_payload = utils.evaluate_job(workflow, task, env_config, duration) + + success_flag = kpi_payload["success"] + print(f">>> Trained {job_name} success flag: {success_flag}.") + print("-" * 80) + + # Save KPI + kpi_store[job_name] = kpi_payload + + # Verify job was successful + if not kpi_payload["success"]: + pytest.fail(f"Job {job_name} failed to meet success criteria") diff --git a/source/isaaclab_tasks/test/env_test_utils.py b/source/isaaclab_tasks/test/env_test_utils.py new file mode 100644 index 000000000000..b6f0383abee1 --- /dev/null +++ b/source/isaaclab_tasks/test/env_test_utils.py @@ -0,0 +1,283 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Shared test utilities for Isaac Lab environments.""" + +import inspect +import os + +import gymnasium as gym +import pytest +import torch + +import carb +import omni.usd + +from isaaclab.envs.utils.spaces import sample_space +from isaaclab.utils.version import get_isaac_sim_version + +from isaaclab_tasks.utils.parse_cfg import parse_env_cfg + + +def setup_environment( + include_play: bool = False, + factory_envs: bool | None = None, + multi_agent: bool | None = None, +) -> list[str]: + """ + Acquire all registered Isaac environment task IDs with optional filters. + + Args: + include_play: If True, include environments ending in 'Play-v0'. + factory_envs: + - True: include only Factory environments + - False: exclude Factory environments + - None: include both Factory and non-Factory environments + multi_agent: + - True: include only multi-agent environments + - False: include only single-agent environments + - None: include all environments regardless of agent type + + Returns: + A sorted list of task IDs matching the selected filters. + """ + # disable interactive mode for wandb for automate environments + os.environ["WANDB_DISABLED"] = "true" + + # acquire all Isaac environment names + registered_tasks = [] + for task_spec in gym.registry.values(): + # only consider Isaac environments + if "Isaac" not in task_spec.id: + continue + + # filter Play environments, if needed + if not include_play and task_spec.id.endswith("Play-v0"): + continue + + # TODO: factory environments cause tests to fail if run together with other envs, + # so we collect these environments separately to run in a separate unit test. + # apply factory filter + if (factory_envs is True and ("Factory" not in task_spec.id and "Forge" not in task_spec.id)) or ( + factory_envs is False and ("Factory" in task_spec.id or "Forge" in task_spec.id) + ): + continue + # if None: no filter + + # apply multi agent filter + if multi_agent is not None: + # parse config + env_cfg = parse_env_cfg(task_spec.id) + if (multi_agent is True and not hasattr(env_cfg, "possible_agents")) or ( + multi_agent is False and hasattr(env_cfg, "possible_agents") + ): + continue + # if None: no filter + + registered_tasks.append(task_spec.id) + + # sort environments alphabetically + registered_tasks.sort() + + # this flag is necessary to prevent a bug where the simulation gets stuck randomy when running many environments + carb.settings.get_settings().set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + print(">>> All registered environments:", registered_tasks) + + return registered_tasks + + +def _run_environments( + task_name, + device, + num_envs, + num_steps=100, + multi_agent=False, + create_stage_in_memory=False, + disable_clone_in_fabric=False, +): + """Run all environments and check environments return valid signals. + + Args: + task_name: Name of the environment. + device: Device to use (e.g., 'cuda'). + num_envs: Number of environments. + num_steps: Number of simulation steps. + multi_agent: Whether the environment is multi-agent. + create_stage_in_memory: Whether to create stage in memory. + disable_clone_in_fabric: Whether to disable fabric cloning. + """ + + # skip test if stage in memory is not supported + if get_isaac_sim_version().major < 5 and create_stage_in_memory: + pytest.skip("Stage in memory is not supported in this version of Isaac Sim") + + # skip suction gripper environments as they require CPU simulation and cannot be run with GPU simulation + if "Suction" in task_name and device != "cpu": + return + + # skip these environments as they cannot be run with 32 environments within reasonable VRAM + if num_envs == 32 and task_name in [ + "Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0", + "Isaac-Stack-Cube-Instance-Randomize-Franka-IK-Rel-v0", + "Isaac-Stack-Cube-Instance-Randomize-Franka-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 + + # Check if this is the teddy bear environment and if it's being called from the right test file + if task_name == "Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0": + # Get the calling frame to check which test file is calling this function + frame = inspect.currentframe() + while frame: + filename = frame.f_code.co_filename + if "test_lift_teddy_bear.py" in filename: + # Called from the dedicated test file, allow it to run + break + frame = frame.f_back + + # If not called from the dedicated test file, skip it + if not frame: + return + + print(f""">>> Running test for environment: {task_name}""") + _check_random_actions( + task_name, + device, + num_envs, + num_steps=num_steps, + multi_agent=multi_agent, + create_stage_in_memory=create_stage_in_memory, + disable_clone_in_fabric=disable_clone_in_fabric, + ) + print(f""">>> Closing environment: {task_name}""") + print("-" * 80) + + +def _check_random_actions( + task_name: str, + device: str, + num_envs: int, + num_steps: int = 100, + multi_agent: bool = False, + create_stage_in_memory: bool = False, + disable_clone_in_fabric: bool = False, +): + """Run random actions and check environments return valid signals. + + Args: + task_name: Name of the environment. + device: Device to use (e.g., 'cuda'). + num_envs: Number of environments. + num_steps: Number of simulation steps. + multi_agent: Whether the environment is multi-agent. + create_stage_in_memory: Whether to create stage in memory. + disable_clone_in_fabric: Whether to disable fabric cloning. + """ + # create a new context stage, if stage in memory is not enabled + if not create_stage_in_memory: + omni.usd.get_context().new_stage() + + # reset the rtx sensors carb setting to False + carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + try: + # parse config + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # set config args + env_cfg.sim.create_stage_in_memory = create_stage_in_memory + if disable_clone_in_fabric: + env_cfg.scene.clone_in_fabric = False + + # filter based off multi agents mode and create env + if multi_agent: + if not hasattr(env_cfg, "possible_agents"): + print(f"[INFO]: Skipping {task_name} as it is not a multi-agent task") + return + env = gym.make(task_name, cfg=env_cfg) + else: + if hasattr(env_cfg, "possible_agents"): + print(f"[INFO]: Skipping {task_name} as it is a multi-agent task") + return + env = gym.make(task_name, cfg=env_cfg) + + except Exception as e: + # try to close environment on exception + if "env" in locals() and hasattr(env, "_is_closed"): + env.close() + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") + + # disable control on stop + env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore + + # override action space if set to inf for `Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0` + if task_name == "Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0": + for i in range(env.unwrapped.single_action_space.shape[0]): + if env.unwrapped.single_action_space.low[i] == float("-inf"): + env.unwrapped.single_action_space.low[i] = -1.0 + if env.unwrapped.single_action_space.high[i] == float("inf"): + env.unwrapped.single_action_space.low[i] = 1.0 + + # reset environment + obs, _ = env.reset() + + # check signal + assert _check_valid_tensor(obs) + + # simulate environment for num_steps + with torch.inference_mode(): + for _ in range(num_steps): + # sample actions according to the defined space + if multi_agent: + actions = { + agent: sample_space( + env.unwrapped.action_spaces[agent], device=env.unwrapped.device, batch_size=num_envs + ) + for agent in env.unwrapped.possible_agents + } + else: + actions = sample_space( + env.unwrapped.single_action_space, device=env.unwrapped.device, batch_size=num_envs + ) + # apply actions + transition = env.step(actions) + # check signals + for data in transition[:-1]: # exclude info + if multi_agent: + for agent, agent_data in data.items(): + assert _check_valid_tensor(agent_data), f"Invalid data ('{agent}'): {agent_data}" + else: + assert _check_valid_tensor(data), f"Invalid data: {data}" + + # close environment + env.close() + + +def _check_valid_tensor(data: torch.Tensor | dict) -> bool: + """Checks if given data does not have corrupted values. + + Args: + data: Data buffer. + + Returns: + True if the data is valid. + """ + if isinstance(data, torch.Tensor): + return not torch.any(torch.isnan(data)) + elif isinstance(data, (tuple, list)): + return all(_check_valid_tensor(value) for value in data) + elif isinstance(data, dict): + return all(_check_valid_tensor(value) for value in data.values()) + else: + raise ValueError(f"Input data of invalid type: {type(data)}.") diff --git a/source/isaaclab_tasks/test/test_environment_determinism.py b/source/isaaclab_tasks/test/test_environment_determinism.py index 6902607cc29d..52ac1f34980e 100644 --- a/source/isaaclab_tasks/test/test_environment_determinism.py +++ b/source/isaaclab_tasks/test/test_environment_determinism.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True) @@ -15,8 +15,8 @@ """Rest everything follows.""" import gymnasium as gym +import pytest import torch -import unittest import carb import omni.usd @@ -25,115 +25,104 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -class TestEnvironmentDeterminism(unittest.TestCase): - """Test cases for environment determinism. - - We make separate test cases for manipulation, locomotion, and dextrous manipulation environments. - This is because each of these environments has different simulation dynamics and different types of actions. - The test cases are run for different devices and seeds to ensure that the environment creation is deterministic. - """ - - @classmethod - def setUpClass(cls): - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - """ - Test fixtures. - """ - - def test_manipulation_env_determinism(self): - """Check deterministic environment creation for manipulation.""" - for task_name in [ - "Isaac-Open-Drawer-Franka-v0", - "Isaac-Lift-Cube-Franka-v0", - ]: - for device in ["cuda", "cpu"]: - with self.subTest(task_name=task_name, device=device): - self._test_environment_determinism(task_name, device) - - def test_locomotion_env_determinism(self): - """Check deterministic environment creation for locomotion.""" - for task_name in [ - "Isaac-Velocity-Flat-Anymal-C-v0", - "Isaac-Velocity-Rough-Anymal-C-v0", - "Isaac-Velocity-Rough-Anymal-C-Direct-v0", - ]: - for device in ["cuda", "cpu"]: - with self.subTest(task_name=task_name, device=device): - self._test_environment_determinism(task_name, device) - - def test_dextrous_env_determinism(self): - """Check deterministic environment creation for dextrous manipulation.""" - for task_name in [ - "Isaac-Repose-Cube-Allegro-v0", - # "Isaac-Repose-Cube-Allegro-Direct-v0", # FIXME: @kellyg, any idea why it is not deterministic? - ]: - for device in ["cuda", "cpu"]: - with self.subTest(task_name=task_name, device=device): - self._test_environment_determinism(task_name, device) - - """ - Helper functions. - """ - - def _test_environment_determinism(self, task_name: str, device: str): - """Check deterministic environment creation.""" - # fix number of steps - num_envs = 32 - num_steps = 100 - # call function to create and step the environment - obs_1, rew_1 = self._obtain_transition_tuples(task_name, num_envs, device, num_steps) - obs_2, rew_2 = self._obtain_transition_tuples(task_name, num_envs, device, num_steps) - - # check everything is as expected - # -- rewards should be the same - torch.testing.assert_close(rew_1, rew_2) - # -- observations should be the same - for key in obs_1.keys(): - torch.testing.assert_close(obs_1[key], obs_2[key]) - - def _obtain_transition_tuples( - self, task_name: str, num_envs: int, device: str, num_steps: int - ) -> tuple[dict, torch.Tensor]: - """Run random actions and obtain transition tuples after fixed number of steps.""" - # create a new stage - omni.usd.get_context().new_stage() - try: - # parse configuration - env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) - # set seed - env_cfg.seed = 42 - # create environment - env = gym.make(task_name, cfg=env_cfg) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # disable control on stop - env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore - - # reset environment - obs, _ = env.reset() - # simulate environment for fixed steps - with torch.inference_mode(): - for _ in range(num_steps): - # sample actions from -1 to 1 - actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 - # apply actions and get initial observation - obs, rewards = env.step(actions)[:2] - - # close the environment - env.close() - - return obs, rewards - - -if __name__ == "__main__": - run_tests() +@pytest.fixture(scope="module", autouse=True) +def setup_environment(): + # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the + # test on many environments. + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) + + +@pytest.mark.parametrize( + "task_name", + [ + "Isaac-Open-Drawer-Franka-v0", + "Isaac-Lift-Cube-Franka-v0", + ], +) +@pytest.mark.parametrize("device", ["cuda", "cpu"]) +def test_manipulation_env_determinism(task_name, device): + """Check deterministic environment creation for manipulation.""" + _test_environment_determinism(task_name, device) + + +@pytest.mark.parametrize( + "task_name", + [ + "Isaac-Velocity-Flat-Anymal-C-v0", + "Isaac-Velocity-Rough-Anymal-C-v0", + "Isaac-Velocity-Rough-Anymal-C-Direct-v0", + ], +) +@pytest.mark.parametrize("device", ["cuda", "cpu"]) +def test_locomotion_env_determinism(task_name, device): + """Check deterministic environment creation for locomotion.""" + _test_environment_determinism(task_name, device) + + +@pytest.mark.parametrize( + "task_name", + [ + "Isaac-Repose-Cube-Allegro-v0", + # "Isaac-Repose-Cube-Allegro-Direct-v0", # FIXME: @kellyg, any idea why it is not deterministic? + ], +) +@pytest.mark.parametrize("device", ["cuda", "cpu"]) +def test_dextrous_env_determinism(task_name, device): + """Check deterministic environment creation for dextrous manipulation.""" + _test_environment_determinism(task_name, device) + + +def _test_environment_determinism(task_name: str, device: str): + """Check deterministic environment creation.""" + # fix number of steps + num_envs = 32 + num_steps = 100 + # call function to create and step the environment + obs_1, rew_1 = _obtain_transition_tuples(task_name, num_envs, device, num_steps) + obs_2, rew_2 = _obtain_transition_tuples(task_name, num_envs, device, num_steps) + + # check everything is as expected + # -- rewards should be the same + torch.testing.assert_close(rew_1, rew_2) + # -- observations should be the same + for key in obs_1.keys(): + torch.testing.assert_close(obs_1[key], obs_2[key]) + + +def _obtain_transition_tuples(task_name: str, num_envs: int, device: str, num_steps: int) -> tuple[dict, torch.Tensor]: + """Run random actions and obtain transition tuples after fixed number of steps.""" + # create a new stage + omni.usd.get_context().new_stage() + try: + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + # set seed + env_cfg.seed = 42 + # create environment + env = gym.make(task_name, cfg=env_cfg) + except Exception as e: + if "env" in locals() and hasattr(env, "_is_closed"): + env.close() + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") + + # disable control on stop + env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore + + # reset environment + obs, _ = env.reset() + # simulate environment for fixed steps + with torch.inference_mode(): + for _ in range(num_steps): + # sample actions from -1 to 1 + actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 + # apply actions and get initial observation + obs, rewards = env.step(actions)[:2] + + # close the environment + env.close() + + return obs, rewards diff --git a/source/isaaclab_tasks/test/test_environments.py b/source/isaaclab_tasks/test/test_environments.py index be140d982b9e..879948f9d9a8 100644 --- a/source/isaaclab_tasks/test/test_environments.py +++ b/source/isaaclab_tasks/test/test_environments.py @@ -1,11 +1,19 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +import sys + +# 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 +if sys.platform != "win32": + import pinocchio # noqa: F401 + +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -14,152 +22,17 @@ """Rest everything follows.""" -import gymnasium as gym -import torch -import unittest - -import carb -import omni.usd - -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.envs.utils.spaces import sample_space +import pytest import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils.parse_cfg import parse_env_cfg - - -class TestEnvironments(unittest.TestCase): - """Test cases for all registered environments.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - # TODO: Factory environments causes test to fail if run together with other envs - if "Isaac" in task_spec.id and not task_spec.id.endswith("Play-v0") and "Factory" not in task_spec.id: - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - """ - Test fixtures. - """ - - def test_multiple_num_envs_on_gpu(self): - """Run all environments with multiple instances and check environments return valid signals.""" - # common parameters - num_envs = 32 - device = "cuda" - # iterate over all registered environments - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # check environment - self._check_random_actions(task_name, device, num_envs, num_steps=100) - # close the environment - print(f">>> Closing environment: {task_name}") - print("-" * 80) - - def test_single_env_on_gpu(self): - """Run all environments with single instance and check environments return valid signals.""" - # common parameters - num_envs = 1 - device = "cuda" - # iterate over all registered environments - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # check environment - self._check_random_actions(task_name, device, num_envs, num_steps=100) - # close the environment - print(f">>> Closing environment: {task_name}") - print("-" * 80) - - """ - Helper functions. - """ - - def _check_random_actions(self, task_name: str, device: str, num_envs: int, num_steps: int = 1000): - """Run random actions and check environments returned signals are valid.""" - # create a new stage - omni.usd.get_context().new_stage() - # reset the rtx sensors carb setting to False - carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) - try: - # parse configuration - env_cfg: ManagerBasedRLEnvCfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) - - # skip test if the environment is a multi-agent task - if hasattr(env_cfg, "possible_agents"): - print(f"[INFO]: Skipping {task_name} as it is a multi-agent task") - return - - # create environment - env = gym.make(task_name, cfg=env_cfg) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # disable control on stop - env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore - - # override action space if set to inf for `Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0` - if task_name == "Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0": - for i in range(env.unwrapped.single_action_space.shape[0]): - if env.unwrapped.single_action_space.low[i] == float("-inf"): - env.unwrapped.single_action_space.low[i] = -1.0 - if env.unwrapped.single_action_space.high[i] == float("inf"): - env.unwrapped.single_action_space.low[i] = 1.0 - - # reset environment - obs, _ = env.reset() - # check signal - self.assertTrue(self._check_valid_tensor(obs)) - # simulate environment for num_steps steps - with torch.inference_mode(): - for _ in range(num_steps): - # sample actions according to the defined space - actions = sample_space( - env.unwrapped.single_action_space, device=env.unwrapped.device, batch_size=num_envs - ) - # apply actions - transition = env.step(actions) - # check signals - for data in transition[:-1]: # exclude info - self.assertTrue(self._check_valid_tensor(data), msg=f"Invalid data: {data}") - - # close the environment - env.close() - - @staticmethod - def _check_valid_tensor(data: torch.Tensor | dict) -> bool: - """Checks if given data does not have corrupted values. - - Args: - data: Data buffer. - Returns: - True if the data is valid. - """ - if isinstance(data, torch.Tensor): - return not torch.any(torch.isnan(data)) - elif isinstance(data, (tuple, list)): - return all(TestEnvironments._check_valid_tensor(value) for value in data) - elif isinstance(data, dict): - return all(TestEnvironments._check_valid_tensor(value) for value in data.values()) - else: - raise ValueError(f"Input data of invalid type: {type(data)}.") +# Local imports should be imported last +from env_test_utils import _run_environments, setup_environment # isort: skip -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("num_envs, device", [(32, "cuda"), (1, "cuda")]) +@pytest.mark.parametrize("task_name", setup_environment(include_play=False, factory_envs=False, multi_agent=False)) +@pytest.mark.isaacsim_ci +def test_environments(task_name, num_envs, device): + # run environments without stage in memory + _run_environments(task_name, device, num_envs, create_stage_in_memory=False) diff --git a/source/isaaclab_tasks/test/test_environments_with_stage_in_memory.py b/source/isaaclab_tasks/test/test_environments_with_stage_in_memory.py new file mode 100644 index 000000000000..8a99436b91c7 --- /dev/null +++ b/source/isaaclab_tasks/test/test_environments_with_stage_in_memory.py @@ -0,0 +1,58 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +import sys + +# 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 +if sys.platform != "win32": + import pinocchio # noqa: F401 + +from isaaclab.app import AppLauncher + +# launch the simulator +app_launcher = AppLauncher(headless=True, enable_cameras=True) +simulation_app = app_launcher.app + +from isaaclab.utils.version import get_isaac_sim_version + +"""Rest everything follows.""" + +import pytest + +import isaaclab_tasks # noqa: F401 + +# Local imports should be imported last +from env_test_utils import _run_environments, setup_environment # isort: skip + + +# note, running an env test without stage in memory then +# running an env test with stage in memory causes IsaacLab to hang. +# so, here we run all envs with stage in memory separately + +# TODO(mtrepte): re-enable with fabric cloning fix +# @pytest.mark.parametrize("num_envs, device", [(2, "cuda")]) +# @pytest.mark.parametrize("task_name", setup_environment(include_play=False,factory_envs=False, multi_agent=False)) +# def test_environments_with_stage_in_memory_and_clone_in_fabric_disabled(task_name, num_envs, device): +# # skip test if stage in memory is not supported +# if get_isaac_sim_version().major < 5: +# pytest.skip("Stage in memory is not supported in this version of Isaac Sim") + +# # run environments with stage in memory +# _run_environments(task_name, device, num_envs, create_stage_in_memory=True) + + +@pytest.mark.parametrize("num_envs, device", [(2, "cuda")]) +@pytest.mark.parametrize("task_name", setup_environment(include_play=False, factory_envs=False, multi_agent=False)) +def test_environments_with_stage_in_memory_and_clone_in_fabric_disabled(task_name, num_envs, device): + # skip test if stage in memory is not supported + if get_isaac_sim_version().major < 5: + pytest.skip("Stage in memory is not supported in this version of Isaac Sim") + + # run environments with stage in memory + _run_environments(task_name, device, num_envs, create_stage_in_memory=True, disable_clone_in_fabric=True) diff --git a/source/isaaclab_tasks/test/test_factory_environments.py b/source/isaaclab_tasks/test/test_factory_environments.py index e9d6d2b8211d..059080006557 100644 --- a/source/isaaclab_tasks/test/test_factory_environments.py +++ b/source/isaaclab_tasks/test/test_factory_environments.py @@ -1,164 +1,32 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) simulation_app = app_launcher.app - """Rest everything follows.""" -import gymnasium as gym -import torch -import unittest - -import carb -import omni.usd - -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.envs.utils.spaces import sample_space +import pytest import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils.parse_cfg import parse_env_cfg - - -class TestEnvironments(unittest.TestCase): - """Test cases for all registered environments.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id and not task_spec.id.endswith("Play-v0") and "Factory" in task_spec.id: - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - - """ - Test fixtures. - """ - - def test_multiple_num_envs_on_gpu(self): - """Run all environments with multiple instances and check environments return valid signals.""" - # common parameters - num_envs = 32 - device = "cuda" - # iterate over all registered environments - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # check environment - self._check_random_actions(task_name, device, num_envs, num_steps=100) - # close the environment - print(f">>> Closing environment: {task_name}") - print("-" * 80) - - def test_single_env_on_gpu(self): - """Run all environments with single instance and check environments return valid signals.""" - # common parameters - num_envs = 1 - device = "cuda" - # iterate over all registered environments - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # check environment - self._check_random_actions(task_name, device, num_envs, num_steps=100) - # close the environment - print(f">>> Closing environment: {task_name}") - print("-" * 80) - - """ - Helper functions. - """ - - def _check_random_actions(self, task_name: str, device: str, num_envs: int, num_steps: int = 1000): - """Run random actions and check environments returned signals are valid.""" - # create a new stage - omni.usd.get_context().new_stage() - # reset the rtx sensors carb setting to False - carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) - try: - # parse configuration - env_cfg: ManagerBasedRLEnvCfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) - - # skip test if the environment is a multi-agent task - if hasattr(env_cfg, "possible_agents"): - print(f"[INFO]: Skipping {task_name} as it is a multi-agent task") - return - - # create environment - env = gym.make(task_name, cfg=env_cfg) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # disable control on stop - env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore - - # override action space if set to inf for `Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0` - if task_name == "Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0": - for i in range(env.unwrapped.single_action_space.shape[0]): - if env.unwrapped.single_action_space.low[i] == float("-inf"): - env.unwrapped.single_action_space.low[i] = -1.0 - if env.unwrapped.single_action_space.high[i] == float("inf"): - env.unwrapped.single_action_space.low[i] = 1.0 - - # reset environment - obs, _ = env.reset() - # check signal - self.assertTrue(self._check_valid_tensor(obs)) - # simulate environment for num_steps steps - with torch.inference_mode(): - for _ in range(num_steps): - # sample actions according to the defined space - actions = sample_space( - env.unwrapped.single_action_space, device=env.unwrapped.device, batch_size=num_envs - ) - # apply actions - transition = env.step(actions) - # check signals - for data in transition[:-1]: # exclude info - self.assertTrue(self._check_valid_tensor(data), msg=f"Invalid data: {data}") - - # close the environment - env.close() - - @staticmethod - def _check_valid_tensor(data: torch.Tensor | dict) -> bool: - """Checks if given data does not have corrupted values. - - Args: - data: Data buffer. - Returns: - True if the data is valid. - """ - if isinstance(data, torch.Tensor): - return not torch.any(torch.isnan(data)) - elif isinstance(data, (tuple, list)): - return all(TestEnvironments._check_valid_tensor(value) for value in data) - elif isinstance(data, dict): - return all(TestEnvironments._check_valid_tensor(value) for value in data.values()) - else: - raise ValueError(f"Input data of invalid type: {type(data)}.") +# Local imports should be imported last +from env_test_utils import _check_random_actions, setup_environment # isort: skip -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("num_envs, device", [(32, "cuda"), (1, "cuda")]) +@pytest.mark.parametrize("task_name", setup_environment(factory_envs=True, multi_agent=False)) +@pytest.mark.isaacsim_ci +def test_factory_environments(task_name, num_envs, device): + """Run all factory environments and check environments return valid signals.""" + print(f">>> Running test for environment: {task_name}") + _check_random_actions(task_name, device, num_envs) + print(f">>> Closing environment: {task_name}") + print("-" * 80) diff --git a/source/isaaclab_tasks/test/test_hydra.py b/source/isaaclab_tasks/test/test_hydra.py index a6633e107c2b..5c81cb3e650f 100644 --- a/source/isaaclab_tasks/test/test_hydra.py +++ b/source/isaaclab_tasks/test/test_hydra.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -7,7 +7,7 @@ import sys -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True) @@ -17,7 +17,6 @@ """Rest everything follows.""" import functools -import unittest from collections.abc import Callable import hydra @@ -62,51 +61,45 @@ def wrapper(*args, **kwargs): return decorator -class TestHydra(unittest.TestCase): - """Test the Hydra configuration system.""" - - def test_hydra(self): - """Test the hydra configuration system.""" - - # set hardcoded command line arguments - sys.argv = [ - sys.argv[0], - "env.decimation=42", # test simple env modification - "env.events.physics_material.params.asset_cfg.joint_ids='slice(0 ,1, 2)'", # test slice setting - "env.scene.robot.init_state.joint_vel={.*: 4.0}", # test regex setting - "env.rewards.feet_air_time=null", # test setting to none - "agent.max_iterations=3", # test simple agent modification - ] - - @hydra_task_config_test("Isaac-Velocity-Flat-H1-v0", "rsl_rl_cfg_entry_point") - def main(env_cfg, agent_cfg, self): - # env - self.assertEqual(env_cfg.decimation, 42) - self.assertEqual(env_cfg.events.physics_material.params["asset_cfg"].joint_ids, slice(0, 1, 2)) - self.assertEqual(env_cfg.scene.robot.init_state.joint_vel, {".*": 4.0}) - self.assertIsNone(env_cfg.rewards.feet_air_time) - # agent - self.assertEqual(agent_cfg.max_iterations, 3) - - main(self) - # clean up - sys.argv = [sys.argv[0]] - hydra.core.global_hydra.GlobalHydra.instance().clear() - - def test_nested_iterable_dict(self): - """Test the hydra configuration system when dict is nested in an Iterable.""" - - @hydra_task_config_test("Isaac-Lift-Cube-Franka-v0", "rsl_rl_cfg_entry_point") - def main(env_cfg, agent_cfg, self): - # env - self.assertEqual(env_cfg.scene.ee_frame.target_frames[0].name, "end_effector") - self.assertEqual(env_cfg.scene.ee_frame.target_frames[0].offset.pos[2], 0.1034) - - main(self) - # clean up - sys.argv = [sys.argv[0]] - hydra.core.global_hydra.GlobalHydra.instance().clear() - - -if __name__ == "__main__": - run_tests() +def test_hydra(): + """Test the hydra configuration system.""" + + # set hardcoded command line arguments + sys.argv = [ + sys.argv[0], + "env.decimation=42", # test simple env modification + "env.events.physics_material.params.asset_cfg.joint_ids='slice(0 ,1, 2)'", # test slice setting + "env.scene.robot.init_state.joint_vel={.*: 4.0}", # test regex setting + "env.rewards.feet_air_time=null", # test setting to none + "agent.max_iterations=3", # test simple agent modification + ] + + @hydra_task_config_test("Isaac-Velocity-Flat-H1-v0", "rsl_rl_cfg_entry_point") + def main(env_cfg, agent_cfg): + # env + assert env_cfg.decimation == 42 + assert env_cfg.events.physics_material.params["asset_cfg"].joint_ids == slice(0, 1, 2) + assert env_cfg.scene.robot.init_state.joint_vel == {".*": 4.0} + assert env_cfg.rewards.feet_air_time is None + # agent + assert agent_cfg.max_iterations == 3 + + main() + # clean up + sys.argv = [sys.argv[0]] + hydra.core.global_hydra.GlobalHydra.instance().clear() + + +def test_nested_iterable_dict(): + """Test the hydra configuration system when dict is nested in an Iterable.""" + + @hydra_task_config_test("Isaac-Lift-Cube-Franka-v0", "rsl_rl_cfg_entry_point") + def main(env_cfg, agent_cfg): + # env + assert env_cfg.scene.ee_frame.target_frames[0].name == "end_effector" + assert env_cfg.scene.ee_frame.target_frames[0].offset.pos[2] == 0.1034 + + main() + # clean up + sys.argv = [sys.argv[0]] + hydra.core.global_hydra.GlobalHydra.instance().clear() diff --git a/source/isaaclab_tasks/test/test_lift_teddy_bear.py b/source/isaaclab_tasks/test/test_lift_teddy_bear.py new file mode 100644 index 000000000000..e131e0357498 --- /dev/null +++ b/source/isaaclab_tasks/test/test_lift_teddy_bear.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +import sys + +# 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 +if sys.platform != "win32": + import pinocchio # noqa: F401 + +from isaaclab.app import AppLauncher + +# launch the simulator with specific settings for teddy bear environment +app_launcher = AppLauncher( + headless=True, enable_cameras=False, kit_args='--/app/extensions/excluded=["omni.usd.metrics.assembler.ui"]' +) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import pytest + +import isaaclab_tasks # noqa: F401 + +# Local imports should be imported last +from env_test_utils import _run_environments # isort: skip + + +@pytest.mark.parametrize("num_envs, device", [(32, "cuda"), (1, "cuda")]) +def test_lift_teddy_bear_environment(num_envs, device): + """Test the Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0 environment in isolation.""" + task_name = "Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0" + + # Try to run the environment with specific settings for this problematic case + try: + _run_environments(task_name, device, num_envs, create_stage_in_memory=False) + except Exception as e: + # If it still fails, skip the test with a descriptive message + pytest.skip(f"Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0 environment failed to load: {e}") diff --git a/source/isaaclab_tasks/test/test_multi_agent_environments.py b/source/isaaclab_tasks/test/test_multi_agent_environments.py index 3f4380d0ead3..478cb3942e10 100644 --- a/source/isaaclab_tasks/test/test_multi_agent_environments.py +++ b/source/isaaclab_tasks/test/test_multi_agent_environments.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -14,145 +14,21 @@ """Rest everything follows.""" -import gymnasium as gym -import torch -import unittest - -import omni.usd - -from isaaclab.envs import DirectMARLEnv, DirectMARLEnvCfg -from isaaclab.envs.utils.spaces import sample_space +import pytest import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils.parse_cfg import parse_env_cfg - - -class TestEnvironments(unittest.TestCase): - """Test cases for all registered multi-agent environments.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id and not task_spec.id.endswith("Play-v0"): - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - # print all existing task names - print(">>> All registered environments:", cls.registered_tasks) - - """ - Test fixtures. - """ - - def test_multiple_instances_gpu(self): - """Run all environments with multiple instances and check environments return valid signals.""" - # common parameters - num_envs = 32 - device = "cuda" - # iterate over all registered environments - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # check environment - self._check_random_actions(task_name, device, num_envs, num_steps=100) - # close the environment - print(f">>> Closing environment: {task_name}") - print("-" * 80) - - def test_single_instance_gpu(self): - """Run all environments with single instance and check environments return valid signals.""" - # common parameters - num_envs = 1 - device = "cuda" - # iterate over all registered environments - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # check environment - self._check_random_actions(task_name, device, num_envs, num_steps=100) - # close the environment - print(f">>> Closing environment: {task_name}") - print("-" * 80) - - """ - Helper functions. - """ - - def _check_random_actions(self, task_name: str, device: str, num_envs: int, num_steps: int = 1000): - """Run random actions and check environments return valid signals.""" - # create a new stage - omni.usd.get_context().new_stage() - try: - # parse configuration - env_cfg: DirectMARLEnvCfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) - - # skip test if the environment is not a multi-agent task - if not hasattr(env_cfg, "possible_agents"): - print(f"[INFO]: Skipping {task_name} as it is not a multi-agent task") - return - - # create environment - env: DirectMARLEnv = gym.make(task_name, cfg=env_cfg) - except Exception as e: - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - self.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") - - # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the - # test on many environments. - env.unwrapped.sim.set_setting("/physics/cooking/ujitsoCollisionCooking", False) - - # avoid shutdown of process on simulation stop - env.unwrapped.sim._app_control_on_stop_handle = None - - # reset environment - obs, _ = env.reset() - # check signal - self.assertTrue(self._check_valid_tensor(obs)) - # simulate environment for num_steps steps - with torch.inference_mode(): - for _ in range(num_steps): - # sample actions according to the defined space - actions = { - agent: sample_space( - env.unwrapped.action_spaces[agent], device=env.unwrapped.device, batch_size=num_envs - ) - for agent in env.unwrapped.possible_agents - } - # apply actions - transition = env.step(actions) - # check signals - for item in transition[:-1]: # exclude info - for agent, data in item.items(): - self.assertTrue(self._check_valid_tensor(data), msg=f"Invalid data ('{agent}'): {data}") - - # close the environment - env.close() - - @staticmethod - def _check_valid_tensor(data: torch.Tensor | dict) -> bool: - """Checks if given data does not have corrupted values. - - Args: - data: Data buffer. - Returns: - True if the data is valid. - """ - if isinstance(data, torch.Tensor): - return not torch.any(torch.isnan(data)) - elif isinstance(data, (tuple, list)): - return all(TestEnvironments._check_valid_tensor(value) for value in data) - elif isinstance(data, dict): - return all(TestEnvironments._check_valid_tensor(value) for value in data.values()) - else: - raise ValueError(f"Input data of invalid type: {type(data)}.") +# Local imports should be imported last +from env_test_utils import _check_random_actions, setup_environment # isort: skip -if __name__ == "__main__": - run_tests() +@pytest.mark.parametrize("num_envs, device", [(32, "cuda"), (1, "cuda")]) +@pytest.mark.parametrize("task_name", setup_environment(multi_agent=True)) +def test_environments(task_name, num_envs, device): + """Run all environments with given parameters and check environments return valid signals.""" + print(f">>> Running test for environment: {task_name} with num_envs={num_envs} and device={device}") + # check environment + _check_random_actions(task_name, device, num_envs, multi_agent=True) + # close the environment + print(f">>> Closing environment: {task_name}") + print("-" * 80) diff --git a/source/isaaclab_tasks/test/test_record_video.py b/source/isaaclab_tasks/test/test_record_video.py index 7d5067f07724..a84eb846e887 100644 --- a/source/isaaclab_tasks/test/test_record_video.py +++ b/source/isaaclab_tasks/test/test_record_video.py @@ -1,11 +1,11 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -from isaaclab.app import AppLauncher, run_tests +from isaaclab.app import AppLauncher # launch the simulator app_launcher = AppLauncher(headless=True, enable_cameras=True) @@ -13,80 +13,66 @@ """Rest everything follows.""" -import gymnasium as gym import os + +import gymnasium as gym +import pytest import torch -import unittest import omni.usd import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils import parse_env_cfg - -class TestRecordVideoWrapper(unittest.TestCase): - """Test recording videos using the RecordVideo wrapper.""" - - @classmethod - def setUpClass(cls): - # acquire all Isaac environments names - cls.registered_tasks = list() - for task_spec in gym.registry.values(): - if "Isaac" in task_spec.id: - cls.registered_tasks.append(task_spec.id) - # sort environments by name - cls.registered_tasks.sort() - # print all existing task names - print(">>> All registered environments:", cls.registered_tasks) - # directory to save videos - cls.videos_dir = os.path.join(os.path.dirname(__file__), "output", "videos", "train") - - def setUp(self) -> None: - # common parameters - self.num_envs = 16 - self.device = "cuda" - # video parameters - self.step_trigger = lambda step: step % 225 == 0 - self.video_length = 200 - - def test_record_video(self): - """Run random actions agent with recording of videos.""" - for task_name in self.registered_tasks: - with self.subTest(task_name=task_name): - print(f">>> Running test for environment: {task_name}") - # create a new stage - omni.usd.get_context().new_stage() - - # parse configuration - env_cfg = parse_env_cfg(task_name, device=self.device, num_envs=self.num_envs) - - # create environment - env = gym.make(task_name, cfg=env_cfg, render_mode="rgb_array") - - # directory to save videos - videos_dir = os.path.join(self.videos_dir, task_name) - # wrap environment to record videos - env = gym.wrappers.RecordVideo( - env, - videos_dir, - step_trigger=self.step_trigger, - video_length=self.video_length, - disable_logger=True, - ) - - # reset environment - env.reset() - # simulate environment - with torch.inference_mode(): - for _ in range(500): - # compute zero actions - actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 - # apply actions - _ = env.step(actions) - - # close the simulator - env.close() - - -if __name__ == "__main__": - run_tests() +# Local imports should be imported last +from env_test_utils import setup_environment # isort: skip + + +@pytest.fixture(scope="function") +def setup_video_params(): + # common parameters + num_envs = 16 + device = "cuda" + # video parameters + step_trigger = lambda step: step % 225 == 0 # noqa: E731 + video_length = 200 + return num_envs, device, step_trigger, video_length + + +@pytest.mark.parametrize("task_name", setup_environment(include_play=True)) +def test_record_video(task_name, setup_video_params): + """Run random actions agent with recording of videos.""" + num_envs, device, step_trigger, video_length = setup_video_params + videos_dir = os.path.join(os.path.dirname(__file__), "output", "videos", "train") + # create a new stage + omni.usd.get_context().new_stage() + + # parse configuration + env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) + + # create environment + env = gym.make(task_name, cfg=env_cfg, render_mode="rgb_array") + + # directory to save videos + task_videos_dir = os.path.join(videos_dir, task_name) + # wrap environment to record videos + env = gym.wrappers.RecordVideo( + env, + task_videos_dir, + step_trigger=step_trigger, + video_length=video_length, + disable_logger=True, + ) + + # reset environment + env.reset() + # simulate environment + with torch.inference_mode(): + for _ in range(500): + # compute zero actions + actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1 + # apply actions + _ = env.step(actions) + + # close the simulator + env.close() diff --git a/source/isaaclab_tasks/test/test_rl_device_separation.py b/source/isaaclab_tasks/test/test_rl_device_separation.py new file mode 100644 index 000000000000..ef6bd1e093f1 --- /dev/null +++ b/source/isaaclab_tasks/test/test_rl_device_separation.py @@ -0,0 +1,379 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Test RL device separation across all supported RL libraries. + +This test verifies that RL library wrappers correctly handle device transfers when the +simulation device differs from the RL training device. + +Device Architecture: + 1. sim_device: Where physics simulation runs and environment buffers live + 2. rl_device: Where policy networks and training computations occur + +Test Scenarios: + - GPU simulation + GPU RL: Same device (no transfers needed, optimal performance) + - GPU simulation + CPU RL: Cross-device transfers (wrapper handles transfers) + - CPU simulation + CPU RL: CPU-only operation + +Each test verifies the wrapper correctly: + 1. Unwrapped env: operates entirely on sim_device + 2. Wrapper: accepts actions on rl_device (where policy generates them) + 3. Wrapper: internally transfers actions from rl_device โ†’ sim_device for env.step() + 4. Wrapper: transfers outputs from sim_device โ†’ rl_device (for policy to use) + +Tested Libraries: + - RSL-RL: TensorDict observations, device separation via OnPolicyRunner (agent_cfg.device) + * Wrapper returns data on sim_device, Runner handles transfers to rl_device + - RL Games: Dict observations, explicit rl_device parameter in wrapper + * Wrapper transfers data from sim_device to rl_device + - Stable-Baselines3: Numpy arrays (CPU-only by design) + * Wrapper converts tensors to/from numpy on CPU + - skrl: Dict observations, uses skrl.config.torch.device for RL device + * Wrapper keeps observations on sim_device, only transfers actions + +""" + +from isaaclab.app import AppLauncher + +# launch the simulator +app_launcher = AppLauncher(headless=True) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import gymnasium as gym +import pytest +import torch + +import carb +import omni.usd + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils.parse_cfg import parse_env_cfg + +# Test environment - use Cartpole as it's simple and fast +TEST_ENV = "Isaac-Cartpole-v0" +NUM_ENVS = 4 + + +def _create_env(sim_device: str): + """Create and initialize a test environment. + + Args: + sim_device: Device for simulation (e.g., "cuda:0", "cpu") + + Returns: + Initialized gym environment + """ + # Create a new stage + omni.usd.get_context().new_stage() + # Reset the rtx sensors carb setting to False + carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + + try: + env_cfg = parse_env_cfg(TEST_ENV, device=sim_device, num_envs=NUM_ENVS) + env = gym.make(TEST_ENV, cfg=env_cfg) + except Exception as e: + # Try to close environment on exception + if "env" in locals() and hasattr(env, "_is_closed"): + env.close() + else: + if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + pytest.fail(f"Failed to set-up the environment for task {TEST_ENV}. Error: {e}") + + # Disable control on stop + env.unwrapped.sim._app_control_on_stop_handle = None + return env + + +def _verify_unwrapped_env(env, sim_device: str): + """Verify unwrapped environment operates entirely on sim_device. + + Args: + env: Unwrapped gym environment + sim_device: Expected simulation device + """ + assert env.unwrapped.device == sim_device, ( + f"Environment device mismatch: expected {sim_device}, got {env.unwrapped.device}" + ) + + # Verify reset returns data on sim device + obs_dict, _ = env.reset() + for key, value in obs_dict.items(): + if isinstance(value, torch.Tensor): + assert value.device.type == torch.device(sim_device).type, ( + f"Unwrapped env obs '{key}' should be on {sim_device}, got {value.device}" + ) + + # Verify step returns data on sim device + action_space = env.unwrapped.single_action_space + test_action = torch.zeros(NUM_ENVS, action_space.shape[0], device=sim_device) + obs_dict, rew, term, trunc, extras = env.step(test_action) + assert rew.device.type == torch.device(sim_device).type, ( + f"Unwrapped env rewards should be on {sim_device}, got {rew.device}" + ) + assert term.device.type == torch.device(sim_device).type, ( + f"Unwrapped env terminated should be on {sim_device}, got {term.device}" + ) + + +def _verify_tensor_device(data, expected_device: str, name: str): + """Verify tensor or dict of tensors is on expected device. + + Args: + data: Tensor, dict of tensors, or numpy array + expected_device: Expected device string + name: Name for error messages + """ + if isinstance(data, torch.Tensor): + assert data.device.type == torch.device(expected_device).type, ( + f"{name} should be on {expected_device}, got {data.device}" + ) + elif isinstance(data, dict): + for key, value in data.items(): + if isinstance(value, torch.Tensor): + assert value.device.type == torch.device(expected_device).type, ( + f"{name}['{key}'] should be on {expected_device}, got {value.device}" + ) + + +def _test_rsl_rl_device_separation(sim_device: str, rl_device: str): + """Helper function to test RSL-RL with specified device configuration. + + Note: RSL-RL device separation is handled by the OnPolicyRunner, not the wrapper. + The wrapper returns observations on sim_device, and the runner handles device transfers. + This test verifies the wrapper works correctly when actions come from a different device. + + Args: + sim_device: Device for simulation (e.g., "cuda:0", "cpu") + rl_device: Device for RL agent (e.g., "cuda:0", "cpu") - where policy generates actions + """ + from tensordict import TensorDict + + from isaaclab_rl.rsl_rl import RslRlVecEnvWrapper + + env = _create_env(sim_device) + _verify_unwrapped_env(env, sim_device) + + # Create wrapper - it uses sim_device, runner handles rl_device + env = RslRlVecEnvWrapper(env) + assert env.device == sim_device, f"Wrapper device should be {sim_device}" + + # Test reset - wrapper returns observations on sim_device + obs, extras = env.reset() + assert isinstance(obs, TensorDict), f"Expected TensorDict, got {type(obs)}" + _verify_tensor_device(obs, sim_device, "Observation") + + # Test step with action from RL device (simulating policy output) + # The wrapper should handle transferring action to sim_device internally + action = 2 * torch.rand(env.action_space.shape, device=rl_device) - 1 + obs, reward, dones, extras = env.step(action) + + # Verify outputs are on sim_device (runner would transfer to rl_device) + assert isinstance(obs, TensorDict), f"Expected TensorDict, got {type(obs)}" + _verify_tensor_device(obs, sim_device, "Step observation") + _verify_tensor_device(reward, sim_device, "Reward") + _verify_tensor_device(dones, sim_device, "Dones") + + env.close() + + +def _test_rl_games_device_separation(sim_device: str, rl_device: str): + """Helper function to test RL Games with specified device configuration. + + Args: + sim_device: Device for simulation (e.g., "cuda:0", "cpu") + rl_device: Device for RL agent (e.g., "cuda:0", "cpu") + """ + from isaaclab_rl.rl_games import RlGamesVecEnvWrapper + + env = _create_env(sim_device) + _verify_unwrapped_env(env, sim_device) + + # Create wrapper + env = RlGamesVecEnvWrapper(env, rl_device=rl_device, clip_obs=10.0, clip_actions=1.0) + + # Test reset + obs = env.reset() + _verify_tensor_device(obs, rl_device, "Observation") + + # Test step with action on RL device + action = 2 * torch.rand(NUM_ENVS, *env.action_space.shape, device=rl_device) - 1 + obs, reward, dones, info = env.step(action) + + # Verify outputs are on RL device + _verify_tensor_device(obs, rl_device, "Observation") + _verify_tensor_device(reward, rl_device, "Reward") + _verify_tensor_device(dones, rl_device, "Dones") + + env.close() + + +def _test_sb3_device_separation(sim_device: str): + """Helper function to test Stable-Baselines3 with specified device configuration. + + Note: SB3 always converts to CPU/numpy, so we don't test rl_device parameter. + + Args: + sim_device: Device for simulation (e.g., "cuda:0", "cpu") + """ + import numpy as np + + from isaaclab_rl.sb3 import Sb3VecEnvWrapper + + env = _create_env(sim_device) + _verify_unwrapped_env(env, sim_device) + + # Create wrapper + env = Sb3VecEnvWrapper(env) + + # Test reset - SB3 should return numpy arrays + obs = env.reset() + assert isinstance(obs, np.ndarray), f"SB3 observations should be numpy arrays, got {type(obs)}" + + # Test step with numpy action + action = 2 * np.random.rand(env.num_envs, *env.action_space.shape) - 1 + obs, reward, done, info = env.step(action) + + # Verify outputs are numpy arrays + assert isinstance(obs, np.ndarray), f"Observations should be numpy arrays, got {type(obs)}" + assert isinstance(reward, np.ndarray), f"Rewards should be numpy arrays, got {type(reward)}" + assert isinstance(done, np.ndarray), f"Dones should be numpy arrays, got {type(done)}" + + env.close() + + +def _test_skrl_device_separation(sim_device: str, rl_device: str): + """Helper function to test skrl with specified device configuration. + + Note: skrl uses skrl.config.torch.device for device configuration. + Observations remain on sim_device; only actions are transferred from rl_device. + + Args: + sim_device: Device for simulation (e.g., "cuda:0", "cpu") + rl_device: Device for RL agent (e.g., "cuda:0", "cpu") + """ + try: + import skrl + from skrl.envs.wrappers.torch import wrap_env + except ImportError: + pytest.skip("skrl not installed") + + # Configure skrl device + skrl.config.torch.device = torch.device(rl_device) + + env = _create_env(sim_device) + _verify_unwrapped_env(env, sim_device) + + # Wrap with skrl + env = wrap_env(env, wrapper="isaaclab") + + # Test reset + obs, info = env.reset() + assert isinstance(obs, (dict, torch.Tensor)), f"Observations should be dict or tensor, got {type(obs)}" + + # Test step with action on RL device + action = 2 * torch.rand(NUM_ENVS, *env.action_space.shape, device=skrl.config.torch.device) - 1 + transition = env.step(action) + + # Verify outputs - skrl keeps them on sim_device + if len(transition) == 5: + obs, reward, terminated, truncated, info = transition + _verify_tensor_device(obs, sim_device, "Observation") + _verify_tensor_device(reward, sim_device, "Reward") + _verify_tensor_device(terminated, sim_device, "Terminated") + _verify_tensor_device(truncated, sim_device, "Truncated") + elif len(transition) == 4: + obs, reward, done, info = transition + _verify_tensor_device(obs, sim_device, "Observation") + _verify_tensor_device(reward, sim_device, "Reward") + _verify_tensor_device(done, sim_device, "Done") + else: + pytest.fail(f"Unexpected number of return values from step: {len(transition)}") + + env.close() + + +# ============================================================================ +# Test Functions +# ============================================================================ + + +def test_rsl_rl_device_separation_gpu_to_gpu(): + """Test RSL-RL with GPU simulation and GPU RL (default configuration).""" + try: + import isaaclab_rl.rsl_rl # noqa: F401 + except ImportError: + pytest.skip("RSL-RL not installed") + + _test_rsl_rl_device_separation(sim_device="cuda:0", rl_device="cuda:0") + + +def test_rsl_rl_device_separation_gpu_to_cpu(): + """Test RSL-RL with GPU simulation and CPU RL (cross-device transfer).""" + try: + import isaaclab_rl.rsl_rl # noqa: F401 + except ImportError: + pytest.skip("RSL-RL not installed") + + _test_rsl_rl_device_separation(sim_device="cuda:0", rl_device="cpu") + + +def test_rl_games_device_separation_gpu_to_gpu(): + """Test RL Games with GPU simulation and GPU RL (default configuration).""" + try: + import isaaclab_rl.rl_games # noqa: F401 + except ImportError: + pytest.skip("RL Games not installed") + + _test_rl_games_device_separation(sim_device="cuda:0", rl_device="cuda:0") + + +def test_rl_games_device_separation_gpu_to_cpu(): + """Test RL Games with GPU simulation and CPU RL (cross-device transfer).""" + try: + import isaaclab_rl.rl_games # noqa: F401 + except ImportError: + pytest.skip("RL Games not installed") + + _test_rl_games_device_separation(sim_device="cuda:0", rl_device="cpu") + + +def test_sb3_device_separation_gpu(): + """Test Stable-Baselines3 with GPU simulation. + + Note: SB3 always converts to CPU/numpy, so only GPU simulation is tested. + """ + try: + import isaaclab_rl.sb3 # noqa: F401 + except ImportError: + pytest.skip("Stable-Baselines3 not installed") + + _test_sb3_device_separation(sim_device="cuda:0") + + +def test_skrl_device_separation_gpu(): + """Test skrl with GPU simulation and GPU policy (matching devices).""" + try: + import skrl # noqa: F401 + except ImportError: + pytest.skip("skrl not installed") + + _test_skrl_device_separation(sim_device="cuda:0", rl_device="cuda:0") + + +def test_skrl_device_separation_cpu_to_gpu(): + """Test skrl with CPU simulation and GPU policy. + + Note: Uses skrl.config.torch.device to set the policy device to GPU + while the environment runs on CPU. + """ + try: + import skrl # noqa: F401 + except ImportError: + pytest.skip("skrl not installed") + + _test_skrl_device_separation(sim_device="cpu", rl_device="cuda:0") diff --git a/tools/conftest.py b/tools/conftest.py new file mode 100644 index 000000000000..a61c94f2c474 --- /dev/null +++ b/tools/conftest.py @@ -0,0 +1,420 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import contextlib +import os +import select +import subprocess +import sys +import time + +import pytest +from junitparser import Error, JUnitXml, TestCase, TestSuite +from prettytable import PrettyTable + +# Local imports +import test_settings as test_settings # isort: skip + + +def pytest_ignore_collect(collection_path, config): + # Skip collection and run each test script individually + return True + + +def capture_test_output_with_timeout(cmd, timeout, env): + """Run a command with timeout and capture all output while streaming in real-time.""" + stdout_data = b"" + stderr_data = b"" + + try: + # Use Popen to capture output in real-time + process = subprocess.Popen( + cmd, env=env, stdout=subprocess.PIPE, stderr=subprocess.PIPE, bufsize=0, universal_newlines=False + ) + + # Set up file descriptors for non-blocking reads + stdout_fd = process.stdout.fileno() + stderr_fd = process.stderr.fileno() + + # Set non-blocking mode (Unix systems only) + try: + import fcntl + + for fd in [stdout_fd, stderr_fd]: + flags = fcntl.fcntl(fd, fcntl.F_GETFL) + fcntl.fcntl(fd, fcntl.F_SETFL, flags | os.O_NONBLOCK) + except ImportError: + # fcntl not available on Windows, use a simpler approach + pass + + start_time = time.time() + + while process.poll() is None: + # Check for timeout + if time.time() - start_time > timeout: + process.kill() + try: + remaining_stdout, remaining_stderr = process.communicate(timeout=5) + stdout_data += remaining_stdout + stderr_data += remaining_stderr + except subprocess.TimeoutExpired: + process.terminate() + remaining_stdout, remaining_stderr = process.communicate(timeout=1) + stdout_data += remaining_stdout + stderr_data += remaining_stderr + return -1, stdout_data, stderr_data, True # -1 indicates timeout + + # Check for available output + try: + ready_fds, _, _ = select.select([stdout_fd, stderr_fd], [], [], 0.1) + + for fd in ready_fds: + with contextlib.suppress(OSError): + if fd == stdout_fd: + chunk = process.stdout.read(1024) + if chunk: + stdout_data += chunk + # Print to stdout in real-time + sys.stdout.buffer.write(chunk) + sys.stdout.buffer.flush() + elif fd == stderr_fd: + chunk = process.stderr.read(1024) + if chunk: + stderr_data += chunk + # Print to stderr in real-time + sys.stderr.buffer.write(chunk) + sys.stderr.buffer.flush() + except OSError: + # select failed, fall back to simple polling + time.sleep(0.1) + continue + + # Get any remaining output + remaining_stdout, remaining_stderr = process.communicate() + stdout_data += remaining_stdout + stderr_data += remaining_stderr + + return process.returncode, stdout_data, stderr_data, False + + except Exception as e: + return -1, str(e).encode(), b"", False + + +def create_timeout_test_case(test_file, timeout, stdout_data, stderr_data): + """Create a test case entry for a timeout test with captured logs.""" + test_suite = TestSuite(name=f"timeout_{os.path.splitext(os.path.basename(test_file))[0]}") + test_case = TestCase(name="test_execution", classname=os.path.splitext(os.path.basename(test_file))[0]) + + # Create error message with timeout info and captured logs + error_msg = f"Test timed out after {timeout} seconds" + + # Add captured output to error details + details = f"Timeout after {timeout} seconds\n\n" + + if stdout_data: + details += "=== STDOUT ===\n" + details += stdout_data.decode("utf-8", errors="replace") + "\n" + + if stderr_data: + details += "=== STDERR ===\n" + details += stderr_data.decode("utf-8", errors="replace") + "\n" + + error = Error(message=error_msg) + error.text = details + test_case.result = error + + test_suite.add_testcase(test_case) + return test_suite + + +def run_individual_tests(test_files, workspace_root, isaacsim_ci): + """Run each test file separately, ensuring one finishes before starting the next.""" + failed_tests = [] + test_status = {} + + for test_file in test_files: + print(f"\n\n๐Ÿš€ Running {test_file} independently...\n") + # get file name from path + file_name = os.path.basename(test_file) + env = os.environ.copy() + + # Determine timeout for this test + timeout = test_settings.PER_TEST_TIMEOUTS.get(file_name, test_settings.DEFAULT_TIMEOUT) + + # Prepare command + # Note: Command options matter as they are used for cleanups inside AppLauncher + cmd = [ + sys.executable, + "-m", + "pytest", + "--no-header", + f"--config-file={workspace_root}/pyproject.toml", + f"--junitxml=tests/test-reports-{str(file_name)}.xml", + "--tb=short", + ] + + if isaacsim_ci: + cmd.append("-m") + cmd.append("isaacsim_ci") + + # Add the test file path last + cmd.append(str(test_file)) + + # Run test with timeout and capture output + returncode, stdout_data, stderr_data, timed_out = capture_test_output_with_timeout(cmd, timeout, env) + + if timed_out: + print(f"Test {test_file} timed out after {timeout} seconds...") + failed_tests.append(test_file) + + # Create a special XML report for timeout tests with captured logs + timeout_suite = create_timeout_test_case(test_file, timeout, stdout_data, stderr_data) + timeout_report = JUnitXml() + timeout_report.add_testsuite(timeout_suite) + + # Write timeout report + report_file = f"tests/test-reports-{str(file_name)}.xml" + timeout_report.write(report_file) + + test_status[test_file] = { + "errors": 1, + "failures": 0, + "skipped": 0, + "tests": 1, + "result": "TIMEOUT", + "time_elapsed": timeout, + } + continue + + if returncode != 0: + failed_tests.append(test_file) + + # check report for any failures + report_file = f"tests/test-reports-{str(file_name)}.xml" + if not os.path.exists(report_file): + print(f"Warning: Test report not found at {report_file}") + failed_tests.append(test_file) + test_status[test_file] = { + "errors": 1, # Assume error since we can't read the report + "failures": 0, + "skipped": 0, + "tests": 0, + "result": "FAILED", + "time_elapsed": 0.0, + } + continue + + try: + report = JUnitXml.fromfile(report_file) + + # Rename test suites to be more descriptive + for suite in report: + if suite.name == "pytest": + # Remove .py extension and use the filename as the test suite name + suite_name = os.path.splitext(file_name)[0] + suite.name = suite_name + + # Write the updated report back + report.write(report_file) + + # Parse the integer values with None handling + errors = int(report.errors) if report.errors is not None else 0 + failures = int(report.failures) if report.failures is not None else 0 + skipped = int(report.skipped) if report.skipped is not None else 0 + tests = int(report.tests) if report.tests is not None else 0 + time_elapsed = float(report.time) if report.time is not None else 0.0 + except Exception as e: + print(f"Error reading test report {report_file}: {e}") + failed_tests.append(test_file) + test_status[test_file] = { + "errors": 1, + "failures": 0, + "skipped": 0, + "tests": 0, + "result": "FAILED", + "time_elapsed": 0.0, + } + continue + + # Check if there were any failures + if errors > 0 or failures > 0: + failed_tests.append(test_file) + + test_status[test_file] = { + "errors": errors, + "failures": failures, + "skipped": skipped, + "tests": tests, + "result": "FAILED" if errors > 0 or failures > 0 else "passed", + "time_elapsed": time_elapsed, + } + + print("~~~~~~~~~~~~ Finished running all tests") + + return failed_tests, test_status + + +def pytest_sessionstart(session): + """Intercept pytest startup to execute tests in the correct order.""" + # Get the workspace root directory (one level up from tools) + workspace_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + source_dirs = [ + os.path.join(workspace_root, "scripts"), + os.path.join(workspace_root, "source"), + ] + + # Get filter pattern from environment variable or command line + filter_pattern = os.environ.get("TEST_FILTER_PATTERN", "") + exclude_pattern = os.environ.get("TEST_EXCLUDE_PATTERN", "") + + isaacsim_ci = os.environ.get("ISAACSIM_CI_SHORT", "false") == "true" + + # Also try to get from pytest config + if hasattr(session.config, "option") and hasattr(session.config.option, "filter_pattern"): + filter_pattern = filter_pattern or getattr(session.config.option, "filter_pattern", "") + if hasattr(session.config, "option") and hasattr(session.config.option, "exclude_pattern"): + exclude_pattern = exclude_pattern or getattr(session.config.option, "exclude_pattern", "") + + print("=" * 50) + print("CONFTEST.PY DEBUG INFO") + print("=" * 50) + print(f"Filter pattern: '{filter_pattern}'") + print(f"Exclude pattern: '{exclude_pattern}'") + print(f"TEST_FILTER_PATTERN env var: '{os.environ.get('TEST_FILTER_PATTERN', 'NOT_SET')}'") + print(f"TEST_EXCLUDE_PATTERN env var: '{os.environ.get('TEST_EXCLUDE_PATTERN', 'NOT_SET')}'") + print("=" * 50) + + # Get all test files in the source directories + test_files = [] + + for source_dir in source_dirs: + if not os.path.exists(source_dir): + print(f"Error: source directory not found at {source_dir}") + pytest.exit("Source directory not found", returncode=1) + + for root, _, files in os.walk(source_dir): + for file in files: + if file.startswith("test_") and file.endswith(".py"): + # Skip if the file is in TESTS_TO_SKIP + if file in test_settings.TESTS_TO_SKIP: + print(f"Skipping {file} as it's in the skip list") + continue + + full_path = os.path.join(root, file) + + # Apply include filter + if filter_pattern and filter_pattern not in full_path: + print(f"Skipping {full_path} (does not match include pattern: {filter_pattern})") + continue + + # Apply exclude filter + if exclude_pattern and exclude_pattern in full_path: + print(f"Skipping {full_path} (matches exclude pattern: {exclude_pattern})") + continue + + test_files.append(full_path) + + if isaacsim_ci: + new_test_files = [] + for test_file in test_files: + with open(test_file) as f: + if "@pytest.mark.isaacsim_ci" in f.read(): + new_test_files.append(test_file) + test_files = new_test_files + + if not test_files: + print("No test files found in source directory") + pytest.exit("No test files found", returncode=1) + + print(f"Found {len(test_files)} test files after filtering:") + for test_file in test_files: + print(f" - {test_file}") + + # Run all tests individually + failed_tests, test_status = run_individual_tests(test_files, workspace_root, isaacsim_ci) + + print("failed tests:", failed_tests) + + # Collect reports + print("~~~~~~~~~ Collecting final report...") + + # create new full report + full_report = JUnitXml() + # read all reports and merge them + for report in os.listdir("tests"): + if report.endswith(".xml"): + print(report) + report_file = JUnitXml.fromfile(f"tests/{report}") + full_report += report_file + print("~~~~~~~~~~~~ Writing final report...") + # write content to full report + result_file = os.environ.get("TEST_RESULT_FILE", "full_report.xml") + full_report_path = f"tests/{result_file}" + print(f"Using result file: {result_file}") + full_report.write(full_report_path) + print("~~~~~~~~~~~~ Report written to", full_report_path) + + # print test status in a nice table + # Calculate the number and percentage of passing tests + num_tests = len(test_status) + num_passing = len([test_path for test_path in test_files if test_status[test_path]["result"] == "passed"]) + num_failing = len([test_path for test_path in test_files if test_status[test_path]["result"] == "FAILED"]) + num_timeout = len([test_path for test_path in test_files if test_status[test_path]["result"] == "TIMEOUT"]) + + if num_tests == 0: + passing_percentage = 100 + else: + passing_percentage = num_passing / num_tests * 100 + + # Print summaries of test results + summary_str = "\n\n" + summary_str += "===================\n" + summary_str += "Test Result Summary\n" + summary_str += "===================\n" + + summary_str += f"Total: {num_tests}\n" + summary_str += f"Passing: {num_passing}\n" + summary_str += f"Failing: {num_failing}\n" + summary_str += f"Timeout: {num_timeout}\n" + summary_str += f"Passing Percentage: {passing_percentage:.2f}%\n" + + # Print time elapsed in hours, minutes, seconds + total_time = sum([test_status[test_path]["time_elapsed"] for test_path in test_files]) + + summary_str += f"Total Time Elapsed: {total_time // 3600}h" + summary_str += f"{total_time // 60 % 60}m" + summary_str += f"{total_time % 60:.2f}s" + + summary_str += "\n\n=======================\n" + summary_str += "Per Test Result Summary\n" + summary_str += "=======================\n" + + # Construct table of results per test + per_test_result_table = PrettyTable(field_names=["Test Path", "Result", "Time (s)", "# Tests"]) + per_test_result_table.align["Test Path"] = "l" + per_test_result_table.align["Time (s)"] = "r" + for test_path in test_files: + num_tests_passed = ( + test_status[test_path]["tests"] + - test_status[test_path]["failures"] + - test_status[test_path]["errors"] + - test_status[test_path]["skipped"] + ) + per_test_result_table.add_row( + [ + test_path, + test_status[test_path]["result"], + f"{test_status[test_path]['time_elapsed']:0.2f}", + f"{num_tests_passed}/{test_status[test_path]['tests']}", + ] + ) + + summary_str += per_test_result_table.get_string() + + # Print summary to console and log file + print(summary_str) + + # Exit pytest after custom execution to prevent normal pytest from overwriting our report + pytest.exit("Custom test execution completed", returncode=0 if num_failing == 0 else 1) diff --git a/tools/install_deps.py b/tools/install_deps.py index 996cb17c3873..d2f08bb52e7c 100644 --- a/tools/install_deps.py +++ b/tools/install_deps.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -30,8 +30,9 @@ import argparse import os import shutil +from subprocess import PIPE, STDOUT, Popen + import toml -from subprocess import run # add argparse arguments parser = argparse.ArgumentParser(description="A utility to install dependencies based on extension.toml files.") @@ -52,17 +53,17 @@ def install_apt_packages(paths: list[str]): paths: A list of paths to the extension's root. Raises: - FileNotFoundError: If the extension.toml file is not found. SystemError: If 'apt' is not a known command. This is a system error. """ for path in paths: if shutil.which("apt"): # Check if the extension.toml file exists if not os.path.exists(f"{path}/config/extension.toml"): - raise FileNotFoundError( - "During the installation of 'apt' dependencies, unable to find a" + print( + "[WARN] During the installation of 'apt' dependencies, unable to find a" f" valid file at: {path}/config/extension.toml." ) + continue # Load the extension.toml file and check for apt_deps with open(f"{path}/config/extension.toml") as fd: ext_toml = toml.load(fd) @@ -94,7 +95,6 @@ def install_rosdep_packages(paths: list[str], ros_distro: str = "humble"): ros_distro: The ROS distribution to use for rosdep. Default is 'humble'. Raises: - FileNotFoundError: If the extension.toml file is not found under the path. FileNotFoundError: If a valid ROS workspace is not found while installing ROS dependencies. SystemError: If 'rosdep' is not a known command. This is raised if 'rosdep' is not installed on the system. """ @@ -102,10 +102,11 @@ def install_rosdep_packages(paths: list[str], ros_distro: str = "humble"): if shutil.which("rosdep"): # Check if the extension.toml file exists if not os.path.exists(f"{path}/config/extension.toml"): - raise FileNotFoundError( - "During the installation of 'rosdep' dependencies, unable to find a" + print( + "[WARN] During the installation of 'rosdep' dependencies, unable to find a" f" valid file at: {path}/config/extension.toml." ) + continue # Load the extension.toml file and check for ros_ws with open(f"{path}/config/extension.toml") as fd: ext_toml = toml.load(fd) @@ -125,15 +126,17 @@ def install_rosdep_packages(paths: list[str], ros_distro: str = "humble"): run_and_print(["rosdep", "init"]) run_and_print(["rosdep", "update", f"--rosdistro={ros_distro}"]) # install rosdep packages - run_and_print([ - "rosdep", - "install", - "--from-paths", - f"{ws_path}/src", - "--ignore-src", - "-y", - f"--rosdistro={ros_distro}", - ]) + run_and_print( + [ + "rosdep", + "install", + "--from-paths", + f"{ws_path}/src", + "--ignore-src", + "-y", + f"--rosdistro={ros_distro}", + ] + ) else: print(f"[INFO] No rosdep packages specified for the extension at: {path}") else: @@ -146,13 +149,19 @@ def install_rosdep_packages(paths: list[str], ros_distro: str = "humble"): def run_and_print(args: list[str]): """Runs a subprocess and prints the output to stdout. - This function wraps subprocess.run() and prints the output to stdout. + This function wraps Popen and prints the output to stdout in real-time. Args: - args: A list of arguments to pass to subprocess.run(). + args: A list of arguments to pass to Popen. """ - completed_process = run(args=args, capture_output=True, check=True) - print(f"{str(completed_process.stdout, encoding='utf-8')}") + print(f'Running "{args}"') + with Popen(args, stdout=PIPE, stderr=STDOUT, env=os.environ) as p: + while p.poll() is None: + text = p.stdout.read1().decode("utf-8") + print(text, end="", flush=True) + return_code = p.poll() + if return_code != 0: + raise RuntimeError(f'Subprocess with args: "{args}" failed. The returned error code was: {return_code}') def main(): diff --git a/tools/run_all_tests.py b/tools/run_all_tests.py index e69178eae54e..bbec83183584 100644 --- a/tools/run_all_tests.py +++ b/tools/run_all_tests.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -29,6 +29,7 @@ import time from datetime import datetime from pathlib import Path + from prettytable import PrettyTable # Local imports @@ -343,7 +344,9 @@ def warm_start_app(): capture_output=True, ) if len(warm_start_output.stderr) > 0: - if "DeprecationWarning" not in str(warm_start_output.stderr): + if "omni::fabric::IStageReaderWriter" not in str(warm_start_output.stderr) and "scaling_governor" not in str( + warm_start_output.stderr + ): logging.error(f"Error warm starting the app: {str(warm_start_output.stderr)}") exit(1) @@ -360,7 +363,9 @@ def warm_start_app(): capture_output=True, ) if len(warm_start_rendering_output.stderr) > 0: - if "DeprecationWarning" not in str(warm_start_rendering_output.stderr): + if "omni::fabric::IStageReaderWriter" not in str( + warm_start_rendering_output.stderr + ) and "scaling_governor" not in str(warm_start_output.stderr): logging.error(f"Error warm starting the app with rendering: {str(warm_start_rendering_output.stderr)}") exit(1) diff --git a/tools/run_train_envs.py b/tools/run_train_envs.py index 60f627aeb11e..efc85c0265ba 100644 --- a/tools/run_train_envs.py +++ b/tools/run_train_envs.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/__init__.py b/tools/template/__init__.py index e75ca2bc3f90..460a30569089 100644 --- a/tools/template/__init__.py +++ b/tools/template/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/cli.py b/tools/template/cli.py index 5b50917e93bf..d922025e070f 100644 --- a/tools/template/cli.py +++ b/tools/template/cli.py @@ -1,17 +1,17 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import enum -import glob +import importlib import os from collections.abc import Callable import rich.console import rich.table -from common import ROOT_DIR, TEMPLATE_DIR -from generator import generate +from common import ROOT_DIR +from generator import generate, get_algorithms_per_rl_library from InquirerPy import inquirer, separator @@ -73,7 +73,7 @@ def input_checkbox(self, message: str, choices: list[str], default: str | None = def transformer(result: list[str]) -> str: if "all" in result or "both" in result: token = "all" if "all" in result else "both" - return f'{token} ({", ".join(choices[:choices.index("---")])})' + return f"{token} ({', '.join(choices[: choices.index('---')])})" return ", ".join(result) return inquirer.checkbox( @@ -144,32 +144,29 @@ class State(str, enum.Enum): No = "[red]no[/red]" -def _get_algorithms_per_rl_library(): - data = {"rl_games": [], "rsl_rl": [], "skrl": [], "sb3": []} - for file in glob.glob(os.path.join(TEMPLATE_DIR, "agents", "*_cfg")): - for rl_library in data.keys(): - basename = os.path.basename(file).replace("_cfg", "") - if basename.startswith(f"{rl_library}_"): - data[rl_library].append(basename.replace(f"{rl_library}_", "").upper()) - return data - - def main() -> None: """Main function to run template generation from CLI.""" cli_handler = CLIHandler() - # project type - is_external_project = ( - cli_handler.input_select( - "Task type:", - choices=["External", "Internal"], - long_instruction=( - "External (recommended): task/project is in its own folder/repo outside the Isaac Lab project.\n" - "Internal: the task is implemented within the Isaac Lab project (in source/isaaclab_tasks)." - ), - ).lower() - == "external" - ) + lab_module = importlib.import_module("isaaclab") + lab_path = os.path.realpath(getattr(lab_module, "__file__", "") or (getattr(lab_module, "__path__", [""])[0])) + is_lab_pip_installed = ("site-packages" in lab_path) or ("dist-packages" in lab_path) + + if not is_lab_pip_installed: + # project type + is_external_project = ( + cli_handler.input_select( + "Task type:", + choices=["External", "Internal"], + long_instruction=( + "External (recommended): task/project is in its own folder/repo outside the Isaac Lab project.\n" + "Internal: the task is implemented within the Isaac Lab project (in source/isaaclab_tasks)." + ), + ).lower() + == "external" + ) + else: + is_external_project = True # project path (if 'external') project_path = None @@ -207,10 +204,12 @@ def main() -> None: default=supported_workflows, ) workflow = [{"name": item.split(" | ")[0].lower(), "type": item.split(" | ")[1].lower()} for item in workflow] + single_agent_workflow = [item for item in workflow if item["type"] == "single-agent"] + multi_agent_workflow = [item for item in workflow if item["type"] == "multi-agent"] # RL library rl_library_algorithms = [] - algorithms_per_rl_library = _get_algorithms_per_rl_library() + algorithms_per_rl_library = get_algorithms_per_rl_library() # - show supported RL libraries and features rl_library_table = rich.table.Table(title="Supported RL libraries") rl_library_table.add_column("RL/training feature", no_wrap=True) @@ -219,6 +218,7 @@ def main() -> None: rl_library_table.add_column("skrl") rl_library_table.add_column("sb3") rl_library_table.add_row("ML frameworks", "PyTorch", "PyTorch", "PyTorch, JAX", "PyTorch") + rl_library_table.add_row("Relative performance", "~1X", "~1X", "~1X", "~0.03X") rl_library_table.add_row( "Algorithms", ", ".join(algorithms_per_rl_library.get("rl_games", [])), @@ -226,18 +226,19 @@ def main() -> None: ", ".join(algorithms_per_rl_library.get("skrl", [])), ", ".join(algorithms_per_rl_library.get("sb3", [])), ) - rl_library_table.add_row("Relative performance", "~1X", "~1X", "~1X", "~0.03X") + rl_library_table.add_row("Multi-agent support", State.No, State.No, State.Yes, State.No) rl_library_table.add_row("Distributed training", State.Yes, State.No, State.Yes, State.No) rl_library_table.add_row("Vectorized training", State.Yes, State.Yes, State.Yes, State.No) rl_library_table.add_row("Fundamental/composite spaces", State.No, State.No, State.Yes, State.No) cli_handler.output_table(rl_library_table) # - prompt for RL libraries - supported_rl_libraries = ["rl_games", "rsl_rl", "skrl", "sb3"] + supported_rl_libraries = ["rl_games", "rsl_rl", "skrl", "sb3"] if len(single_agent_workflow) else ["skrl"] selected_rl_libraries = cli_handler.get_choices( cli_handler.input_checkbox("RL library:", choices=[*supported_rl_libraries, "---", "all"]), default=supported_rl_libraries, ) # - prompt for algorithms per RL library + algorithms_per_rl_library = get_algorithms_per_rl_library(len(single_agent_workflow), len(multi_agent_workflow)) for rl_library in selected_rl_libraries: algorithms = algorithms_per_rl_library.get(rl_library, []) if len(algorithms) > 1: diff --git a/tools/template/common.py b/tools/template/common.py index edc5ef23776c..08d2732a1911 100644 --- a/tools/template/common.py +++ b/tools/template/common.py @@ -1,10 +1,15 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import os +# paths ROOT_DIR = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) TASKS_DIR = os.path.join(ROOT_DIR, "source", "isaaclab_tasks", "isaaclab_tasks") TEMPLATE_DIR = os.path.join(ROOT_DIR, "tools", "template", "templates") + +# RL algorithms +SINGLE_AGENT_ALGORITHMS = ["AMP", "PPO"] +MULTI_AGENT_ALGORITHMS = ["IPPO", "MAPPO"] diff --git a/tools/template/generator.py b/tools/template/generator.py index c87b0caf9858..04f4bae6f63d 100644 --- a/tools/template/generator.py +++ b/tools/template/generator.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -11,7 +11,7 @@ from datetime import datetime import jinja2 -from common import ROOT_DIR, TASKS_DIR, TEMPLATE_DIR +from common import MULTI_AGENT_ALGORITHMS, ROOT_DIR, SINGLE_AGENT_ALGORITHMS, TASKS_DIR, TEMPLATE_DIR jinja_env = jinja2.Environment( loader=jinja2.FileSystemLoader(TEMPLATE_DIR), @@ -96,22 +96,22 @@ def _generate_task_per_workflow(task_dir: str, specification: dict) -> None: # workflow-specific content if task_spec["workflow"]["name"] == "direct": # - task/*env_cfg.py - template = jinja_env.get_template(f'tasks/direct_{task_spec["workflow"]["type"]}/env_cfg') + template = jinja_env.get_template(f"tasks/direct_{task_spec['workflow']['type']}/env_cfg") _write_file( - os.path.join(task_dir, f'{task_spec["filename"]}_env_cfg.py'), content=template.render(**specification) + os.path.join(task_dir, f"{task_spec['filename']}_env_cfg.py"), content=template.render(**specification) ) # - task/*env.py - template = jinja_env.get_template(f'tasks/direct_{task_spec["workflow"]["type"]}/env') - _write_file(os.path.join(task_dir, f'{task_spec["filename"]}_env.py'), content=template.render(**specification)) + template = jinja_env.get_template(f"tasks/direct_{task_spec['workflow']['type']}/env") + _write_file(os.path.join(task_dir, f"{task_spec['filename']}_env.py"), content=template.render(**specification)) elif task_spec["workflow"]["name"] == "manager-based": # - task/*env_cfg.py - template = jinja_env.get_template(f'tasks/manager-based_{task_spec["workflow"]["type"]}/env_cfg') + template = jinja_env.get_template(f"tasks/manager-based_{task_spec['workflow']['type']}/env_cfg") _write_file( - os.path.join(task_dir, f'{task_spec["filename"]}_env_cfg.py'), content=template.render(**specification) + os.path.join(task_dir, f"{task_spec['filename']}_env_cfg.py"), content=template.render(**specification) ) # - task/mdp folder shutil.copytree( - os.path.join(TEMPLATE_DIR, "tasks", f'manager-based_{task_spec["workflow"]["type"]}', "mdp"), + os.path.join(TEMPLATE_DIR, "tasks", f"manager-based_{task_spec['workflow']['type']}", "mdp"), os.path.join(task_dir, "mdp"), dirs_exist_ok=True, ) @@ -161,9 +161,10 @@ def _external(specification: dict) -> None: # repo files print(" |-- Copying repo files...") shutil.copyfile(os.path.join(ROOT_DIR, ".dockerignore"), os.path.join(project_dir, ".dockerignore")) - shutil.copyfile(os.path.join(ROOT_DIR, ".flake8"), os.path.join(project_dir, ".flake8")) + shutil.copyfile(os.path.join(ROOT_DIR, "pyproject.toml"), os.path.join(project_dir, "pyproject.toml")) shutil.copyfile(os.path.join(ROOT_DIR, ".gitattributes"), os.path.join(project_dir, ".gitattributes")) - shutil.copyfile(os.path.join(ROOT_DIR, ".gitignore"), os.path.join(project_dir, ".gitignore")) + if os.path.exists(os.path.join(ROOT_DIR, ".gitignore")): + shutil.copyfile(os.path.join(ROOT_DIR, ".gitignore"), os.path.join(project_dir, ".gitignore")) shutil.copyfile( os.path.join(ROOT_DIR, ".pre-commit-config.yaml"), os.path.join(project_dir, ".pre-commit-config.yaml") ) @@ -183,10 +184,12 @@ def _external(specification: dict) -> None: # replace placeholder in scripts for file in glob.glob(os.path.join(dir, rl_library["name"], "*.py")): _replace_in_file( - [( - "# PLACEHOLDER: Extension template (do not remove this comment)", - f"import {name}.tasks # noqa: F401", - )], + [ + ( + "# PLACEHOLDER: Extension template (do not remove this comment)", + f"import {name}.tasks # noqa: F401", + ) + ], src=file, ) # - other scripts @@ -195,6 +198,17 @@ def _external(specification: dict) -> None: src=os.path.join(ROOT_DIR, "scripts", "environments", "list_envs.py"), dst=os.path.join(dir, "list_envs.py"), ) + for script in ["zero_agent.py", "random_agent.py"]: + _replace_in_file( + [ + ( + "# PLACEHOLDER: Extension template (do not remove this comment)", + f"import {name}.tasks # noqa: F401", + ) + ], + src=os.path.join(ROOT_DIR, "scripts", "environments", script), + dst=os.path.join(dir, script), + ) # # docker files # print(" |-- Copying docker files...") # dir = os.path.join(project_dir, "docker") @@ -260,6 +274,28 @@ def _external(specification: dict) -> None: print("-" * 80) +def get_algorithms_per_rl_library(single_agent: bool = True, multi_agent: bool = True): + assert single_agent or multi_agent, "At least one of 'single_agent' or 'multi_agent' must be True" + data = {"rl_games": [], "rsl_rl": [], "skrl": [], "sb3": []} + # get algorithms + for file in glob.glob(os.path.join(TEMPLATE_DIR, "agents", "*_cfg")): + for rl_library in data.keys(): + basename = os.path.basename(file).replace("_cfg", "") + if basename.startswith(f"{rl_library}_"): + algorithm = basename.replace(f"{rl_library}_", "").upper() + assert algorithm in SINGLE_AGENT_ALGORITHMS or algorithm in MULTI_AGENT_ALGORITHMS, ( + f"{algorithm} algorithm is not listed in the supported algorithms" + ) + if single_agent and algorithm in SINGLE_AGENT_ALGORITHMS: + data[rl_library].append(algorithm) + if multi_agent and algorithm in MULTI_AGENT_ALGORITHMS: + data[rl_library].append(algorithm) + # remove duplicates and sort + for rl_library in data.keys(): + data[rl_library] = sorted(list(set(data[rl_library]))) + return data + + def generate(specification: dict) -> None: """Generate the project/task. diff --git a/tools/template/templates/agents/rsl_rl_ppo_cfg b/tools/template/templates/agents/rsl_rl_ppo_cfg index dec441a35874..85970dfc2ce4 100644 --- a/tools/template/templates/agents/rsl_rl_ppo_cfg +++ b/tools/template/templates/agents/rsl_rl_ppo_cfg @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -9,14 +9,15 @@ from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, R @configclass -class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): +class PPORunnerCfg(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_obs_normalization=False, + critic_obs_normalization=False, actor_hidden_dims=[32, 32], critic_hidden_dims=[32, 32], activation="elu", diff --git a/tools/template/templates/agents/sb3_ppo_cfg b/tools/template/templates/agents/sb3_ppo_cfg index 5856f35f8e87..4ac83212e440 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 e435b44eac9c..0946e4c6e6fa 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 bc0c51821792..a89939f95543 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 dcd794f57a5c..255b30eac810 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 1efe67083a5d..96515145faba 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/__init__ext b/tools/template/templates/extension/__init__ext index b41bbc821694..6705ede8b0a4 100644 --- a/tools/template/templates/extension/__init__ext +++ b/tools/template/templates/extension/__init__ext @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/extension/__init__tasks b/tools/template/templates/extension/__init__tasks index 0787019746e7..13df3c3210fa 100644 --- a/tools/template/templates/extension/__init__tasks +++ b/tools/template/templates/extension/__init__tasks @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/extension/__init__workflow b/tools/template/templates/extension/__init__workflow index cdf5f8614e90..65d6e5a24441 100644 --- a/tools/template/templates/extension/__init__workflow +++ b/tools/template/templates/extension/__init__workflow @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/extension/config/extension.toml b/tools/template/templates/extension/config/extension.toml index 66230f334dd5..dbe4b064fbc4 100644 --- a/tools/template/templates/extension/config/extension.toml +++ b/tools/template/templates/extension/config/extension.toml @@ -25,7 +25,7 @@ keywords = ["extension", "template", "isaaclab"] [[python.module]] name = "{{ name }}" -[isaaclab_settings] +[isaac_lab_settings] # TODO: Uncomment and list any apt dependencies here. # If none, leave it commented out. # apt_deps = ["example_package"] diff --git a/tools/template/templates/extension/setup.py b/tools/template/templates/extension/setup.py index f50e3bd7e545..e991708e34d8 100644 --- a/tools/template/templates/extension/setup.py +++ b/tools/template/templates/extension/setup.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -6,8 +6,8 @@ """Installation script for the '{{ name }}' python package.""" import os -import toml +import toml from setuptools import setup # Obtain the extension data from the extension.toml file @@ -32,13 +32,16 @@ description=EXTENSION_TOML_DATA["package"]["description"], keywords=EXTENSION_TOML_DATA["package"]["keywords"], install_requires=INSTALL_REQUIRES, - license="MIT", + license="Apache-2.0", include_package_data=True, python_requires=">=3.10", classifiers=[ "Natural Language :: English", "Programming Language :: Python :: 3.10", + "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/template/templates/extension/ui_extension_example.py b/tools/template/templates/extension/ui_extension_example.py index 009589d88b56..483f323954a7 100644 --- a/tools/template/templates/extension/ui_extension_example.py +++ b/tools/template/templates/extension/ui_extension_example.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/external/.vscode/tasks.json b/tools/template/templates/external/.vscode/tasks.json index cc67d77812d6..0ebe2101cff6 100644 --- a/tools/template/templates/external/.vscode/tasks.json +++ b/tools/template/templates/external/.vscode/tasks.json @@ -15,7 +15,7 @@ "inputs": [ { "id": "isaac_path", - "description": "Absolute path to the current Isaac Sim installation. Can be skipped if Isaac Sim installed from pip.", + "description": "Absolute path to the current Isaac Sim installation. If you installed IsaacSim from pip, the import of it failed. Please make sure you run the task with the correct python environment. As fallback, you can directly execute the python script by running: ``python.sh /.vscode/tools/setup_vscode.py``", {% if platform == "win32" %} "default": "C:/isaacsim", {% else %} diff --git a/tools/template/templates/external/.vscode/tools/settings.template.json b/tools/template/templates/external/.vscode/tools/settings.template.json index 5b97ac267e51..c1528d65dd73 100644 --- a/tools/template/templates/external/.vscode/tools/settings.template.json +++ b/tools/template/templates/external/.vscode/tools/settings.template.json @@ -55,15 +55,8 @@ ], // This enables python language server. Seems to work slightly better than jedi: "python.languageServer": "Pylance", - // We use "black" as a formatter: - "python.formatting.provider": "black", - "python.formatting.blackArgs": ["--line-length", "120"], - // Use flake8 for linting - "python.linting.pylintEnabled": false, - "python.linting.flake8Enabled": true, - "python.linting.flake8Args": [ - "--max-line-length=120" - ], + // Use ruff as a formatter and linter + "ruff.configuration": "${workspaceFolder}/pyproject.toml", // Use docstring generator "autoDocstring.docstringFormat": "google", "autoDocstring.guessTypes": true, diff --git a/tools/template/templates/external/.vscode/tools/setup_vscode.py b/tools/template/templates/external/.vscode/tools/setup_vscode.py index feb93c07b8f9..f8c61b76c5b7 100644 --- a/tools/template/templates/external/.vscode/tools/setup_vscode.py +++ b/tools/template/templates/external/.vscode/tools/setup_vscode.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -49,11 +49,11 @@ # check if the isaac-sim directory exists if not os.path.exists(isaacsim_dir): raise FileNotFoundError( - f"Could not find the isaac-sim directory: {isaacsim_dir}. There are two possible reasons for this:" - "\n\t1. The Isaac Sim directory does not exist as provided CLI path." - "\n\t2. The script could import the 'isaacsim' package. This could be due to the 'isaacsim' package not being " - "installed in the Python environment.\n" - "\nPlease make sure that the Isaac Sim directory exists or that the 'isaacsim' package is installed." + f"Could not find the isaac-sim directory: {isaacsim_dir}. There are two possible reasons for this:\n\t1. The" + " Isaac Sim directory does not exist as provided CLI path.\n\t2. The script couldn't import the 'isaacsim'" + " package. This could be due to the 'isaacsim' package not being installed in the Python" + " environment.\n\nPlease make sure that the Isaac Sim directory exists or that the 'isaacsim' package is" + " installed." ) ISAACSIM_DIR = isaacsim_dir diff --git a/tools/template/templates/external/README.md b/tools/template/templates/external/README.md index a3af34aa1b59..3b11b5407a85 100644 --- a/tools/template/templates/external/README.md +++ b/tools/template/templates/external/README.md @@ -15,7 +15,7 @@ It allows you to develop in an isolated environment, outside of the core Isaac L ## Installation - Install Isaac Lab by following the [installation guide](https://isaac-sim.github.io/IsaacLab/main/source/setup/installation/index.html). - We recommend using the conda installation as it simplifies calling Python scripts from the terminal. + We recommend using the conda or uv installation as it simplifies calling Python scripts from the terminal. - Clone or copy this project/repository separately from the Isaac Lab installation (i.e. outside the `IsaacLab` directory): @@ -44,6 +44,23 @@ It allows you to develop in an isolated environment, outside of the core Isaac L python scripts//train.py --task= ``` + - Running a task with dummy agents: + + These include dummy agents that output zero or random agents. They are useful to ensure that the environments are configured correctly. + + - Zero-action agent + + ```bash + # use 'FULL_PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda + python scripts/zero_agent.py --task= + ``` + - Random-action agent + + ```bash + # use 'FULL_PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda + python scripts/random_agent.py --task= + ``` + ### Set up IDE (Optional) To setup the IDE, please follow these instructions: diff --git a/tools/template/templates/external/docker/docker-compose.yaml b/tools/template/templates/external/docker/docker-compose.yaml index 501495f4529e..f1505b90d225 100644 --- a/tools/template/templates/external/docker/docker-compose.yaml +++ b/tools/template/templates/external/docker/docker-compose.yaml @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + x-default-isaac-lab-template-environment: &default-isaac-lab-template-environment - OMNI_KIT_ALLOW_ROOT=1 diff --git a/tools/template/templates/tasks/__init__agents b/tools/template/templates/tasks/__init__agents index e75ca2bc3f90..2e924fbf1b13 100644 --- a/tools/template/templates/tasks/__init__agents +++ b/tools/template/templates/tasks/__init__agents @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/tasks/__init__task b/tools/template/templates/tasks/__init__task index 1c85fd045e9b..e8890743df1d 100644 --- a/tools/template/templates/tasks/__init__task +++ b/tools/template/templates/tasks/__init__task @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -27,15 +27,15 @@ gym.register( {% for algorithm in rl_library.algorithms %} {# configuration file #} {% if rl_library.name == "rsl_rl" %} - {% set agent_config = rl_library.name ~ "_" ~ algorithm ~ "_cfg:" ~ algorithm|upper ~ "RunnerCfg" %} + {% set agent_config = "." ~ rl_library.name ~ "_" ~ algorithm ~ "_cfg:" ~ algorithm|upper ~ "RunnerCfg" %} {% else %} - {% set agent_config = rl_library.name ~ "_" ~ algorithm ~ "_cfg.yaml" %} + {% set agent_config = ":" ~ rl_library.name ~ "_" ~ algorithm ~ "_cfg.yaml" %} {% endif %} {# library configuration #} {% if algorithm == "ppo" %} - "{{ rl_library.name }}_cfg_entry_point": f"{agents.__name__}:{{ agent_config }}", + "{{ rl_library.name }}_cfg_entry_point": f"{agents.__name__}{{ agent_config }}", {% else %} - "{{ rl_library.name }}_{{ algorithm }}_cfg_entry_point": f"{agents.__name__}:{{ agent_config }}", + "{{ rl_library.name }}_{{ algorithm }}_cfg_entry_point": f"{agents.__name__}{{ agent_config }}", {% endif %} {% endfor %} {% endfor %} diff --git a/tools/template/templates/tasks/direct_multi-agent/env b/tools/template/templates/tasks/direct_multi-agent/env index 18664ac1535c..eec2331722e8 100644 --- a/tools/template/templates/tasks/direct_multi-agent/env +++ b/tools/template/templates/tasks/direct_multi-agent/env @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -37,6 +37,9 @@ class {{ task.classname }}Env(DirectMARLEnv): spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) # 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=[]) # add articulation to scene self.scene.articulations["robot"] = self.robot # add lights diff --git a/tools/template/templates/tasks/direct_multi-agent/env_cfg b/tools/template/templates/tasks/direct_multi-agent/env_cfg index 7cd27e3e91c6..3b207209b736 100644 --- a/tools/template/templates/tasks/direct_multi-agent/env_cfg +++ b/tools/template/templates/tasks/direct_multi-agent/env_cfg @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/tasks/direct_single-agent/env b/tools/template/templates/tasks/direct_single-agent/env index b07e33a4e1bc..e6f47fd3366b 100644 --- a/tools/template/templates/tasks/direct_single-agent/env +++ b/tools/template/templates/tasks/direct_single-agent/env @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -36,6 +36,9 @@ class {{ task.classname }}Env(DirectRLEnv): spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) # 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=[]) # add articulation to scene self.scene.articulations["robot"] = self.robot # add lights diff --git a/tools/template/templates/tasks/direct_single-agent/env_cfg b/tools/template/templates/tasks/direct_single-agent/env_cfg index 4d77489f5623..10588cd3e845 100644 --- a/tools/template/templates/tasks/direct_single-agent/env_cfg +++ b/tools/template/templates/tasks/direct_single-agent/env_cfg @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/tasks/manager-based_single-agent/env_cfg b/tools/template/templates/tasks/manager-based_single-agent/env_cfg index 1573fb441478..3ab42ecf166b 100644 --- a/tools/template/templates/tasks/manager-based_single-agent/env_cfg +++ b/tools/template/templates/tasks/manager-based_single-agent/env_cfg @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/tasks/manager-based_single-agent/mdp/__init__.py b/tools/template/templates/tasks/manager-based_single-agent/mdp/__init__.py index 44872114b8a1..966d4a3f4b7c 100644 --- a/tools/template/templates/tasks/manager-based_single-agent/mdp/__init__.py +++ b/tools/template/templates/tasks/manager-based_single-agent/mdp/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/tools/template/templates/tasks/manager-based_single-agent/mdp/rewards.py b/tools/template/templates/tasks/manager-based_single-agent/mdp/rewards.py index acfc7389010c..5500089d7f94 100644 --- a/tools/template/templates/tasks/manager-based_single-agent/mdp/rewards.py +++ b/tools/template/templates/tasks/manager-based_single-agent/mdp/rewards.py @@ -1,13 +1,14 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations -import torch from typing import TYPE_CHECKING +import torch + from isaaclab.assets import Articulation from isaaclab.managers import SceneEntityCfg from isaaclab.utils.math import wrap_to_pi diff --git a/tools/test_settings.py b/tools/test_settings.py index 09bb0c0cfe94..3738eab3d2e0 100644 --- a/tools/test_settings.py +++ b/tools/test_settings.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -12,24 +12,30 @@ ISAACLAB_PATH = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) """Path to the root directory of the Isaac Lab repository.""" -DEFAULT_TIMEOUT = 120 +DEFAULT_TIMEOUT = 300 """The default timeout for each test in seconds.""" PER_TEST_TIMEOUTS = { - "test_articulation.py": 200, - "test_deformable_object.py": 200, - "test_environments.py": 1850, # This test runs through all the environments for 100 steps each - "test_environment_determinism.py": 200, # This test runs through many the environments for 100 steps each - "test_factory_environments.py": 300, # This test runs through Factory environments for 100 steps each - "test_env_rendering_logic.py": 300, - "test_camera.py": 500, - "test_tiled_camera.py": 300, - "test_generate_dataset.py": 300, # This test runs annotation for 10 demos and generation until one succeeds - "test_rsl_rl_wrapper.py": 200, - "test_sb3_wrapper.py": 200, - "test_skrl_wrapper.py": 200, - "test_operational_space.py": 300, - "test_terrain_importer.py": 200, + "test_articulation.py": 500, + "test_stage_in_memory.py": 500, + "test_environments.py": 2500, # This test runs through all the environments for 100 steps each + "test_environments_with_stage_in_memory.py": ( + 2500 + ), # Like the above, with stage in memory and with and without fabric cloning + "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_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, + "test_skrl_wrapper.py": 500, + "test_rsl_rl_wrapper.py": 500, } """A dictionary of tests and their timeouts in seconds.