diff --git a/.flake8 b/.flake8 new file mode 100644 index 0000000000..65cf02277a --- /dev/null +++ b/.flake8 @@ -0,0 +1,8 @@ +[flake8] +show-source=True +statistics=True +per-file-ignores=*/__init__.py:F401 +ignore=E402,E501,W503,E203 +max-line-length = 120 +max-complexity = 30 +exclude=_*,.vscode,.git diff --git a/.github/ISSUE_TEMPLATE/bug.md b/.github/ISSUE_TEMPLATE/bug.md new file mode 100644 index 0000000000..7b05ae167d --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug.md @@ -0,0 +1,47 @@ +--- +name: Bug Report +about: Submit a bug report +title: "[Bug Report] Bug title" + +--- + +If you are submitting a bug report, please fill in the following details and use the tag [bug]. + +### Describe the bug + +A clear and concise description of what the bug is. + +### Steps to reproduce + +Please try to provide a minimal example to reproduce the bug. Error messages and stack traces are also helpful. + + + +### System Info + +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.] + +### Additional context + +Add any other context about the problem here. + +### Checklist + +- [ ] I have checked that there is no similar issue in the repo (**required**) +- [ ] I have checked that the issue is not in running Isaac Sim itself and is related to the repo diff --git a/.github/ISSUE_TEMPLATE/proposal.md b/.github/ISSUE_TEMPLATE/proposal.md new file mode 100644 index 0000000000..2b382f379b --- /dev/null +++ b/.github/ISSUE_TEMPLATE/proposal.md @@ -0,0 +1,34 @@ +--- +name: Proposal +about: Propose changes that are not fixes bugs +title: "[Proposal] Proposal title" +--- + + +### Proposal + +A clear and concise description of the proposal. In a few sentences, describe the feature and its core capabilities. + +### Motivation + +Please outline the motivation for the proposal. Summarize the core use cases and user problems and needs you are trying to solve. + +Is your feature request related to a problem? e.g.,"I'm always frustrated when [...]". + +If this is related to another GitHub issue, please link here too. + +### Pitch + +A clear and concise description of what you want to happen. + +### Alternatives + +A clear and concise description of any alternative solutions or features you've considered, if any. + +### Additional context + +Add any other context or screenshots about the feature request here. + +### Checklist + +- [ ] I have checked that there is no similar issue in the repo (**required**) diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md new file mode 100644 index 0000000000..b6e18b7fd5 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/question.md @@ -0,0 +1,13 @@ +--- +name: Question +about: Ask a question +title: "[Question] Question title" +--- + +### Question + +Basic questions, related to robot learning, that are not bugs or feature requests will be closed without reply, because GitHub issues are not an appropriate venue for these. + +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). diff --git a/.github/LICENSE_HEADER.txt b/.github/LICENSE_HEADER.txt new file mode 100644 index 0000000000..8252de99fb --- /dev/null +++ b/.github/LICENSE_HEADER.txt @@ -0,0 +1,4 @@ +Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +All rights reserved. + +SPDX-License-Identifier: BSD-3-Clause diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 0000000000..b4defdb8e4 --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,55 @@ +# Description + + + +Please include a summary of the change and which issue is fixed. Please also include relevant motivation and context. +List any dependencies that are required for this change. + +Fixes # (issue) + + + +## Type of change + + + +- 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 + +## Screenshots + +Please attach before and after screenshots of the change if applicable. + + + +## Checklist + +- [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `pre-commit run --all-files` (see [here](https://pre-commit.com/#install) instructions to set it up) +- [ ] I have made corresponding changes to the documentation +- [ ] My changes generate no new warnings +- [ ] I have added tests that prove my fix is effective or that my feature works +- [ ] I have updated the changelog and the corresponding version in the extension's `setup.py` and `config/extension.toml` files + + diff --git a/.github/stale.yml b/.github/stale.yml new file mode 100644 index 0000000000..9df5640208 --- /dev/null +++ b/.github/stale.yml @@ -0,0 +1,62 @@ +# Configuration for probot-stale - https://github.com/probot/stale + +# Number of days of inactivity before an Issue or Pull Request becomes stale +daysUntilStale: 60 + +# Number of days of inactivity before an Issue or Pull Request with the stale label is closed. +# Set to false to disable. If disabled, issues still need to be closed manually, but will remain marked as stale. +daysUntilClose: 14 + +# Only issues or pull requests with all of these labels are check if stale. Defaults to `[]` (disabled) +onlyLabels: + - more-information-needed + +# Issues or Pull Requests with these labels will never be considered stale. Set to `[]` to disable +exemptLabels: + - pinned + - security + - "[Status] Maybe Later" + +# Set to true to ignore issues in a project (defaults to false) +exemptProjects: true + +# Set to true to ignore issues in a milestone (defaults to false) +exemptMilestones: true + +# Set to true to ignore issues with an assignee (defaults to false) +exemptAssignees: true + +# Label to use when marking as stale +staleLabel: stale + +# Comment to post when marking as stale. Set to `false` to disable +markComment: > + This issue has been automatically marked as stale because it has not had + recent activity. It will be closed if no further activity occurs. Thank you + for your contributions. + +# Comment to post when removing the stale label. +# unmarkComment: > +# Your comment here. + +# Comment to post when closing a stale Issue or Pull Request. +# closeComment: > +# Your comment here. + +# Limit the number of actions per hour, from 1-30. Default is 30 +limitPerRun: 30 + +# Limit to only `issues` or `pulls` +only: issues + +# Optionally, specify configuration settings that are specific to just 'issues' or 'pulls': +# pulls: +# daysUntilStale: 30 +# markComment: > +# This pull request has been automatically marked as stale because it has not had +# recent activity. It will be closed if no further activity occurs. Thank you +# for your contributions. + +# issues: +# exemptLabels: +# - confirmed diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml new file mode 100644 index 0000000000..9391c7bc35 --- /dev/null +++ b/.github/workflows/docs.yaml @@ -0,0 +1,33 @@ +name: Build & deploy docs + +on: [push] + +jobs: + build-docs: + name: Build and deploy documentation + runs-on: ubuntu-latest + if: github.repository == 'Isaac-Orbit/orbit' && github.ref == 'refs/heads/main' + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Setup python + uses: actions/setup-python@v2 + with: + python-version: "3.8" + architecture: x64 + + - name: Install dev requirements + working-directory: ./docs + run: pip install -r requirements.txt + + - name: Generate docs + working-directory: ./docs + run: make html + + - name: Deploy to gh-pages + uses: peaceiris/actions-gh-pages@v3 + with: + github_token: ${{ secrets.GITHUB_TOKEN }} + publish_dir: ./docs/_build/html diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 0000000000..d34fcde446 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,14 @@ +name: Run linters using pre-commit + +on: + pull_request: + push: + branches: [main] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v3 + - uses: pre-commit/action@v3.0.0 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..1671f03734 --- /dev/null +++ b/.gitignore @@ -0,0 +1,38 @@ +# C++ +**/cmake-build*/ +**/build*/ +**/*.so + +# Omniverse +**/*.dmp +**/.thumbs + +# Python +.DS_Store +**/*.egg-info/ +**/__pycache__/ +**/.pytest_cache/ +**/*.pyc +**/*.pb + +# IDE +**/.idea/ +**/.vscode/.** + +# Outputs +**/output/* +*.tmp + +# Isaac-Sim packman +_isaac_sim +_repo +_build +.lastformat + +# Internal +# TODO: Remove this once we have a better way to handle internal files. +source/extensions/omni.isaac.assets + +# RL-Games +**/runs/* +**/logs/* diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000000..f91599f38c --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,46 @@ +repos: + - repo: https://github.com/python/black + rev: 22.10.0 + hooks: + - id: black + args: ["--line-length", "120"] + - repo: https://github.com/pycqa/flake8 + rev: 6.0.0 + hooks: + - id: flake8 + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: trailing-whitespace + - id: check-yaml + - id: check-merge-conflict + - id: check-case-conflict + - id: check-executables-have-shebangs + - id: check-toml + - id: end-of-file-fixer + - repo: https://github.com/pycqa/isort + rev: 5.10.1 + hooks: + - id: isort + name: isort (python) + args: ["--profile", "black", "--filter-files"] + - repo: https://github.com/asottile/pyupgrade + rev: v3.3.0 + hooks: + - id: pyupgrade + args: ["--py37-plus"] + # FIXME: Figure out why this is getting stuck under VPN. + # - repo: https://github.com/RobertCraigie/pyright-python + # rev: v1.1.282 + # hooks: + # - id: pyright + # Note: We disable this by default since not all code is compatible with it. + # - repo: https://github.com/Lucas-C/pre-commit-hooks + # rev: v1.3.1 + # hooks: + # - id: insert-license + # files: \.py$ + # args: + # # - --remove-header # Remove existing license headers. Useful when updating license. + # - --license-filepath + # - .github/LICENSE_HEADER.txt diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000000..b4f0b50306 --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,12 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "ms-vscode.cpptools", + "ms-python.python", + "ms-python.vscode-pylance", + "ban.spellright", + "ms-iot.vscode-ros", + "ExecutableBookProject.myst-highlight", + ] +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000000..3736746e14 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,36 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Python: Current File", + "type": "python", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal", + "env": { + "EXP_PATH": "${workspaceFolder}/_isaac_sim/apps", + "RESOURCE_NAME": "IsaacSim" + }, + "python": "${workspaceFolder}/_isaac_sim/python.sh", + "envFile": "${workspaceFolder}/.vscode/.python.env", + "preLaunchTask": "setup_python_env" + }, + { + "name": "Python: Attach (windows-x86_64/linux-x86_64)", + "type": "python", + "request": "attach", + "localRoot": "${workspaceFolder}", + "remoteRoot": "${workspaceFolder}", + "port": 3000, + "host": "localhost", + "subProcess": true, + "runtimeArgs": [ + "--preserve-symlinks", + "--preserve-symlinks-main" + ] + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..350e699d3a --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,70 @@ +{ + "files.associations": { + "*.tpp": "cpp", + "*.kit": "toml", + "*.rst": "restructuredtext", + }, + "editor.rulers": [120], + + // files to be ignored by the linter + "files.watcherExclude": { + "**/.git/objects/**": true, + "**/.git/subtree-cache/**": true, + "**/node_modules/**": true, + "**/_isaac_sim/**": true, + "**/_compiler/**": true + }, + // Configuration for spelling checker + "spellright.language": [ + "en-US-10-1." + ], + "spellright.documentTypes": [ + "markdown", + "latex", + "plaintext", + "cpp", + "asciidoc", + "python", + "restructuredtext", + ], + "cSpell.words": [ + "literalinclude", + "linenos", + "instanceable", + "isaacSim", + "jacobians", + "pointcloud", + "ridgeback", + "rllib", + "robomimic", + "teleoperation", + "xform", + "numpy", + ], + // 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 docstring gnerator + "autoDocstring.docstringFormat": "google", + "autoDocstring.guessTypes": true, + // Python environment path + "python.defaultInterpreterPath": "${workspaceFolder}/_isaac_sim/kit/python/bin/python3", + "python.envFile": "${workspaceFolder}/.vscode/.python.env", + // ROS distribution + "ros.distro": "noetic", + // Language specific settings + "[python]": { + "editor.tabSize": 4 + }, + "[restructuredtext]": { + "editor.tabSize": 2 + } +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000000..3178c536b6 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,21 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "setup_python_env", + "type": "shell", + "linux": { + "command": "export CARB_APP_PATH=${workspaceFolder}/_isaac_sim/kit && export ISAAC_PATH=${workspaceFolder}/_isaac_sim && export EXP_PATH=${workspaceFolder}/_isaac_sim/apps && source ${workspaceFolder}/_isaac_sim/setup_python_env.sh && printenv >${workspaceFolder}/.vscode/.python.env" + } + }, + { + "label": "run_formatter", + "type": "shell", + "linux": { + "command": "pre-commit run --all-files" + } + } + ] +} diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000000..d19127dfff --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +Copyright (c) 2022, ETH Zurich +Copyright (c) 2022, University of Toronto +Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES +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/README.md b/README.md index 052ec68e65..96d4206557 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,37 @@ -# orbit -Nvidia Isaac Orbit Environment Suite +![Example Tasks created with ORBIT](docs/source/_static/tasks.jpg) + +--- + +# Omniverse Isaac Orbit + +[![IsaacSim](https://img.shields.io/badge/Isaac%20Sim-2022.2.0-orange.svg)](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html) +[![Python](https://img.shields.io/badge/python-3.7-blue.svg)](https://docs.python.org/3/whatsnew/3.7.html) +[![Linux platform](https://img.shields.io/badge/platform-linux--64-lightgrey.svg)](https://releases.ubuntu.com/20.04/) +[![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit&logoColor=white)](https://pre-commit.com/) +[![Docs status](https://img.shields.io/badge/docs-passing-brightgreen.svg)](https://isaac-orbit.github.io/orbit) +[![License](https://img.shields.io/badge/license-BSD--3-yellow.svg)](https://opensource.org/licenses/BSD-3-Clause) + + + +Isaac Orbit (or *orbit* in short) is a unified and modular framework for robot learning powered by [NVIDIA Isaac Sim](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html). It offers a modular design to easily and efficiently create robotic environments with photo-realistic scenes, and fast and accurate simulation. + +Please refer our [documentation page](https://isaac-orbit.github.io/orbit) to learn more about the installation steps, features and tutorials. + +## Acknowledgement + +NVIDIA Isaac Sim is available freely under [individual license](https://www.nvidia.com/en-us/omniverse/download/). For more information about its license terms, please check [here](https://docs.omniverse.nvidia.com/app_isaacsim/common/NVIDIA_Omniverse_License_Agreement.html#software-support-supplement). + +ORBIT framework is released under [BSD-3 License](LICENSE). The license files of its dependencies and assets are present in the [`docs/licenses`](docs/licenses) directory. + +## Citation + +If you use **`orbit`** in your work, please cite: + +``` +@misc{mittal2023orbit, + author = {Mayank Mittal and Calvin Yu and Qinxi Yu and Jingzhou Liu and Nikita Rudin and David Hoeller and Jia Lin Yuan and Pooria Poorsarvi Tehrani and Ritvik Singh and Yunrong Guo and Hammad Mazhar and Ajay Mandlekar and Buck Babich and Gavriel State and Marco Hutter and Animesh Garg}, + title = {ORBIT: A Unified Simulation Framework for Interactive Robot Learning Environments}, + year = {2023}, + eprint = {arXiv:2301.04195}, +} +``` diff --git a/VERSION b/VERSION new file mode 100644 index 0000000000..6e8bf73aa5 --- /dev/null +++ b/VERSION @@ -0,0 +1 @@ +0.1.0 diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000000..d4bb2cbb9e --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,20 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/README.md b/docs/README.md new file mode 100644 index 0000000000..c154e0ad0f --- /dev/null +++ b/docs/README.md @@ -0,0 +1,30 @@ +# Building Documentation + +We use [Sphinx](https://www.sphinx-doc.org/en/master/) with the [Book Theme](https://sphinx-book-theme.readthedocs.io/en/stable/) for maintaining the documentation. + +> **Note:** To build the documentation, we recommend creating a virtual environment to avoid any conflicts with system installed dependencies. + +Execute the following instructions to build the documentation (assumed from the top of the repository): + +1. Install the dependencies for [Sphinx](https://www.sphinx-doc.org/en/master/): + + ```bash + # enter the location where this readme exists + cd docs + # install dependencies + pip install -r requirements.txt + ``` + +2. Generate the documentation file via: + + ```bash + # make the html version + make html + ``` + +3. The documentation is now available at `docs/_build/html/index.html`: + + ```bash + # open on default browser + xdg-open _build/html/index.html + ``` diff --git a/docs/conf.py b/docs/conf.py new file mode 100644 index 0000000000..212f7e19b7 --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,148 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys + +sys.path.insert(0, os.path.abspath("../source/extensions/omni.isaac.orbit")) +sys.path.insert(0, os.path.abspath("../source/extensions/omni.isaac.orbit/omni/isaac/orbit")) +sys.path.insert(0, os.path.abspath("../source/extensions/omni.isaac.orbit_envs")) +sys.path.insert(0, os.path.abspath("../source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs")) + +# -- Project information ----------------------------------------------------- + +project = "orbit" +copyright = "2022, NVIDIA, ETH Zurich and University of Toronto" +author = "NVIDIA, ETH Zurich and University of Toronto" + +version = "0.1.0" + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.napoleon", + "sphinx.ext.mathjax", + "sphinx.ext.intersphinx", + "sphinx.ext.autosummary", + "sphinx.ext.githubpages", + "sphinx.ext.todo", + "sphinxcontrib.bibtex", + "myst_parser", + "autodocsumm", +] + +# mathjax hacks +mathjax3_config = { + "tex": { + "inlineMath": [["\\(", "\\)"]], + "displayMath": [["\\[", "\\]"]], + }, +} + +# supported file extensions for source files +source_suffix = { + ".rst": "restructuredtext", + ".md": "markdown", +} + +# put type hints inside the description instead of the signature (easier to read) +autodoc_typehints = "description" +autodoc_typehints_description_target = "documented" +# document class *and* __init__ methods +autoclass_content = "class" # +# separate class docstring from __init__ docstring +autodoc_class_signature = "separated" +# sort members by source order +autodoc_member_order = "groupwise" +# BibTeX configuration +bibtex_bibfiles = ["source/_static/refs.bib"] +# default autodoc settings +autodoc_default_options = { + "autosummary": True, +} + +# generate links to the documentation of objects in external projects +intersphinx_mapping = { + "python": ("https://docs.python.org/3", None), + "numpy": ("http://docs.scipy.org/doc/numpy/", None), + "torch": ("https://pytorch.org/docs/stable/", None), +} + +# Add any paths that contain templates here, relative to this directory. +templates_path = [] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store", "README.md", "licenses/*"] + +# Mock out modules that are not available on RTD +autodoc_mock_imports = [ + "torch", + "numpy", + "scipy", + "carb", + "pxr", + "omni.kit", + "omni.usd", + "omni.client", + "pxr.PhysxSchema", + "pxr.PhysicsSchemaTools", + "omni.replicator", + "omni.isaac.core", + "omni.isaac.kit", + "omni.isaac.cloner", + "gym", + "stable_baselines3", + "rsl_rl", + "rl_games", + "ray", + "h5py", + "hid", + "prettytable", +] + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +import sphinx_book_theme + +html_theme_path = [sphinx_book_theme.get_html_theme_path()] +html_theme = "sphinx_book_theme" +html_logo = "source/_static/nv-logo.png" +html_favicon = "source/_static/favicon.ico" + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["source/_static"] +html_css_files = ["css/nvidia.css"] + +html_theme_options = { + "collapse_navigation": True, + "repository_url": "https://github.com/NVIDIA-Omniverse/Orbit", + "use_repository_button": True, + "use_issues_button": True, + "use_edit_page_button": True, + "show_toc_level": 2, + "use_sidenotes": True, + "announcement": "⚠️This is a pre-release version of Orbit. Please report any issues on GitHub.", +} + +html_show_copyright = True +html_show_sphinx = False diff --git a/docs/index.rst b/docs/index.rst new file mode 100644 index 0000000000..802a3da3d0 --- /dev/null +++ b/docs/index.rst @@ -0,0 +1,77 @@ +Overview +======== + +**Isaac Orbit** (or *orbit* in short) is a unified and modular framework, built on top of `NVIDIA +Omniverse `__ and `Isaac +Sim `__, +for robot learning. It offers a modular design to easily and efficiently +create robot learning environments with photo-realistic scenes, and fast +and efficient simulation. + +.. figure:: source/_static/tasks.jpg + :width: 100% + :alt: Example tasks created using orbit + + +If you use ``orbit`` in your work, please cite the `paper `_ +using the following BibTeX entry: + +.. code-block:: bibtex + + @misc{mittal2023orbit, + author = {Mayank Mittal and Calvin Yu and Qinxi Yu and Jingzhou Liu and Nikita Rudin and David Hoeller and Jia Lin Yuan and Pooria Poorsarvi Tehrani and Ritvik Singh and Yunrong Guo and Hammad Mazhar and Ajay Mandlekar and Buck Babich and Gavriel State and Marco Hutter and Animesh Garg}, + title = {ORBIT: A Unified Simulation Framework for Interactive Robot Learning Environments}, + year = {2023}, + eprint = {arXiv:2301.04195}, + } + + +.. toctree:: + :maxdepth: 2 + :caption: Getting Started + + source/setup/installation + source/setup/developer + source/setup/sample + +.. toctree:: + :maxdepth: 2 + :caption: Features + + source/features/actuators + source/features/motion_generators + +.. toctree:: + :maxdepth: 1 + :caption: Tutorials (beginner) + + source/tutorials/00_empty + source/tutorials/01_arms + source/tutorials/02_cloner + source/tutorials/03_ik_controller + source/tutorials/04_gym_env + +.. toctree:: + :maxdepth: 2 + :caption: Source API + + source/api/index + +.. toctree:: + :maxdepth: 1 + :caption: References + + source/refs/contributing + source/refs/troubleshooting + source/refs/changelog + source/refs/roadmap + source/refs/license + source/refs/bibliography + + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` diff --git a/docs/licenses/assets/anymal_b-license.txt b/docs/licenses/assets/anymal_b-license.txt new file mode 100644 index 0000000000..5d62a45504 --- /dev/null +++ b/docs/licenses/assets/anymal_b-license.txt @@ -0,0 +1,29 @@ +Copyright 2019, ANYbotics AG. + +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/assets/anymal_c-license.txt b/docs/licenses/assets/anymal_c-license.txt new file mode 100644 index 0000000000..4defe40774 --- /dev/null +++ b/docs/licenses/assets/anymal_c-license.txt @@ -0,0 +1,29 @@ +Copyright 2020, ANYbotics AG. + +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/assets/franka-license.txt b/docs/licenses/assets/franka-license.txt new file mode 100644 index 0000000000..d9a10c0d8e --- /dev/null +++ b/docs/licenses/assets/franka-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/assets/kinova-license.txt b/docs/licenses/assets/kinova-license.txt new file mode 100644 index 0000000000..9b1705a232 --- /dev/null +++ b/docs/licenses/assets/kinova-license.txt @@ -0,0 +1,95 @@ +Copyright (c) 2017, Kinova Robotics 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 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. + +============================================================================== + +Copyright (c) 2018, Kinova 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 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. + +____________________________________________________________________ + + +Protocol Buffer license + +Copyright 2008 Google 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 Google Inc. 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. + +Code generated by the Protocol Buffer compiler is owned by the owner +of the input file used when generating it. This code is not +standalone and requires a support library to be linked with it. This +support library is itself covered by the above license. diff --git a/docs/licenses/assets/robotiq-license.txt b/docs/licenses/assets/robotiq-license.txt new file mode 100644 index 0000000000..78c0f73570 --- /dev/null +++ b/docs/licenses/assets/robotiq-license.txt @@ -0,0 +1,23 @@ +Copyright (c) 2013, ROS-Industrial +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/black-license.txt b/docs/licenses/dependencies/black-license.txt new file mode 100644 index 0000000000..7a9b891f71 --- /dev/null +++ b/docs/licenses/dependencies/black-license.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2018 Ł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. diff --git a/docs/licenses/dependencies/flake8-license.txt b/docs/licenses/dependencies/flake8-license.txt new file mode 100644 index 0000000000..e5e3d6f940 --- /dev/null +++ b/docs/licenses/dependencies/flake8-license.txt @@ -0,0 +1,22 @@ +== 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. diff --git a/docs/licenses/dependencies/gym-license.txt b/docs/licenses/dependencies/gym-license.txt new file mode 100644 index 0000000000..979a5ce5ae --- /dev/null +++ b/docs/licenses/dependencies/gym-license.txt @@ -0,0 +1,34 @@ +The MIT License + +Copyright (c) 2016 OpenAI (https://openai.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. + +# Mujoco models +This work is derived from [MuJuCo models](http://www.mujoco.org/forum/index.php?resources/) used under the following license: +``` +This file is part of MuJoCo. +Copyright 2009-2015 Roboti LLC. +Mujoco :: Advanced physics simulation engine +Source : www.roboti.us +Version : 1.31 +Released : 23Apr16 +Author :: Vikash Kumar +Contacts : kumar@roboti.us +``` diff --git a/docs/licenses/dependencies/h5py-license.txt b/docs/licenses/dependencies/h5py-license.txt new file mode 100644 index 0000000000..28ca56277f --- /dev/null +++ b/docs/licenses/dependencies/h5py-license.txt @@ -0,0 +1,30 @@ +Copyright (c) 2008 Andrew Collette 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: + +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/hidapi-license.txt b/docs/licenses/dependencies/hidapi-license.txt new file mode 100644 index 0000000000..538cdf95cf --- /dev/null +++ b/docs/licenses/dependencies/hidapi-license.txt @@ -0,0 +1,26 @@ +Copyright (c) 2010, Alan Ott, Signal 11 Software +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 Signal 11 Software 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 new file mode 100644 index 0000000000..80cff4a451 --- /dev/null +++ b/docs/licenses/dependencies/isaacsim-license.txt @@ -0,0 +1,13 @@ +Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + +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. + +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. + +For more information: https://docs.omniverse.nvidia.com/app_isaacsim/common/NVIDIA_Omniverse_License_Agreement.html + +For sub-dependencies of Isaac Sim: https://docs.omniverse.nvidia.com/app_isaacsim/common/licenses.html diff --git a/docs/licenses/dependencies/isort-license.txt b/docs/licenses/dependencies/isort-license.txt new file mode 100644 index 0000000000..b5083a50d8 --- /dev/null +++ b/docs/licenses/dependencies/isort-license.txt @@ -0,0 +1,21 @@ +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. diff --git a/docs/licenses/dependencies/matplotlib-license.txt b/docs/licenses/dependencies/matplotlib-license.txt new file mode 100644 index 0000000000..0752df48ef --- /dev/null +++ b/docs/licenses/dependencies/matplotlib-license.txt @@ -0,0 +1,99 @@ +License agreement for matplotlib versions 1.3.0 and later +========================================================= + +1. This LICENSE AGREEMENT is between the Matplotlib Development Team +("MDT"), and the Individual or Organization ("Licensee") accessing and +otherwise using matplotlib software in source or binary form and its +associated documentation. + +2. Subject to the terms and conditions of this License Agreement, MDT +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 matplotlib +alone or in any derivative version, provided, however, that MDT's +License Agreement and MDT's notice of copyright, i.e., "Copyright (c) +2012- Matplotlib Development Team; All Rights Reserved" are retained in +matplotlib alone or in any derivative version prepared by +Licensee. + +3. In the event Licensee prepares a derivative work that is based on or +incorporates matplotlib 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 matplotlib . + +4. MDT is making matplotlib available to Licensee on an "AS +IS" basis. MDT MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR +IMPLIED. BY WAY OF EXAMPLE, BUT NOT LIMITATION, MDT MAKES NO AND +DISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS +FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF MATPLOTLIB +WILL NOT INFRINGE ANY THIRD PARTY RIGHTS. + +5. MDT SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF MATPLOTLIB + FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR +LOSS AS A RESULT OF MODIFYING, DISTRIBUTING, OR OTHERWISE USING +MATPLOTLIB , 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 MDT and +Licensee. This License Agreement does not grant permission to use MDT +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 matplotlib , +Licensee agrees to be bound by the terms and conditions of this License +Agreement. + +License agreement for matplotlib versions prior to 1.3.0 +======================================================== + +1. This LICENSE AGREEMENT is between John D. Hunter ("JDH"), and the +Individual or Organization ("Licensee") accessing and otherwise using +matplotlib software in source or binary form and its associated +documentation. + +2. Subject to the terms and conditions of this License Agreement, JDH +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 matplotlib +alone or in any derivative version, provided, however, that JDH's +License Agreement and JDH's notice of copyright, i.e., "Copyright (c) +2002-2011 John D. Hunter; All Rights Reserved" are retained in +matplotlib alone or in any derivative version prepared by +Licensee. + +3. In the event Licensee prepares a derivative work that is based on or +incorporates matplotlib 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 matplotlib. + +4. JDH is making matplotlib available to Licensee on an "AS +IS" basis. JDH MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR +IMPLIED. BY WAY OF EXAMPLE, BUT NOT LIMITATION, JDH MAKES NO AND +DISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS +FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF MATPLOTLIB +WILL NOT INFRINGE ANY THIRD PARTY RIGHTS. + +5. JDH SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF MATPLOTLIB + FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR +LOSS AS A RESULT OF MODIFYING, DISTRIBUTING, OR OTHERWISE USING +MATPLOTLIB , 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 JDH and +Licensee. This License Agreement does not grant permission to use JDH +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 matplotlib, +Licensee agrees to be bound by the terms and conditions of this License +Agreement. diff --git a/docs/licenses/dependencies/numpy-license.txt b/docs/licenses/dependencies/numpy-license.txt new file mode 100644 index 0000000000..e1fb614d66 --- /dev/null +++ b/docs/licenses/dependencies/numpy-license.txt @@ -0,0 +1,30 @@ +Copyright (c) 2005-2022, NumPy 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 NumPy Developers nor the names of any + 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/onnx-license.txt b/docs/licenses/dependencies/onnx-license.txt new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/docs/licenses/dependencies/onnx-license.txt @@ -0,0 +1,202 @@ + + 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/open3d-license.txt b/docs/licenses/dependencies/open3d-license.txt new file mode 100644 index 0000000000..79d162876d --- /dev/null +++ b/docs/licenses/dependencies/open3d-license.txt @@ -0,0 +1,22 @@ +The MIT License (MIT) + +Open3D: www.open3d.org +Copyright (c) 2018-2021 www.open3d.org + +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/pillow-license.txt b/docs/licenses/dependencies/pillow-license.txt new file mode 100644 index 0000000000..40aabc3239 --- /dev/null +++ b/docs/licenses/dependencies/pillow-license.txt @@ -0,0 +1,30 @@ +The Python Imaging Library (PIL) is + + Copyright © 1997-2011 by Secret Labs AB + Copyright © 1995-2011 by Fredrik Lundh + +Pillow is the friendly PIL fork. It is + + Copyright © 2010-2022 by Alex Clark and contributors + +Like PIL, Pillow is licensed under the open source HPND License: + +By obtaining, using, and/or copying this software and/or its associated +documentation, you agree that you have read, understood, and will comply +with the following terms and conditions: + +Permission to use, copy, modify, and distribute this software and its +associated documentation for any purpose and without fee is hereby granted, +provided that the above copyright notice appears in all copies, and that +both that copyright notice and this permission notice appear in supporting +documentation, and that the name of Secret Labs AB or the author not be +used in advertising or publicity pertaining to distribution of the software +without specific, written prior permission. + +SECRET LABS AB AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS +SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. +IN NO EVENT SHALL SECRET LABS AB OR THE AUTHOR 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. diff --git a/docs/licenses/dependencies/pre-commit-hooks-license.txt b/docs/licenses/dependencies/pre-commit-hooks-license.txt new file mode 100644 index 0000000000..4a071fc533 --- /dev/null +++ b/docs/licenses/dependencies/pre-commit-hooks-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2014 pre-commit dev team: Anthony Sottile, Ken Struys + +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/pre-commit-license.txt b/docs/licenses/dependencies/pre-commit-license.txt new file mode 100644 index 0000000000..4a071fc533 --- /dev/null +++ b/docs/licenses/dependencies/pre-commit-license.txt @@ -0,0 +1,19 @@ +Copyright (c) 2014 pre-commit dev team: Anthony Sottile, Ken Struys + +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/protobuf-license.txt b/docs/licenses/dependencies/protobuf-license.txt new file mode 100644 index 0000000000..19b305b000 --- /dev/null +++ b/docs/licenses/dependencies/protobuf-license.txt @@ -0,0 +1,32 @@ +Copyright 2008 Google 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 Google Inc. 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. + +Code generated by the Protocol Buffer compiler is owned by the owner +of the input file used when generating it. This code is not +standalone and requires a support library to be linked with it. This +support library is itself covered by the above license. diff --git a/docs/licenses/dependencies/pyright-license.txt b/docs/licenses/dependencies/pyright-license.txt new file mode 100644 index 0000000000..dd44ffc071 --- /dev/null +++ b/docs/licenses/dependencies/pyright-license.txt @@ -0,0 +1,47 @@ +MIT License + +Copyright (c) 2021 Robert Craigie + +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. + + +=============================================================================== + +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 diff --git a/docs/licenses/dependencies/pyupgrade-license.txt b/docs/licenses/dependencies/pyupgrade-license.txt new file mode 100644 index 0000000000..522fbe20b8 --- /dev/null +++ b/docs/licenses/dependencies/pyupgrade-license.txt @@ -0,0 +1,19 @@ +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. diff --git a/docs/licenses/dependencies/ray-license.txt b/docs/licenses/dependencies/ray-license.txt new file mode 100644 index 0000000000..b813b34b89 --- /dev/null +++ b/docs/licenses/dependencies/ray-license.txt @@ -0,0 +1,450 @@ + 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. + +-------------------------------------------------------------------------------- + +Code in python/ray/rllib/{evolution_strategies, dqn} adapted from +https://github.com/openai (MIT License) + +Copyright (c) 2016 OpenAI (http://openai.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. + +-------------------------------------------------------------------------------- + +Code in python/ray/rllib/impala/vtrace.py from +https://github.com/deepmind/scalable_agent + +Copyright 2018 Google LLC + +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 + + https://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. + +-------------------------------------------------------------------------------- +Code in python/ray/rllib/ars is adapted from https://github.com/modestyachts/ARS + +Copyright (c) 2018, ARS contributors (Horia Mania, Aurelia Guy, Benjamin Recht) +All rights reserved. + +Redistribution and use of ARS 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. + +------------------ +Code in python/ray/_private/prometheus_exporter.py is adapted from https://github.com/census-instrumentation/opencensus-python/blob/master/contrib/opencensus-ext-prometheus/opencensus/ext/prometheus/stats_exporter/__init__.py + +# Copyright 2018, OpenCensus Authors +# +# 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. + +-------------------------------------------------------------------------------- +Code in python/ray/tests/modin/test_modin and +python/ray/tests/modin/modin_test_utils adapted from: +- http://github.com/modin-project/modin/master/modin/pandas/test/test_general.py +- http://github.com/modin-project/modin/master/modin/pandas/test/utils.py + +Copyright (c) 2018-2020 Modin Developers. + +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 + + https://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. + +-------------------------------------------------------------------------------- +Code in src/ray/util/logging.h is adapted from +https://github.com/google/glog/blob/master/src/glog/logging.h.in + +Copyright (c) 2008, Google 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 Google Inc. 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. + +-------------------------------------------------------------------------------- +Code in python/ray/_private/runtime_env/conda_utils.py is adapted from +https://github.com/mlflow/mlflow/blob/master/mlflow/utils/conda.py + +Copyright (c) 2018, Databricks, Inc. +All rights reserved. + +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. + +-------------------------------------------------------------------------------- +Code in python/ray/_private/runtime_env/_clonevirtualenv.py is adapted from https://github.com/edwardgeorge/virtualenv-clone/blob/master/clonevirtualenv.py + +Copyright (c) 2011, Edward George, based on code contained within the +virtualenv project. + +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. + +-------------------------------------------------------------------------------- +Code in python/ray/_private/async_compat.py is adapted from +https://github.com/python-trio/async_generator/blob/master/async_generator/_util.py + +Copyright (c) 2022, Nathaniel J. Smith + +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. + +--------------------------------------------------------------------------------------------------------------- +Code in python/ray/_private/thirdparty/tabulate/tabulate.py is adapted from https://github.com/astanin/python-tabulate/blob/4892c6e9a79638c7897ccea68b602040da9cc7a7/tabulate.py + +Copyright (c) 2011-2020 Sergey Astanin 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. + +-------------------------------------------------------------------------------- +Code in python/ray/_private/thirdparty/dacite is adapted from https://github.com/konradhalas/dacite/blob/master/dacite + +Copyright (c) 2018 Konrad Hałas + +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/rl_games-license.txt b/docs/licenses/dependencies/rl_games-license.txt new file mode 100644 index 0000000000..313ca229e6 --- /dev/null +++ b/docs/licenses/dependencies/rl_games-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019 Denys88 + +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/robomimc-license.txt b/docs/licenses/dependencies/robomimc-license.txt new file mode 100644 index 0000000000..934eaa87bb --- /dev/null +++ b/docs/licenses/dependencies/robomimc-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/rsl_rl-license.txt b/docs/licenses/dependencies/rsl_rl-license.txt new file mode 100644 index 0000000000..5e09cdaf7e --- /dev/null +++ b/docs/licenses/dependencies/rsl_rl-license.txt @@ -0,0 +1,141 @@ +Copyright (c) 2021, ETH Zurich, Nikita Rudin +Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES +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. + +See licenses/dependencies for license information of dependencies of this package. + +=============================================================================== + +Copyright (c) 2005-2021, NumPy 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 NumPy Developers nor the names of any + 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. + +=============================================================================== + +From PyTorch: + +Copyright (c) 2016- Facebook, Inc (Adam Paszke) +Copyright (c) 2014- Facebook, Inc (Soumith Chintala) +Copyright (c) 2011-2014 Idiap Research Institute (Ronan Collobert) +Copyright (c) 2012-2014 Deepmind Technologies (Koray Kavukcuoglu) +Copyright (c) 2011-2012 NEC Laboratories America (Koray Kavukcuoglu) +Copyright (c) 2011-2013 NYU (Clement Farabet) +Copyright (c) 2006-2010 NEC Laboratories America (Ronan Collobert, Leon Bottou, Iain Melvin, Jason Weston) +Copyright (c) 2006 Idiap Research Institute (Samy Bengio) +Copyright (c) 2001-2004 Idiap Research Institute (Ronan Collobert, Samy Bengio, Johnny Mariethoz) + +=============================================================================== + +From Caffe2: + +Copyright (c) 2016-present, Facebook Inc. All rights reserved. + +All contributions by Facebook: +Copyright (c) 2016 Facebook Inc. + +All contributions by Google: +Copyright (c) 2015 Google Inc. +All rights reserved. + +All contributions by Yangqing Jia: +Copyright (c) 2015 Yangqing Jia +All rights reserved. + +All contributions by Kakao Brain: +Copyright 2019-2020 Kakao Brain + +All contributions from Caffe: +Copyright(c) 2013, 2014, 2015, the respective contributors +All rights reserved. + +All other contributions: +Copyright(c) 2015, 2016 the respective contributors +All rights reserved. + +Caffe2 uses a copyright model similar to Caffe: each contributor holds +copyright over their contributions to Caffe2. The project versioning records +all such contribution and copyright details. If a contributor wants to further +mark their specific copyright on a particular contribution, they should +indicate their copyright solely in the commit message of the change when it is +committed. + +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 names of Facebook, Deepmind Technologies, NYU, NEC Laboratories America + and IDIAP Research Institute 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/tensorboard-license.txt b/docs/licenses/dependencies/tensorboard-license.txt new file mode 100644 index 0000000000..15ae421404 --- /dev/null +++ b/docs/licenses/dependencies/tensorboard-license.txt @@ -0,0 +1,203 @@ +Copyright 2017 The TensorFlow Authors. All rights reserved. + + 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 2017, The TensorFlow Authors. + + 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/torch-license.txt b/docs/licenses/dependencies/torch-license.txt new file mode 100644 index 0000000000..69570b3a73 --- /dev/null +++ b/docs/licenses/dependencies/torch-license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2016, Soumith Chintala, Ronan Collobert, Koray Kavukcuoglu, Clement Farabet +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 distro 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/wandb-license.txt b/docs/licenses/dependencies/wandb-license.txt new file mode 100644 index 0000000000..1f4e645404 --- /dev/null +++ b/docs/licenses/dependencies/wandb-license.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Weights and Biases, 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 new file mode 100644 index 0000000000..9fd8e1dc40 --- /dev/null +++ b/docs/licenses/dependencies/warp-license.txt @@ -0,0 +1,36 @@ +# NVIDIA Source Code License for Warp + +## 1. Definitions + +“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. + +## 2. License Grant + +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. + +## 3. Limitations + +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. + +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. + +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. + +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. + +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. + +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. + +## 4. Disclaimer of Warranty. + +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. + +## 5. Limitation of Liability. + +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. diff --git a/docs/make.bat b/docs/make.bat new file mode 100644 index 0000000000..2119f51099 --- /dev/null +++ b/docs/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=. +set BUILDDIR=_build + +if "%1" == "" goto help + +%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% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + +:end +popd diff --git a/docs/requirements.txt b/docs/requirements.txt new file mode 100644 index 0000000000..093610e3a6 --- /dev/null +++ b/docs/requirements.txt @@ -0,0 +1,11 @@ +# for building the docs +Jinja2==3.0.3 +Sphinx==4.5.0 +sphinx-book-theme==0.3.3 +myst-parser==0.18.1 +sphinxcontrib-bibtex==2.5.0 +autodocsumm==0.2.9 + +# basic python +numpy==1.24.1 +matplotlib==3.6.2 diff --git a/docs/source/_static/actuator_groups.svg b/docs/source/_static/actuator_groups.svg new file mode 100644 index 0000000000..4b37806cb3 --- /dev/null +++ b/docs/source/_static/actuator_groups.svg @@ -0,0 +1,10215 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + DC Motor + Actuator Net(MLP/LSTM) + + Gripper + + + Arm + Base + Mimic Group + + + + open/close (1) + joint position(6) + joint position(12) + joint torque(12) + joint torque(6) + joint velocity(6) + + Simulation + + + + Actions + + + + + + + + + + + diff --git a/docs/source/_static/css/nvidia.css b/docs/source/_static/css/nvidia.css new file mode 100644 index 0000000000..348adc1adb --- /dev/null +++ b/docs/source/_static/css/nvidia.css @@ -0,0 +1,352 @@ +/* Modified from https://github.com/ARISE-Initiative/robosuite */ + + +.wy-side-nav-search { + background: rgb(143,144,147); +} + +.wy-side-nav-search > div.version { + color: black; +} + + +.wy-nav-content-wrap { + background: inherit; +} + +.wy-side-nav-search input[type="text"] { + border: none; + box-shadow: none; + background: white; + border-radius: 0; + font-size: 100%; +} + +.wy-menu-vertical li.current a, +.wy-menu-vertical li.toctree-l1.current > a { + border: none; +} + +.ethical-rtd > div.ethical-sidebar, +.ethical-rtd > div.ethical-footer { + display: none !important; +} + +h1 { + /* text-transform: uppercase; */ + font-family: inherit; + font-weight: 200; +} + +h2, +.rst-content .toctree-wrapper p.caption { + font-family: inherit; + font-weight: 200; +} + +.rst-content a:visited { + color: #76b900; +} + +/* Begin code */ +.rst-content pre.literal-block, +.rst-content div[class^="highlight"] { + border: none; +} + +.rst-content pre.literal-block, +.rst-content div[class^="highlight"] pre, +.rst-content .linenodiv pre { + font-size: 80%; +} + +.highlight { + background: #f6f8fa; + border-radius: 6px; +} + +.highlight .kn, +.highlight .k { + color: #76b900; +} + +.highlight .nn { + color: inherit; + font-weight: inherit; +} + +.highlight .nc { + color: #76b900; + font-weight: inherit; +} + +.highlight .fm, +.highlight .nd, +.highlight .nf, +.highlight .nb { + color: #76b900; +} + +.highlight .bp, +.highlight .n { + color: inherit; +} + +.highlight .kc, +.highlight .s1, +.highlight .s2, +.highlight .mi, +.highlight .mf, +.highlight .bp, +.highlight .bn, +.highlight .ow { + color: #76b900; + font-weight: inherit; +} + +.highlight .c1 { + color: #6a737d; +} + +.rst-content code.xref { + padding: .2em .4em; + background: rgba(27,31,35,.05); + border-radius: 6px; + border: none; +} +/* End code */ + +/* This ensures that multiple constructors will remain in separate lines. */ +.rst-content dl:not(.docutils) dt, +.rst-content dl:not(.docutils) dl dt { + display: table; + background: rgb(243,244,247); + color: rgba(83, 150, 0, 0.9); + background-color: rgba(118, 185, 0, 0.15); + border-top-color: rgba(83, 150, 0, 0.9); +} + +.rst-content tt.literal, .rst-content tt.literal, .rst-content code.literal { + color: #76b900 !important; +} + +.rst-content dl:not(.docutils) dt.field-odd, +.rst-content dl:not(.docutils) dt.field-odd { + text-transform: uppercase; + background: inherit; + border: none; + padding: 6px 0; +} + +.rst-content dl:not(.docutils) .property { + text-transform: uppercase; + font-style: normal; + padding-right: 12px; +} + +em.sig-param span.n:first-child, em.sig-param span.n:nth-child(2) { + color: black; + font-style: normal; +} + +em.sig-param span.n:nth-child(3), +em.sig-param span.n:nth-child(3) a { + color: inherit; + font-weight: normal; + font-style: normal; +} + +em.sig-param span.default_value { + font-family: SFMono-Regular,Menlo,Monaco,Consolas,"Liberation Mono","Courier New",Courier,monospace; + font-style: normal; + font-size: 90%; +} + +.sig-paren { + padding: 0 4px; +} + +.wy-table-responsive table td, +.wy-table-responsive table th { + white-space: normal; +} + +.wy-table-bordered-all, +.rst-content table.docutils { + border: none; +} + +.wy-table-bordered-all td, +.rst-content table.docutils td { + border: none; +} + +.wy-table-odd td, +.wy-table-striped tr:nth-child(2n-1) td, +.rst-content table.docutils:not(.field-list) tr:nth-child(2n-1) td { + background: rgb(243,244,247); +} + +.wy-table td, +.rst-content table.docutils td, +.rst-content table.field-list td, +.wy-table th, +.rst-content table.docutils th, +.rst-content table.field-list th { + padding: 16px; +} +/* +.admonition { + content: '\f12a'; + font-family: FontAwesome; +} */ + +.admonition.note, div.admonition.note { + border-color: rgba(var(--pst-color-admonition-note),1); +} + +.admonition.note>.admonition-title:before, div.admonition.note>.admonition-title:before { + color: rgba(var(--pst-color-admonition-note),1); + content: '\f12a'!important; + /* content: var(--pst-icon-admonition-note); */ +} + +.admonition.question>.admonition-title:before, div.admonition.question>.admonition-title:before { + color: rgba(var(--pst-color-admonition-note),1); + content: '\003f'!important; + /* content: var(--pst-icon-admonition-note); */ +} + +.admonition.explanation>.admonition-title:before, div.admonition.explanation>.admonition-title:before { + color: rgba(var(--pst-color-admonition-note),1); + content: '\f02d'!important; + /* content: var(--pst-icon-admonition-note); */ +} + +.card { + /* Add shadows to create the "card" effect */ + box-shadow: 0 4px 8px 0 rgba(0,0,0,0.2); + transition: 0.3s; + border-radius: 5px; /* 5px rounded corners */ + width: 100%; + padding-bottom: 10px; +} + +/* On mouse-over, add a deeper shadow */ +.card:hover { + box-shadow: 0 8px 16px 0 rgba(0,0,0,0.2); + +} + +/* Add some padding inside the card container */ +.container { + padding: 2px 16px; +} + +.row:after { + content: ""; + display: table; + clear: both; +} + +.column { + float: left; + width: 50%; + padding: 20px 10px; +} + +.box{ + display: none; + width: 100%; +} + +/* Use NVIDIA Colors */ + +a { + color: #76b900; +} + +a:hover { + color: #76b900; +} + +a:visited { + color: #76b900; +} + +a:hover + .box,.box:hover{ + display: block; + position: absolute; + z-index: 100; + border-radius: 50px!important; + margin-left: 60px; + margin-top: 0px; + color: #76b900; +} + +a:hover + .card:hover{ + display: block; + position: absolute; + z-index: 100; + border-radius: 50px!important; + margin-left: 60px; + margin-top: 0px; + color: #76b900; +} + +a.reference.external { + color: #76b900!important; +} + +#p1 a { + color: #76b900!important; +} + + +#frame { zoom: 0.75; -moz-transform: scale(0.75); -moz-transform-origin: 0 0; } + +/* Global */ + +#typewriter body{ + height: calc(100vh - 8em); + padding: 4em; + color: rgba(255,255,255,1.0); + font-family: "Lato","proxima-nova","Helvetica Neue",Arial,sans-serif; + background-color: rgb(25,25,25); +} + +#typewriter .line-1{ + position: relative; + top: 50%; + width: 24em; + margin: 0 auto; + border-right: 2px solid rgba(255,255,255,.75); + font-size: 180%; + text-align: center; + white-space: nowrap; + overflow: hidden; + transform: translateY(-50%); +} + +/* Animation */ +.anim-typewriter{ + animation: typewriter 4s steps(44) 1s 1 normal both, + blinkTextCursor 500ms steps(44) infinite normal; +} +@keyframes typewriter{ + from{width: 0;} + to{width: 24em;} +} +@keyframes blinkTextCursor{ + from{border-right-color: rgba(255,255,255,.75);} + to{border-right-color: transparent;} +} + + +.trimmed-cover { + object-fit: cover; + width: 120%; + height: 177px; + object-position: center 40%; + margin-right: -100px; +} diff --git a/docs/source/_static/favicon.ico b/docs/source/_static/favicon.ico new file mode 100644 index 0000000000..7b39f8af7a Binary files /dev/null and b/docs/source/_static/favicon.ico differ diff --git a/docs/source/_static/nv-logo.png b/docs/source/_static/nv-logo.png new file mode 100644 index 0000000000..3cd1028ba0 Binary files /dev/null and b/docs/source/_static/nv-logo.png differ diff --git a/docs/source/_static/refs.bib b/docs/source/_static/refs.bib new file mode 100644 index 0000000000..ab191c0df9 --- /dev/null +++ b/docs/source/_static/refs.bib @@ -0,0 +1,88 @@ +@inproceedings{rudin2022learning, + title={Learning to walk in minutes using massively parallel deep reinforcement learning}, + author={Rudin, Nikita and Hoeller, David and Reist, Philipp and Hutter, Marco}, + booktitle={Conference on Robot Learning}, + pages={91--100}, + year={2022}, + organization={PMLR} +} + +@article{hwangbo2019learning, + title={Learning agile and dynamic motor skills for legged robots}, + author={Hwangbo, Jemin and Lee, Joonho and Dosovitskiy, Alexey and Bellicoso, Dario and Tsounis, Vassilios and Koltun, Vladlen and Hutter, Marco}, + journal={Science Robotics}, + volume={4}, + number={26}, + pages={eaau5872}, + year={2019}, + publisher={American Association for the Advancement of Science} +} + +@article{khatib1987osc, + author={Khatib, O.}, + journal={IEEE Journal on Robotics and Automation}, + title={A unified approach for motion and force control of robot manipulators: The operational space formulation}, + year={1987}, + volume={3}, + number={1}, + pages={43-53}, + doi={10.1109/JRA.1987.1087068} +} + +@book{siciliano2009force, + title={Force control}, + author={Siciliano, Bruno and Sciavicco, Lorenzo and Villani, Luigi and Oriolo, Giuseppe}, + year={2009}, + publisher={Springer} +} + +@article{cheng2021rmpflow, + author={Cheng, Ching-An and Mukadam, Mustafa and Issac, Jan and Birchfield, Stan and Fox, Dieter and Boots, Byron and Ratliff, Nathan}, + journal={IEEE Transactions on Automation Science and Engineering}, + title={RMPflow: A Geometric Framework for Generation of Multitask Motion Policies}, + year={2021}, + volume={18}, + number={3}, + pages={968-987}, + doi={10.1109/TASE.2021.3053422} +} + +@article{buss2004ik, + author = {Buss, Samuel}, + year = {2004}, + pages = {}, + title = {Introduction to inverse kinematics with Jacobian transpose, pseudoinverse and damped least squares methods}, + volume = {17}, + journal={IEEE Transactions in Robotics and Automation}, +} + +@article{sucan2012ompl, + Author = {Ioan A. {\c{S}}ucan and Mark Moll and Lydia E. Kavraki}, + Doi = {10.1109/MRA.2012.2205651}, + Journal = {{IEEE} Robotics \& Automation Magazine}, + Month = {December}, + Number = {4}, + Pages = {72--82}, + Title = {The {O}pen {M}otion {P}lanning {L}ibrary}, + Note = {\url{https://ompl.kavrakilab.org}}, + Volume = {19}, + Year = {2012} +} + +@article{mittal2021articulated, + title={Articulated object interaction in unknown scenes with whole-body mobile manipulation}, + author={Mittal, Mayank and Hoeller, David and Farshidian, Farbod and Hutter, Marco and Garg, Animesh}, + journal={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + year={2022} +} + +@INPROCEEDINGS{rudin2022advanced, + author={Rudin, Nikita and Hoeller, David and Bjelonic, Marko and Hutter, Marco}, + booktitle={2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + title={Advanced Skills by Learning Locomotion and Local Navigation End-to-End}, + year={2022}, + volume={}, + number={}, + pages={2497-2503}, + doi={10.1109/IROS47612.2022.9981198} +} diff --git a/docs/source/_static/tasks.jpg b/docs/source/_static/tasks.jpg new file mode 100644 index 0000000000..8aa96c179c Binary files /dev/null and b/docs/source/_static/tasks.jpg differ diff --git a/docs/source/_static/vscode_tasks.png b/docs/source/_static/vscode_tasks.png new file mode 100644 index 0000000000..41d5cdb471 Binary files /dev/null and b/docs/source/_static/vscode_tasks.png differ diff --git a/docs/source/api/index.rst b/docs/source/api/index.rst new file mode 100644 index 0000000000..925bd27d9e --- /dev/null +++ b/docs/source/api/index.rst @@ -0,0 +1,32 @@ +API Reference +============= + +omni.isaac.orbit extension +-------------------------- + +.. toctree:: + :maxdepth: 1 + + orbit.actuators.group + orbit.actuators.model + orbit.devices + orbit.markers + orbit.objects.articulated + orbit.objects.rigid + orbit.robots + orbit.utils + orbit.utils.assets + orbit.utils.kit + orbit.utils.math + orbit.utils.mdp + +omni.isaac.orbit_envs extension +------------------------------- + +.. toctree:: + :maxdepth: 1 + + orbit_envs.isaac_env + orbit_envs.utils + orbit_envs.utils.data_collector + orbit_envs.utils.wrappers diff --git a/docs/source/api/orbit.actuators.group.rst b/docs/source/api/orbit.actuators.group.rst new file mode 100644 index 0000000000..46023b7f1e --- /dev/null +++ b/docs/source/api/orbit.actuators.group.rst @@ -0,0 +1,174 @@ +omni.isaac.orbit.actuators.group +================================ + +Actuator groups apply the same actuator model over a collection of actuated joints. +It deals with both explicit and implicit models, and processes the input joint +configuration and actions accordingly. The joint names, that are a part of a given +actuator group, are configured using regex expressions. These expressions are matched +with the joint names in the robot's kinematic tree. For instance, in the Franka Panda +Emika arm, the first four joints and last three joints can be captured using the regex +expressions ``"panda_joint[1-4]"`` and ``"panda_joint[5-7]"``, + +For a given actuator group, it is possible to provide multiple joint-level command types +(e.g. position, velocity, torque, etc.). The command types are processed as a list of strings. +Each string has two sub-strings joined by underscore: + +- **type of command mode:** "p" (position), "v" (velocity), "t" (torque) +- **type of command resolving:** "abs" (absolute), "rel" (relative) + +For instance, the command type ``"p_abs"`` defines a position command in absolute mode, while ``"v_rel"`` +defines a velocity command in relative mode. + +To facilitate easier composibility, the actuator group handles the offsets and scalings applied to +the input commands. These are set through the :class:`ActuatorControlCfg` and by default are set to zero +and one, respectively. Based on these, the input commands are processed as follows: + +.. math:: + + \text{command} = \text{offset} + \text{scaling} \times \text{input command} + +where :math:`\text{command}` is the command that is sent to the actuator model, :math:`\text{offset}` +is the offset applied to the input command, :math:`\text{scaling}` is the scaling applied to the input +command, and :math:`\text{input command}` is the input command from the user. + +1. **Relative command:** The offset is based on the current joint state. For instance, if the + command type is "p_rel", the offset is the current joint position. +2. **Absolute command:** The offset is based on the values set in the :class:`ActuatorControlCfg`. + For instance, if the command type is "p_abs", the offset is the value for :attr:`dof_pos_offset` + in the :class:`ActuatorControlCfg` instance. + +.. note:: + + Currently, the following joint command types are supported: "p_abs", "p_rel", "v_abs", "v_rel", "t_abs". + + +On initialization, the actuator group performs the following: + +1. **Sets the control mode into simulation:** The control mode is set into the simulator for each joint + based on the command types and actuator models. For implicit actuator models, this is interpreted + from the first entry in the input list of command types. For explicit actuator models, the control + mode is always set to torque. +2. **Sets the joint stiffness and damping gains:** In case of implicit actuators, these are set into + simulator directly, while for explicit actuators, the gains are set into the actuator model. +3. **Sets the joint torque limits:** In case of implicit actuators, these are set into simulator directly + based on the parsed configuration. For explicit actuators, the torque limits are set high since the + actuator model itself is responsible for enforcing the torque limits. + +While computing the joint actions, the actuator group takes the following steps: + +1. **Formats the input actions:** It formats the input actions to account for additional constraints over + the joints. These include mimicking the input command over the joints, or considering non-holonomic steering + constraint for a wheel base. +2. **Pre-process joint commands:** It splits the formatted commands based on number of command types. For + instance, if the input command types are "p_abs" and "v_abs", the input command is split into two tensors. + Over each tensor, the scaling and offset are applied. +3. **Computes the joint actions:** The joint actions are computed based on the actuator model. For implicit + actuator models, the joint actions are returned directly. For explicit actuator models, the joint actions + are computed by calling the :meth:`IdealActuator.compute` and :meth:`IdealActuator.clip_torques` method. + + +Consider a scene with multiple Franka Emika Panda robots present in the stage at the USD paths *"/World/Robot_{n}"* +where *n* is the number of the instance. The following is an example of using the default actuator group to control +the robot with position and velocity commands: + +.. code-block:: python + + import torch + from omni.isaac.core.articulations import ArticulationView + from omni.isaac.orbit.actuator.model import IdealActuatorCfg + from omni.isaac.orbit.actuator.group import ActuatorControlCfg, ActuatorGroupCfg, ActuatorGroup + + # Note: We assume the stage is created and simulation is playing. + + # create an articulation view for the arms + # Reference: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/ext_omni_isaac_core.html + articulation_view = ArticulationView( + "/World/Robot_.*", "panda_arm", reset_xform_properties=False + ) + articulation_view.initialize() + + # create a configuration instance + # -- model + model_cfg = IdealActuatorCfg(motor_torque_limit=40, gear_ratio=1) + # -- control + control_cfg = ActuatorControlCfg( + command_types=["p_abs", "v_abs"], + stiffness={".*": 1000}, + damping={".*": 10} + ) + # -- group + group_cfg = ActuatorGroupCfg( + dof_names=["panda_joint[1-7]"], + model_cfg=model_cfg, + control_cfg=control_cfg, + ) + # create the actuator group + group = ActuatorGroup(group_cfg, articulation_view) + # clear history in the actuator model (if any) + group.reset() + + # create random commands + shape = (articulation_view.count, 7) + dof_pos_des, dof_vel_des = torch.rand(*shape), torch.rand(*shape) + # concatenate the commands into a single tensor + group_actions = torch.cat([dof_pos_des, dof_vel_des], dim=1) + # check that commands are of the correct shape + assert group_actions.shape == (group.num_articulation, group.control_dim) + + # read current joint state + dof_pos = articulation_view.get_joint_positions(joint_indices=group.dof_indices) + dof_vel = articulation_view.get_joint_velocities(joint_indices=group.dof_indices) + + # compute the joint actions + joint_actions = group.compute(group_actions, dof_pos, dof_vel) + # set actions into simulator + articulation_view.apply_actions(joint_actions) + + +Actuator Control Configuration +------------------------------ + +.. autoclass:: omni.isaac.orbit.actuators.group.ActuatorControlCfg + :members: + :undoc-members: + :show-inheritance: + +Default Actuator Group +---------------------- + +.. autoclass:: omni.isaac.orbit.actuators.group.ActuatorGroup + :members: + :undoc-members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.group.ActuatorGroupCfg + :members: + :undoc-members: + :show-inheritance: + + +Gripper Actuator Group +----------------------- + +.. autoclass:: omni.isaac.orbit.actuators.group.GripperActuatorGroup + :members: + :undoc-members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.group.GripperActuatorGroupCfg + :members: + :undoc-members: + :show-inheritance: + +Non-Holonomic Kinematics Group +------------------------------ + +.. autoclass:: omni.isaac.orbit.actuators.group.NonHolonomicKinematicsGroup + :members: + :undoc-members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.group.NonHolonomicKinematicsGroupCfg + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.actuators.model.rst b/docs/source/api/orbit.actuators.model.rst new file mode 100644 index 0000000000..9046357ecd --- /dev/null +++ b/docs/source/api/orbit.actuators.model.rst @@ -0,0 +1,123 @@ +omni.isaac.orbit.actuators.model +================================ + +There are two main categories of actuator models that are supported: + +- **Implicit**: Motor model with ideal PD from the physics engine. +- **Explicit**: Motor models based on physical drive models. + + - **Physics-based**: Derives the motor models based on first-principles. + - **Neural Network-based**: Learned motor models from actuator data. + +Currently, all the explicit motor models convert input joint commands into joint efforts to +apply into the simulation. This process comprise of three main steps: + +1. :func:`set_command`: Set the desired command to the model. These include joint positions, velocities, feedforward torque, stiffness and damping gain. +2. :func:`compute_torque`: Compute the joint efforts using the actuator model. +3. :func:`clip_torques`: Clip the desired torques epxlicitly using an actuator saturation model. + +It is upto the model how the input values from step (1) are processed and dealt with in step (2). +The steps (2) and (3) are segregrated explicitly, since many times in learning, we need both the +computed (desired) or clipped (applied) joint efforts. For instance, to penalize the difference +between the computed and clipped joint efforts, so that the learned policy does not keep outputting +arbitrarily large commands. + +All explicit models inherit from the base actuator model, :class:`IdealActuator`, which implements a +PD controller with feed-forward effort, and simple clipping based on the configured maximum effort. + +The following is a simple example of using the actuator model: + +.. code-block:: python + + import torch + from omni.isaac.orbit.actuators.model import IdealActuator, IdealActuatorCfg + + # joint information from the articulation + num_actuators, num_envs = 5, 32 + device ="cpu" + # create a configuration instance + cfg = IdealActuatorCfg(motor_torque_limit=20, gear_ratio=1) + # create the actuator model instance + model = IdealActuator(cfg, num_actuators, num_envs, device) + # clear history in the actuator model (if any) + model.reset() + + # creat random commands + dof_pos_des, dof_vel_des = torch.rand(32, 5), torch.rand(32, 5) + # create random state + dof_pos, dof_vel = torch.rand(32, 5), torch.rand(32, 5) + + # set desired joint state + model.set_command(dof_pos_des, dof_vel_des) + # compute the torques + computed_torques = model.compute_torques(dof_pos, dof_vel) + applied_torques = model.clip_torques(computed_torques) + + +Implicit Actuator +----------------- + +.. autoclass:: omni.isaac.orbit.actuators.model.ImplicitActuatorCfg + :members: + :show-inheritance: + +Ideal Actuator +--------------- + +.. autoclass:: omni.isaac.orbit.actuators.model.IdealActuator + :members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.model.IdealActuatorCfg + :members: + :show-inheritance: + +Direct Control (DC) Actuator +---------------------------- + +Fixed Gear Ratio +~~~~~~~~~~~~~~~~ + +.. autoclass:: omni.isaac.orbit.actuators.model.DCMotor + :members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.model.DCMotorCfg + :members: + :show-inheritance: + +Variable Gear Ratio +~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: omni.isaac.orbit.actuators.model.VariableGearRatioDCMotor + :members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.model.VariableGearRatioDCMotorCfg + :members: + :show-inheritance: + +Actuator Networks +----------------- + +Multi-layer Perceptron (MLP) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: omni.isaac.orbit.actuators.model.ActuatorNetMLP + :members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.model.ActuatorNetMLPCfg + :members: + :show-inheritance: + +Long Short-term Memory (LSTM) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. autoclass:: omni.isaac.orbit.actuators.model.ActuatorNetLSTM + :members: + :show-inheritance: + +.. autoclass:: omni.isaac.orbit.actuators.model.ActuatorNetLSTMCfg + :members: + :show-inheritance: diff --git a/docs/source/api/orbit.devices.rst b/docs/source/api/orbit.devices.rst new file mode 100644 index 0000000000..eff66385c7 --- /dev/null +++ b/docs/source/api/orbit.devices.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.devices +======================== + +.. automodule:: omni.isaac.orbit.devices + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.markers.rst b/docs/source/api/orbit.markers.rst new file mode 100644 index 0000000000..ac1aa7ecaf --- /dev/null +++ b/docs/source/api/orbit.markers.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.markers +======================== + +.. automodule:: omni.isaac.orbit.markers + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.objects.articulated.rst b/docs/source/api/orbit.objects.articulated.rst new file mode 100644 index 0000000000..7d7912ec07 --- /dev/null +++ b/docs/source/api/orbit.objects.articulated.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.objects.articulated +==================================== + +.. automodule:: omni.isaac.orbit.objects.articulated + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.objects.rigid.rst b/docs/source/api/orbit.objects.rigid.rst new file mode 100644 index 0000000000..d773ee00ca --- /dev/null +++ b/docs/source/api/orbit.objects.rigid.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.objects.rigid +============================== + +.. automodule:: omni.isaac.orbit.objects.rigid + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.robots.rst b/docs/source/api/orbit.robots.rst new file mode 100644 index 0000000000..4d6ad14c59 --- /dev/null +++ b/docs/source/api/orbit.robots.rst @@ -0,0 +1,45 @@ +omni.isaac.orbit.robots +======================== + +Robot Base +---------- + +.. automodule:: omni.isaac.orbit.robots.robot_base + :members: + :undoc-members: + :show-inheritance: + +.. automodule:: omni.isaac.orbit.robots.robot_base_cfg + :members: + :undoc-members: + :show-inheritance: + +.. automodule:: omni.isaac.orbit.robots.robot_base_data + :members: + :undoc-members: + :show-inheritance: + +Legged Robot +------------ + +.. automodule:: omni.isaac.orbit.robots.legged_robot + :members: + :undoc-members: + :show-inheritance: + + +Single Arm Manipulator +---------------------- + +.. automodule:: omni.isaac.orbit.robots.single_arm + :members: + :undoc-members: + :show-inheritance: + +Mobile Manipulator +------------------ + +.. automodule:: omni.isaac.orbit.robots.mobile_manipulator + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.utils.assets.rst b/docs/source/api/orbit.utils.assets.rst new file mode 100644 index 0000000000..79a13d7b0f --- /dev/null +++ b/docs/source/api/orbit.utils.assets.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.utils.assets +============================= + +.. automodule:: omni.isaac.orbit.utils.assets + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.utils.kit.rst b/docs/source/api/orbit.utils.kit.rst new file mode 100644 index 0000000000..9d6fd3a366 --- /dev/null +++ b/docs/source/api/orbit.utils.kit.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.utils.kit +========================== + +.. automodule:: omni.isaac.orbit.utils.kit + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.utils.math.rst b/docs/source/api/orbit.utils.math.rst new file mode 100644 index 0000000000..f28244356f --- /dev/null +++ b/docs/source/api/orbit.utils.math.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.utils.math +=========================== + +.. automodule:: omni.isaac.orbit.utils.math + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.utils.mdp.rst b/docs/source/api/orbit.utils.mdp.rst new file mode 100644 index 0000000000..c56959644a --- /dev/null +++ b/docs/source/api/orbit.utils.mdp.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit.utils.mdp +========================== + +.. automodule:: omni.isaac.orbit.utils.mdp + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit.utils.rst b/docs/source/api/orbit.utils.rst new file mode 100644 index 0000000000..0d2c20ac34 --- /dev/null +++ b/docs/source/api/orbit.utils.rst @@ -0,0 +1,44 @@ +omni.isaac.orbit.utils +====================== + +Sub-module containing utilities for the Orbit framework. + +Configuration operations +~~~~~~~~~~~~~~~~~~~~~~~~ + +.. automodule:: omni.isaac.orbit.utils.configclass + :members: + :undoc-members: + :show-inheritance: + +Loading and saving data +~~~~~~~~~~~~~~~~~~~~~~~ + +.. automodule:: omni.isaac.orbit.utils.io + :members: + :undoc-members: + :show-inheritance: + +Dictionary operations +~~~~~~~~~~~~~~~~~~~~~ + +.. automodule:: omni.isaac.orbit.utils.dict + :members: + :undoc-members: + :show-inheritance: + +String operations +~~~~~~~~~~~~~~~~~ + +.. automodule:: omni.isaac.orbit.utils.string + :members: + :undoc-members: + :show-inheritance: + +Timer operations +~~~~~~~~~~~~~~~~ + +.. automodule:: omni.isaac.orbit.utils.timer + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit_envs.isaac_env.rst b/docs/source/api/orbit_envs.isaac_env.rst new file mode 100644 index 0000000000..05cc7754d8 --- /dev/null +++ b/docs/source/api/orbit_envs.isaac_env.rst @@ -0,0 +1,62 @@ +omni.isaac.orbit_envs.isaac_env +=============================== + +We use OpenAI Gym registry to register the environment and their default configuration file. +The default configuration file is passed to the argument "kwargs" in the Gym specification registry. +The string is parsed into respective configuration container which needs to be passed to the environment +class. This is done using the function :meth:`load_default_env_cfg` in the sub-module +:mod:`omni.isaac.orbit.utils.parse_cfg`. + + +.. note:: + + There is a slight abuse of kwargs since they are meant to be directly passed into the environment class. + Instead, we remove the key :obj:`cfg_file` from the "kwargs" dictionary and the user needs to provide + the kwarg argument :obj:`cfg` while creating the environment. + + +.. code-block:: python + + import gym + import omni.isaac.orbit_envs + from omni.isaac.orbit_envs.utils.parse_cfg import load_default_env_cfg + + task_name = "Isaac-Cartpole-v0" + cfg = load_default_env_cfg(task_name) + env = gym.make(task_name, cfg=cfg) + + +All environments must inherit from :class:`IsaacEnv` class which is defined in the sub-module +:mod:`omni.isaac.orbit_envs.isaac_env`. +The main methods that needs to be implemented by an inherited environment class: + +* :meth:`_design_scene`: Design the template environment for cloning. +* :meth:`_reset_idx`: Environment reset function based on environment indices. +* :meth:`_step_impl`: Apply actions into simulation and compute MDP signals. +* :meth:`_get_observations`: Get observations from the environment. + +The following attributes need to be set by the inherited class: + +* :attr:`action_space`: The Space object corresponding to valid actions +* :attr:`observation_space`: The Space object corresponding to valid observations +* :attr:`reward_range`: A tuple corresponding to the min and max possible rewards. A default reward range set to [-inf, +inf] already exists. + +The action and observation space correspond to single environment (and not vectorized). + + +Base Environment +---------------- + +.. automodule:: omni.isaac.orbit_envs.isaac_env + :members: + :undoc-members: + :show-inheritance: + :private-members: + +Base Configuration +--------------------- + +.. automodule:: omni.isaac.orbit_envs.isaac_env_cfg + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit_envs.utils.data_collector.rst b/docs/source/api/orbit_envs.utils.data_collector.rst new file mode 100644 index 0000000000..d128ad6d71 --- /dev/null +++ b/docs/source/api/orbit_envs.utils.data_collector.rst @@ -0,0 +1,79 @@ +omni.isaac.orbit_envs.utils.data_collector +========================================== + +All post-processed robomimic compatible datasets share the same data structure. A single dataset is a +single HDF5 file. The stored data follows the structure provided +`here `_. + +The collector takes input data in its batched format and stores them as different demonstrations, each corresponding +to a given environment index. The demonstrations are flushed to disk when the :meth:`flush()` is called for the +respective environments. All the data is saved when the :meth:`close()` is called. + +The following sample shows how to use the ``robomimic`` data collector: + +.. code-block:: python + + import os + import torch + + from omni.isaac.orbit_envs.utils.data_collector import RobomimicDataCollector + + # name of the environment (needed by robomimic) + task_name = "Isaac-Franka-Lift-v0" + # specify directory for logging experiments + test_dir = os.path.dirname(os.path.abspath(__file__)) + log_dir = os.path.join(test_dir, "logs", "demos") + # name of the file to save data + filename = "hdf_dataset.hdf5" + # number of episodes to collect + num_demos = 10 + # number of environments to simulate + num_envs = 4 + + # create data-collector + collector_interface = RobomimicDataCollector(task_name, log_dir, filename, num_demos) + + # reset the collector + collector_interface.reset() + + while not collector_interface.is_stopped(): + # generate random data to store + # -- obs + obs = { + "joint_pos": torch.randn(num_envs, 10), + "joint_vel": torch.randn(num_envs, 10) + } + # -- actions + actions = torch.randn(num_envs, 10) + # -- rewards + rewards = torch.randn(num_envs) + # -- dones + dones = torch.rand(num_envs) > 0.5 + + # store signals + # -- obs + for key, value in obs.items(): + collector_interface.add(f"obs/{key}", value) + # -- actions + collector_interface.add("actions", actions) + # -- next_obs + for key, value in obs.items(): + collector_interface.add(f"next_obs/{key}", value.cpu().numpy()) + # -- rewards + collector_interface.add("rewards", rewards) + # -- dones + collector_interface.add("dones", dones) + + # flush data from collector for successful environments + # note: in this case we flush all the time + reset_env_ids = dones.nonzero(as_tuple=False).squeeze(-1) + collector_interface.flush(reset_env_ids) + + # close collector + collector_interface.close() + + +.. automodule:: omni.isaac.orbit_envs.utils.data_collector + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit_envs.utils.rst b/docs/source/api/orbit_envs.utils.rst new file mode 100644 index 0000000000..ab7b955376 --- /dev/null +++ b/docs/source/api/orbit_envs.utils.rst @@ -0,0 +1,7 @@ +omni.isaac.orbit_envs.utils +=============================== + +.. automodule:: omni.isaac.orbit_envs.utils + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/api/orbit_envs.utils.wrappers.rst b/docs/source/api/orbit_envs.utils.wrappers.rst new file mode 100644 index 0000000000..5adf812f8a --- /dev/null +++ b/docs/source/api/orbit_envs.utils.wrappers.rst @@ -0,0 +1,46 @@ +omni.isaac.orbit_envs.utils.wrappers +==================================== + +Wrappers allow you to modify the behavior of an environment without modifying the environment itself. +This is useful for modifying the observation space, action space, or reward function. Additionally, +they can be used to cast a given environment into the respective environment class definition used by +different learning frameworks. This operation may include handling of asymmetric actor-critic observations, +casting the data between different backends such `numpy` and `pytorch`, or organizing the returned data +into the expected data structure by the learning framework. + +All wrappers derive from the ``gym.Wrapper``` class. Using a wrapper is as simple as passing the initialized +environment instance to the wrapper constructor. For instance, to wrap an environment in the +`Stable-Baselines3`_ wrapper, you can do the following: + +.. code-block:: python + + from omni.isaac.orbit_envs.utils.wrappers.sb3 import Sb3VecEnvWrapper + + env = Sb3VecEnvWrapper(env) + + +.. _RL-Games: https://github.com/Denys88/rl_games +.. _RSL-RL: https://github.com/leggedrobotics/rsl_rl +.. _Stable-Baselines3: https://github.com/DLR-RM/stable-baselines3 + + +RL-Games Wrapper +---------------- + +.. automodule:: omni.isaac.orbit_envs.utils.wrappers.rl_games + :members: + :show-inheritance: + +RSL-RL Wrapper +-------------- + +.. automodule:: omni.isaac.orbit_envs.utils.wrappers.rsl_rl + :members: + :show-inheritance: + +Stable-Baselines3 Wrapper +------------------------- + +.. automodule:: omni.isaac.orbit_envs.utils.wrappers.sb3 + :members: + :show-inheritance: diff --git a/docs/source/features/actuators.rst b/docs/source/features/actuators.rst new file mode 100644 index 0000000000..2920b32d07 --- /dev/null +++ b/docs/source/features/actuators.rst @@ -0,0 +1,76 @@ +Actuators +========= + +An articulated system comprises of actuated joints, also called the degrees of freedom (DOF). +In a physical system, the actuation typically happens either through active components, such as +electric or hydraulic motors, or passive components, such as springs. These components can introduce +certain non-linear characteristics which includes delays or maximum producible velocity or torque. + +In simulation, the joints are either position, velocity, or torque-controlled. For position and velocity +control, the physics engine internally implements a spring-damp (PD) controller which computes the torques +applied on the actuated joints. In torque-control, the commands are set directly as the joint efforts. +While this mimics an ideal behavior of the joint mechanism, it does not truly model how the drives work +in the physical world. Thus, we provide a mechanism to inject external models to compute the +joint commands that would represent the physical robot's behavior. + +Actuator models +--------------- + +We name two different types of actuator models: + +1. **implicit**: corresponds to the ideal simulation mechanism (provided by physics engine). +2. **explicit**: corresponds to external drive models (implemented by user). + +The explicit actuator model performs two steps: 1) it computes the desired joint torques for tracking +the input commands, and 2) it clips the desired torques based on the motor capabilities. The clipped +torques are the desired actuation efforts that are set into the simulation. + +All explicit models inherit from the base actuator model, :class:`IdealActuator`, which implements a +PD controller with feed-forward effort, and simple clipping based on the configured maximum effort: + +.. math:: + + \tau_{j, computed} & = k_p * (q - q_{des}) + k_d * (\dot{q} - \dot{q}_{des}) + \tau_{ff} \\ + \tau_{j, applied} & = clip(\tau_{computed}, -\tau_{j, max}, \tau_{j, max}) + + +where, :math:`k_p` and :math:`k_d` are joint stiffness and damping gains, :math:`q` and :math:`\dot{q}` +are the current joint positions and velocities, :math:`q_{des}`, :math:`\dot{q}_{des}` and :math:`\tau_{ff}` +are the desired joint positions, velocities and torques commands. The parameters :math:`\gamma` and +:math:`\tau_{motor, max}` are the gear box ratio and the maximum motor effort possible. + +.. note:: + + We provide implementations for various explicit actuator models. These are detailed in + `omni.isaac.orbit.actuators.model <../api/orbit.actuators.model.html>`_ sub-package. + +Actuator groups +--------------- + +The actuator models by themselves are computational blocks that take as inputs the desired joint commands +and output the the joint commands to apply into the simulator. They do not contain any knowledge about the +joints they are acting on themselves. These are handled by the actuator groups. + +Actuator groups collect a set of actuated joints on an articulation that are using the same actuator model. +For instance, the quadruped, ANYmal-C, uses series elastic actuator, ANYdrive 3.0, for all its joints. This +grouping configures the actuator model for those joints, translates the input commands to the joint level +commands, and returns the articulation action to set into the simulator. Through this mechanism, it is also +possible to configure the included joints under some constraints, such as mimicking of gripper commands or +introducing non-holonomic constraints for a wheel base. + +An articulated system can be composed of different actuator groups. For instance, a legged mobile manipulator +can be composed of a base group, an arm group, and a gripper group. If the base is the quadruped, ANYmal-C, +the base group can utilize the actuator network to model the series elastic actuation. Similarly, the arm, +a Kinova Jaco2, can use a DC motor model and take joint positions as input commands. Finally, the gripper group +can employ the gripper mimic group which processes binary open/close command into individual gripper joint actions. + +.. image:: ../_static/actuator_groups.svg + :width: 600 + :align: center + :alt: Actuator groups for a legged mobile manipulator + + +.. note:: + + For more information on the actuator groups, please refer to the documentation of the + `omni.isaac.orbit.actuators.group <../api/orbit.actuators.group.html>`_ subpackage. diff --git a/docs/source/features/motion_generators.rst b/docs/source/features/motion_generators.rst new file mode 100644 index 0000000000..4cb97a0b59 --- /dev/null +++ b/docs/source/features/motion_generators.rst @@ -0,0 +1,229 @@ +Motion Generators +================= + +Robotic tasks are typically defined in task-space in terms of desired +end-effector trajectory, while control actions are executed in the +joint-space. This naturally leads to *joint-space* and *task-space* +(operational-space) control methods. However, successful execution of +interaction tasks using motion control often requires an accurate model +of both the robot manipulator as well as its environment. While a +sufficiently precise manipulator's model might be known, detailed +description of environment is hard to obtain :cite:p:`siciliano2009force`. +Planning errors caused by this mismatch can be overcome by introducing a +*compliant* behavior during interaction. + +While compliance is achievable passively through robot's structure (such +as elastic actuators, soft robot arms), we are more interested in +controller designs that focus on active interaction control. These are +broadly categorized into: + +1. **impedance control:** indirect control method where motion deviations + caused during interaction relates to contact force as a mass-spring-damper + system with adjustable parameters (stiffness and damping). A specialized case + of this is *stiffness* control where only the static relationship between + position error and contact force is considered. + +2. **hybrid force/motion control:** active control method which controls motion + and force along unconstrained and constrained task directions respectively. + Among the various schemes for hybrid motion control, the provided implementation + is based on inverse dynamics control in the operational space :cite:p:`khatib1987osc`. + +.. note:: + + To provide an even broader set of motion generators, we welcome contributions from the + community. If you are interested, please open an issue to start a discussion! + + +Joint-space controllers +----------------------- + +Torque control +~~~~~~~~~~~~~~ + +Action dimensions: ``"n"`` (number of joints) + +In torque control mode, the input actions are directly set as feed-forward +joint torque commands, i.e. at every time-step, + +.. math:: + + \tau = \tau_{des} + +Thus, this control mode is achievable by setting the command type for the actuator group, via +the :class:`ActuatorControlCfg` class, to `"t_abs"`. + + +Velocity control +~~~~~~~~~~~~~~~~ + +Action dimensions: ``"n"`` (number of joints) + +In velocity control mode, a proportional control law is required to reduce the error between the +current and desired joint velocities. Based on input actions, the joint torques commands are computed as: + +.. math:: + + \tau = k_d (\dot{q}_{des} - \dot{q}) + +where :math:`k_d` are the gains parsed from configuration. + +This control mode is achievable by setting the command type for the actuator group, via +the :class:`ActuatorControlCfg` class, to `"v_abs"` or `"v_rel"`. + +.. note:: + + While performing velocity control, in many cases, gravity compensation is required to ensure better + tracking of the command. In this case, we suggest disabling gravity for the links in the articulation + in simulation. + +Position control with fixed impedance +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Action dimensions: ``"n"`` (number of joints) + +In position control mode, a proportional-damping (PD) control law is employed to track the desired joint +positions and ensuring the articulation remains still at the desired location (i.e., desired joint velocities +are zero). Based on the input actions, the joint torque commands are computed as: + +.. math:: + + \tau = k_p (q_{des} - q) - k_d \dot{q} + +where :math:`k_p` and :math:`k_d` are the gains parsed from configuration. + +In its simplest above form, the control mode is achievable by setting the command type for the actuator group, +via the :class:`ActuatorControlCfg` class, to `"p_abs"` or `"p_rel"`. + +However, a more complete formulation which considers the dynamics of the articulation would be: + +.. math:: + + \tau = M \left( k_p (q_{des} - q) - k_d \dot{q} \right) + g + +where :math:`M` is the joint-space inertia matrix of size :math:`n \times n`, and :math:`g` is the joint-space +gravity vector. This implementation is available through the :class:`JointImpedanceController` class by setting the +impedance mode to ``"fixed"``. The gains :math:`k_p` are parsed from the input configuration and :math:`k_d` +are computed while considering the system as a decoupled point-mass oscillator, i.e., + +.. math:: + + k_d = 2 \sqrt{k_p} \times D + +where :math:`D` is the damping ratio of the system. Critical damping is achieved for :math:`D = 1`, overcritical +damping for :math:`D > 1` and undercritical damping for :math:`D < 1`. + +Additionally, it is possible to disable the inertial or gravity compensation in the controller by setting the +flags :attr:`inertial_compensation` and :attr:`gravity_compensation` in the configuration to :obj:`False`, +respectively. + +Position control with variable stiffness +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Action dimensions: ``"2n"`` (number of joints) + +In stiffness control, the same formulation as above is employed, however, the gains :math:`k_p` are part of +the input commands. This implementation is available through the :class:`JointImpedanceController` class by +setting the impedance mode to ``"variable_kp"``. + +Position control with variable impedance +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Action dimensions: ``"3n"`` (number of joints) + +In impedance control, the same formulation as above is employed, however, both :math:`k_p` and :math:`k_d` +are part of the input commands. This implementation is available through the :class:`JointImpedanceController` +class by setting the impedance mode to ``"variable"``. + +Task-space controllers +---------------------- + +Differential inverse kinematics (IK) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Action dimensions: ``"3"`` (relative/absolute position), ``"6"`` (relative pose), or ``"7"`` (absolute pose) + +Inverse kinematics converts the task-space tracking error to joint-space error. In its most typical implementation, +the pose error in the task-sace, :math:`\Delta \chi_e = (\Delta p_e, \Delta \phi_e)`, is computed as the cartesian +distance between the desired and current task-space positions, and the shortest distance in :math:`\mathbb{SO}(3)` +between the desired and current task-space orientations. + +Using the geometric Jacobian :math:`J_{eO} \in \mathbb{R}^{6 \times n}`, that relates task-space velocity to joint-space velocities, +we design the control law to obtain the desired joint positions as: + +.. math:: + + q_{des} = q + \eta J_{eO}^{-} \Delta \chi_e + +where :math:`\eta` is a scaling parameter and :math:`J_{eO}^{-}` is the pseudo-inverse of the Jacobian. + +It is possible to compute the pseudo-inverse of the Jacobian using different formulations: + +* Moore-Penrose pseduo-inverse: :math:`A^{-} = A^T(AA^T)^{-1}`. +* Levenberg-Marquardt pseduo-inverse (damped least-squares): :math:`A^{-} = A^T (AA^T + \lambda \mathbb{I})^{-1}`. +* Tanspose pseudo-inverse: :math:`A^{-} = A^T`. +* Adaptive singular-vale decomposition (SVD) pseduo-inverse from :cite:t:`buss2004ik`. + +These implementations are available through the :class:`DifferentialInverseKinematics` class. + +Impedance controller +~~~~~~~~~~~~~~~~~~~~ + + +It uses task-space pose error and Jacobian to compute join torques through mass-spring-damper system +with a) fixed stiffness, b) variable stiffness (stiffness control), +and c) variable stiffness and damping (impedance control). + +Operational-space controller +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Similar to task-space impedance +control but uses the Equation of Motion (EoM) for computing the +task-space force + +Closed-loop proportional force controller +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +It uses a proportional term +to track the desired wrench command with respect to current wrench at +the end-effector. + +Hybrid force-motion controller +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +It combines closed-loop force control +and operational-space motion control to compute the desired wrench at +the end-effector. It uses selection matrices that define the +unconstrainted and constrained task directions. + + +Reactive planners +----------------- + +Typical task-space controllers do not account for motion constraints +such as joint limits, self-collision and environment collision. Instead +they rely on high-level planners (such as RRT) to handle these +non-Euclidean constraints and give joint/task-space way-points to the +controller. However, these methods are often conservative and have +undesirable deceleration when close to an object. More recently, +different approaches combine the constraints directly into an +optimization problem, thereby providing a holistic solution for motion +generation and control. + +We currently support the following planners: + +- **RMPFlow (lula):** An acceleration-based policy that composes various Reimannian Motion Policies (RMPs) to + solve a hierarchy of tasks :cite:p:`cheng2021rmpflow`. It is capable of performing dynamic collision + avoidance while navigating the end-effector to a target. + +- **MPC (OCS2):** A receding horizon control policy based on sequential linear-quadratic (SLQ) programming. + It formulates various constraints into a single optimization problem via soft-penalties and uses automatic + differentiation to compute derivatives of the system dynamics, constriants and costs. Currently, we support + the MPC formulation for end-effector trajectory tracking in fixed-arm and mobile manipulators. The formulation + considers a kinematic system model with joint limits and self-collision avoidance :cite:p:`mittal2021articulated`. + + +.. warning:: + + We wrap around the python bindings for these reactive planners to perform a batched computing of + robot actions. However, their current implementations are CPU-based which may cause certain + slowdown for learning. diff --git a/docs/source/refs/bibliography.rst b/docs/source/refs/bibliography.rst new file mode 100644 index 0000000000..0d21440d01 --- /dev/null +++ b/docs/source/refs/bibliography.rst @@ -0,0 +1,4 @@ +Bibliography +============ + +.. bibliography:: diff --git a/docs/source/refs/changelog.rst b/docs/source/refs/changelog.rst new file mode 100644 index 0000000000..08f3292078 --- /dev/null +++ b/docs/source/refs/changelog.rst @@ -0,0 +1,33 @@ +Extensions Changelog +==================== + +All notable changes to this project are documented in this file. The format is based on `Keep a Changelog `__ and this project adheres to `Semantic Versioning `__. + +Each extension has its own changelog. The changelog for each extension is located in the ``docs`` directory of the extension. +The changelog for each extension is also included in this changelog to make it easier to find the changelog for a specific extension. + +omni.isaac.orbit +----------------- + +Extension containing the core framework of Orbit. + +.. include:: ../../../source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst + :start-line: 3 + + +omni.isaac.orbit_envs +--------------------- + +Extension containing the environments built using Orbit. + +.. include:: ../../../source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst + :start-line: 3 + + +omni.isaac.contrib_envs +----------------------- + +Extension for environments contributed by the community. + +.. include:: ../../../source/extensions/omni.isaac.contrib_envs/docs/CHANGELOG.rst + :start-line: 3 diff --git a/docs/source/refs/contributing.rst b/docs/source/refs/contributing.rst new file mode 100644 index 0000000000..fbdbe63609 --- /dev/null +++ b/docs/source/refs/contributing.rst @@ -0,0 +1,219 @@ +Contribution Guidelines +======================= + +We wholeheartedly welcome contributions to the project to make the framework more mature +and useful for everyone. These may happen in forms of: + +* Bug reports: Please report any bugs you find in the `issue tracker `__. +* Feature requests: Please suggest new features you would like to see in the `issue tracker `__. +* Code contributions: Please submit a `pull request `__. + + * Bug fixes + * New features + * Documentation improvements + * Tutorials and tutorial improvements + + +Contributing Code +----------------- + +We use `GitHub `__ for code hosting. Please +follow the following steps to contribute code: + +1. Create an issue in the `issue tracker `__ to discuss + the changes or additions you would like to make. This helps us to avoid duplicate work and to make + sure that the changes are aligned with the roadmap of the project. +2. Fork the repository. +3. Create a new branch for your changes. +4. Make your changes and commit them. +5. Push your changes to your fork. +6. Submit a pull request to the `main branch `__. +7. Ensure all the checks on the pull request template are performed. + +After sending a pull request, the maintainers will review your code and provide feedback. + +Please ensure that your code is well-formatted, documented and passes all the tests. + +.. note:: + + It is important to keep the pull request as small as possible. This makes it easier for the + maintainers to review your code. If you are making multiple changes, please send multiple pull requests. + Large pull requests are difficult to review and may take a long time to merge. + + +Coding Style +------------ + +We follow the `Google Style +Guides `__ for the +codebase. For Python code, the PEP guidelines are followed. Most +important ones are `PEP-8 `__ +for code comments and layout, +`PEP-484 `__ and +`PEP-585 `__ for +type-hinting. + +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 + Ned Batchelder's McCabe script. + +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 + + pre-commit run --all-files + +Maintaining a changelog +----------------------- + +Each extension maintains a changelog in the ``CHANGELOG.rst`` file in the ``docs`` directory. The +file is written in `reStructuredText `__ format. It +contains a curated, chronologically ordered list of notable changes for each version of the extension. + +The goal of this changelog is to help users and contributors see precisely what notable changes have +been made between each release (or version) of the extension. This is a *MUST* for every extension. + +For updating the changelog, please follow the following guidelines: + +* Each version should have a section with the version number and the release date. +* The version number is updated according to `Semantic Versioning `__. The + release date is the date on which the version is released. +* Each version is divided into subsections based on the type of changes made. + + * ``Added``: For new features. + * ``Changed``: For changes in existing functionality. + * ``Deprecated``: For soon-to-be removed features. + * ``Removed``: For now removed features. + * ``Fixed``: For any bug fixes. + +* Each change is described in its corresponding sub-section with a bullet point. +* The bullet points are written in the past tense and in imperative mode. + +For example, the following is a sample changelog: + +.. code:: rst + + Changelog + --------- + + 0.1.0 (2021-02-01) + ~~~~~~~~~~~~~~~~~~ + + Added + ^^^^^ + + * Added a new feature. + + Changed + ^^^^^^^ + + * Changed an existing feature. + + Deprecated + ^^^^^^^^^^ + + * Deprecated an existing feature. + + Removed + ^^^^^^^ + + * Removed an existing feature. + + Fixed + ^^^^^ + + * Fixed a bug. + + 0.0.1 (2021-01-01) + ~~~~~~~~~~~~~~~~~~ + + Added + ^^^^^ + + * Added a new feature. + + +Contributing Documentation +-------------------------- + +Contributing to the documentation is as easy as contributing to the codebase. All the source files +for the documentation are located in the ``orbit/docs`` directory. The documentation is written in +`reStructuredText `__ format. + +We use `Sphinx `__ with the +`Book Theme `__ +for maintaining the documentation. + +Sending a pull request for the documentation is the same as sending a pull request for the codebase. +Please follow the steps mentioned in the `Contributing Code`_ section. + +To build the documentation, we recommend creating a `virtual environment `__ +to install the dependencies. This can also be a `conda environment `__. + +Execute the following commands in the terminal: + +1. Enter the ``orbit/docs`` directory. + + .. code:: bash + + # enter the location of the docs directory (relative to the root of the repository) + cd docs + +2. Install the dependencies (preferably in a virtual/conda environment). + + .. code:: bash + + # install the dependencies + pip install -r requirements.txt + +3. Build the documentation. + + .. code:: bash + + # build the documentation + make html + +4. Open the documentation in a browser. + + .. code:: bash + + # open the documentation in a browser + xdg-open _build/html/index.html + + +Contributing assets +------------------- + +Currently, we host the assets for the extensions on `NVIDIA Nucleus Server `__. +Nucleus is a cloud-based storage service that allows users to store and share large files. It is +integrated with the `NVIDIA Omniverse Platform `__. + +Since all assets are hosted on Nucleus, we do not need to include them in the repository. However, +we need to include the links to the assets in the documentation. + +The included assets are part of the `Isaac Sim Content `__. +To use this content, you need to download the files to a Nucleus server or create an **Isaac** Mount on +a Nucleus server. + +Please check the `Isaac Sim documentation `__ +for more information on how to download the assets. + +.. note:: + We are currently working on a better way to contribute assets. We will update this section once we + have a solution. In the meantime, please follow the steps mentioned below. + +To host your own assets, the current solution is: + +1. Create a separate repository for the assets and add it over there +2. Make sure the assets are licensed for use and distribution +3. Include images of the assets in the README file of the repository +4. Send a pull request with a link to the repository + +We will then verify the assets, its licensing, and include the assets into the Nucleus server for hosting. +In case you have any questions, please feel free to reach out to us through e-mail or by opening an issue +in the repository. diff --git a/docs/source/refs/license.rst b/docs/source/refs/license.rst new file mode 100644 index 0000000000..d86278862a --- /dev/null +++ b/docs/source/refs/license.rst @@ -0,0 +1,45 @@ +License +======= + +NVIDIA Isaac Sim is available freely under `indivudal license +`_. 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 `_. + + +Orbit framework is open-sourced under the +`BSD-3-Clause license `_. + + +.. code-block:: text + + Copyright (c) 2022, ETH Zurich + Copyright (c) 2022, University of Toronto + Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES + 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/source/refs/roadmap.rst b/docs/source/refs/roadmap.rst new file mode 100644 index 0000000000..0f48dbbd0a --- /dev/null +++ b/docs/source/refs/roadmap.rst @@ -0,0 +1,71 @@ +Development Roadmap +=================== + +Following is a loosely defined roadmap for the development of the codebase. The roadmap is subject to +change and is not a commitment to deliver specific features by specific dates or in the specified order. + +Some of the features listed below are already implemented in the codebase, but are not yet documented +and/or tested. We will be working on improving the documentation and testing of these features in the +coming months. + +**January 2023** + +* Experimental functional API +* Supported motion generators + + * Joint-space control + * Differential inverse kinematics control + * Riemannian Motion Policies (RMPs) + +* Supported robots + + * Quardupeds: ANYmal-B, ANYmal-C, Unitree A1 + * Arms: Franka Emika Panda, UR10 + * Mobile manipulators: Franka Emika Panda and UR10 on Clearpath Ridgeback + +* Supported sensors + + * Camera (non-parallelized) + * Height scanner (non-parallelized) + +* Included environments + + * classic: MuJoCo classic environments (ant, humanoid, cartpole) + * locomotion: flat terrain for legged robots + * rigid-object manipulation: end-effector tracking, object lifting + +**February 2023** + +* Example on using the APIs in an Omniverse extension +* Supported motion generators + + * Operational-space control + * Model predictive control (OCS2) + +* Supported sensors + + * Height scanner (parallelized for terrains) + +* Supported robots + + * Quardupeds: Unitree B1, Unitree Go1 + * Arms: Kinova Jaco2, Kinova Gen3, Sawyer, UR10e + * Mobile manipulators: Fetch + +* Included environments + + * locomotion: rough terrain for legged robots + * rigid-object manipulation: in-hand manipulation, hockey puck pushing, peg-in-hole, stacking + * deformable-object manipulation: cloth folding, cloth lifting + +**March or April 2023** + +* Add functional versions of all environments +* Included environments + + * deformable-object manipulation: fluid transfer, fluid pouring, deformable object lifting + +**May 2023** + +* Update code documentation and tutorials +* Release 1.0 diff --git a/docs/source/refs/troubleshooting.rst b/docs/source/refs/troubleshooting.rst new file mode 100644 index 0000000000..4a086c94e0 --- /dev/null +++ b/docs/source/refs/troubleshooting.rst @@ -0,0 +1,136 @@ +Tricks and Troubleshooting +========================== + +Using CPU Scaling Governor for performance +------------------------------------------ + +By default on many systems, the CPU frequency governor is set to +“powersave” mode, which sets the CPU to lowest static frequency. To +increase the maximum performance, we recommend setting the CPU frequency +governor to “performance” mode. For more details, please check the the +link +`here `__. + +.. warning:: + We advice not to set the governor to “performance” mode on a system with poor + cooling (such as laptops), since it may cause the system to overheat. + +- To view existing ``scaling_governor`` value per CPU: + + .. code:: bash + + cat /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor + +- To change the governor to “performance” mode for each CPU: + + .. code:: bash + + echo performance | sudo tee /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor + + +Observing long load times at the start of the simulation +-------------------------------------------------------- + +The first time you run the simulator, it will take a long time to load up. This is because the +simulator is compiling shaders and loading assets. Subsequent runs should be faster to start up, +but may still take some time. + +Please note that once the Isaac Sim app loads, the environment creation time may scale linearly with +the number of environments. Please expect a longer load time if running with thousands of +environments or if each environment contains a larger number of assets. We are continually working +on improving the time needed for this. + +When an instance of Isaac Sim is already running, launching another Isaac Sim instance in a different +process may appear to hang at startup for the first time. Please be patient and give it some time as +the second process will take longer to start up due to slower shader compilation. + + +Receiving a “PhysX error” when running simulation on GPU +-------------------------------------------------------- + +When using the GPU pipeline, the buffers used for the physics simulation are allocated on the GPU only +once at the start of the simulation. This means that they do not grow dynamically as the number of +collisions or objects in the scene changes. If the number of collisions or objects in the scene +exceeds the size of the buffers, the simulation will fail with an error such as the following: + +.. code:: bash + + PhysX error: the application need to increase the PxgDynamicsMemoryConfig::foundLostPairsCapacity parameter to 3072, otherwise the simulation will miss interactions + +In this case, you need to increase the size of the buffers passed to the :class:`SimulationContext` class. +The size of the buffers can be increased by setting the ``found_lost_pairs_capacity`` in the ``sim_params`` +argument to the :class:`SimulationContext` class. For example, to increase the size of the buffers to +``4096``, you can use the following code: + +.. code:: python + + from omni.isaac.core.simulation_context import SimulationContext + + sim = SimulationContext(sim_params={"gpu_found_lost_pairs_capacity": 4096}) + +These settings are also directly exposed through the :class:`PhysxCfg` class in the ``omni.isaac.orbit_envs`` +extension, which can be used to configure the simulation engine. Please see the documentation for +:class:`PhysxCfg` for more details. + + +Understanding the error logs from crashes +----------------------------------------- + +Many times the simulator crashes due to a bug in the implementation. +This swamps the terminal with exceptions, some of which are coming from +the python interpreter calling ``__del__()`` destructor of the +simulation application. These typically look like the following: + +.. code:: bash + + ... + + [INFO]: Completed setting up the environment... + + Traceback (most recent call last): + File "source/standalone/workflows/robomimic/collect_demonstrations.py", line 166, in + main() + File "source/standalone/workflows/robomimic/collect_demonstrations.py", line 126, in main + actions = pre_process_actions(delta_pose, gripper_command) + File "source/standalone/workflows/robomimic/collect_demonstrations.py", line 57, in pre_process_actions + return torch.concat([delta_pose, gripper_vel], dim=1) + TypeError: expected Tensor as element 1 in argument 0, but got int + Exception ignored in: ._Registry.__del__ at 0x7f94ac097f80> + Traceback (most recent call last): + File "../orbit/_isaac_sim/kit/extscore/omni.kit.viewport.registry/omni/kit/viewport/registry/registry.py", line 103, in __del__ + File "../orbit/_isaac_sim/kit/extscore/omni.kit.viewport.registry/omni/kit/viewport/registry/registry.py", line 98, in destroy + TypeError: 'NoneType' object is not callable + Exception ignored in: ._Registry.__del__ at 0x7f94ac097f80> + Traceback (most recent call last): + File "../orbit/_isaac_sim/kit/extscore/omni.kit.viewport.registry/omni/kit/viewport/registry/registry.py", line 103, in __del__ + File "../orbit/_isaac_sim/kit/extscore/omni.kit.viewport.registry/omni/kit/viewport/registry/registry.py", line 98, in destroy + TypeError: 'NoneType' object is not callable + Exception ignored in: + Traceback (most recent call last): + File "../orbit/_isaac_sim/kit/kernel/py/omni/kit/app/_impl/__init__.py", line 114, in __del__ + AttributeError: 'NoneType' object has no attribute 'get_settings' + Exception ignored in: + Traceback (most recent call last): + File "../orbit/_isaac_sim/extscache/omni.kit.viewport.menubar.lighting-104.0.7/omni/kit/viewport/menubar/lighting/actions.py", line 345, in __del__ + File "../orbit/_isaac_sim/extscache/omni.kit.viewport.menubar.lighting-104.0.7/omni/kit/viewport/menubar/lighting/actions.py", line 350, in destroy + TypeError: 'NoneType' object is not callable + 2022-12-02 15:41:54 [18,514ms] [Warning] [carb.audio.context] 1 contexts were leaked + ../orbit/_isaac_sim/python.sh: line 41: 414372 Segmentation fault (core dumped) $python_exe "$@" $args + There was an error running python + +This is a known error with running standalone scripts with the Isaac Sim +simulator. Please scroll above the exceptions thrown with +``registry`` to see the actual error log. + +In the above case, the actual error is: + +.. code:: bash + + Traceback (most recent call last): + File "source/standalone/workflows/robomimic/tools/collect_demonstrations.py", line 166, in + main() + File "source/standalone/workflows/robomimic/tools/collect_demonstrations.py", line 126, in main + actions = pre_process_actions(delta_pose, gripper_command) + File "source/standalone/workflows/robomimic/tools/collect_demonstrations.py", line 57, in pre_process_actions + return torch.concat([delta_pose, gripper_vel], dim=1) + TypeError: expected Tensor as element 1 in argument 0, but got int diff --git a/docs/source/setup/developer.rst b/docs/source/setup/developer.rst new file mode 100644 index 0000000000..948933ffb5 --- /dev/null +++ b/docs/source/setup/developer.rst @@ -0,0 +1,251 @@ +Developer's Guide +================= + +For development, we suggest using `Microsoft Visual Studio Code +(VSCode) `__. This is also suggested by +NVIDIA Omniverse and there exists tutorials on how to `debug Omniverse +extensions `__ +using VSCode. + + +Setting up Visual Studio Code +----------------------------- + +The ``orbit`` repository includes the VSCode settings to easily allow setting +up your development environment. These are included in the ``.vscode`` directory +and include the following files: + +.. code-block:: bash + + .vscode + ├── extensions.json + ├── launch.json + ├── settings.json + └── tasks.json + + +To setup the IDE, please follow these instructions: + +1. Open the ``orbit`` directory on Visual Studio Code IDE +2. Run VSCode + `Tasks `__, by + pressing **``Ctrl+Shift+P``**, selecting ``Tasks: Run Task`` and + running the ``setup_python_env`` in the drop down menu. + + .. image:: ../_static/vscode_tasks.png + :width: 600px + :align: center + :alt: VSCode Tasks + +If everything executes correctly, it should create a file +``.python.env`` in the ``.vscode`` directory. The file contains the python +paths to all the extensions provided by Isaac Sim and Omniverse. This helps +in indexing all the python modules for intelligent suggestions while writing +code. + +For more information on VSCode support for Omniverse, please refer to the +following links: + +* `Isaac Sim VSCode support `__ +* `Debugging with VSCode `__ + + +.. note:: + + In the provided configuration, we set the default python interpreter to use the + python executable provided by Omniverse. This is specified in the + ``.vscode/settings.json`` file: + + .. code-block:: json + + { + "python.defaultInterpreterPath": "${workspaceFolder}/_isaac_sim/kit/python/bin/python3", + "python.envFile": "${workspaceFolder}/.vscode/.python.env", + } + + +Repository organization +----------------------- + +The ``orbit`` repository is structured as follows: + +.. code-block:: bash + + orbit + ├── .vscode + ├── LICENSE + ├── orbit.sh + ├── pyproject.toml + ├── CHANGELOG.md + ├── README.md + ├── docs + ├── source + │   ├── extensions + │   │   ├── omni.isaac.orbit + │   │   └── omni.isaac.orbit_envs + │   ├── standalone + │   │   ├── demo + │   │   ├── environments + │   │   └── workflows + │   └── tools + └── VERSION + +The ``source`` directory contains the source code for ``orbit`` *extensions* +and *standalone applications*. The two are the different development workflows +supported in `NVIDIA Isaac Sim `__. + +.. note:: + + Instead of maintaining a `changelog `__ for each + extension, we maintain a common changelog file for the whole repository. This is + located in the root directory of the repository and is named ``CHANGELOG.md``. + + +Extensions +~~~~~~~~~~ + +Extensions are the recommended way to develop applications in Isaac Sim. They are +modularized packages that formulate 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 +an ``extension.toml`` file. More information on extensions can be found in the +`Omniverse documentation `__. + +Orbit in itself provides extensions for robot learning. These are written into the +``source/extensions`` directory. Each extension is written as a python package and +follows the following structure: + +.. code:: bash + + + ├── config + │   └── extension.toml + ├── docs + │   └── README.md + ├── + │ ├── __init__.py + │ ├── .... + │ └── scripts + ├── setup.py + └── tests + +The ``config/extension.toml`` file contains the metadata of the extension. This +includes the name, version, description, dependencies, etc. This information is used +by Omniverse to load the extension. The ``docs`` directory contains the documentation +for the extension with more detailed information about the extension. + +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 +`Extension Manager `__. + +More specificially, when an extension is enabled, the python module specified in the +``config/extension.toml`` file is loaded and scripts that contains children of the +:class:`omni.ext.IExt` class are executed. + +.. code:: python + + import omni.ext + + class MyExt(omni.ext.IExt): + """My extension application.""" + + def on_startup(self, ext_id): + """Called when the extension is loaded.""" + pass + + def on_shutdown(self): + """Called when the extension is unloaded. + + It releases all references to the extension and cleans up any resources. + """ + pass + +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 `__ +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. + +.. note:: + + The ``setup.py`` file is not required for extensions that are only loaded into Omniverse + using the `Extension Manager `__. + +Lastly, the ``tests`` directory contains the unit tests for the extension. These are written +using the `unittest `__ framework. It is +important to note that Omniverse also provides a similar +`testing framework `__. +However, it requires going through the build process and does not support testing of the python module in +standalone applications. + +Standalone applications +~~~~~~~~~~~~~~~~~~~~~~~ + +In a typical Omniverse workflow, the simulator is launched first, after which the extensions are +enabled that load the python module and run the python application. While this is a recommended +workflow, it is not always possible to use this workflow. For example, for robot learning, it is +essential to have complete control over simulation stepping and all the other functionalities +instead of asynchronously waiting for the simulator to step. In such cases, it is necessary to +write a standalone application that launches the simulator using +`SimulationApp `__ +and allows complete control over the simulation through the +`SimulationContext `__ +class. + +.. code:: python + + """Launch Isaac Sim Simulator first.""" + + from omni.isaac.kit import SimulationApp + + # launch omniverse app + config = {"headless": False} + simulation_app = SimulationApp(config) + + + """Rest everything follows.""" + + from omni.isaac.core.simulation_context import SimulationContext + + if __init__ == "__main__": + # get simulation context + simulation_context = SimulationContext() + # rest and play simulation + simulation_context.reset() + # step simulation + simulation_context.step() + # stop simulation + simulation_context.stop() + + +The ``source/standalone`` directory contains various standalone applications designed using the extensions +provided by ``orbit``. These applications are written in python and are structured as follows: + +* **demo**: Contains various demo applications that showcase the core framework ``omni.isaac.orbit``. +* **environments**: Contains applications for running environments defined in ``omni.isaac.orbit_envs`` with different agents. + These include a random policy, zero-action policy, teleoperation or scripted state machines. +* **workflows**: Contains applications for using environments with various learning-based frameworks. These include different + reinforcement learning or imitation learning libraries. + + +Code style +---------- + +The code style used in ``orbit`` is based on the `Google Python Style Guide `__. +For Python code, the PEP guidelines are followed. Most important ones are `PEP-8 `__ +for code comments and layout, `PEP-484 `__ and +`PEP-585 `__ for type-hinting. + +We use `pre-commit `__ checks, that runs +the `black `__ formatter and +`flake8 `__ to check the code. +Please check `here `__ for instructions +to set this up. + +To run over the entire repository, please execute the following command in the terminal: + +.. code:: bash + + pre-commit run --all-files diff --git a/docs/source/setup/installation.rst b/docs/source/setup/installation.rst new file mode 100644 index 0000000000..a15fccd984 --- /dev/null +++ b/docs/source/setup/installation.rst @@ -0,0 +1,188 @@ +Installation Guide +=================== + +.. image:: https://img.shields.io/badge/IsaacSim-2022.2.0-brightgreen.svg + :target: https://developer.nvidia.com/isaac-sim + :alt: IsaacSim 2022.2.0 + +.. image:: https://img.shields.io/badge/python-3.7-blue.svg + :target: https://www.python.org/downloads/release/python-370/ + :alt: Python 3.7 + +.. image:: https://img.shields.io/badge/platform-linux--64-lightgrey.svg + :target: https://releases.ubuntu.com/20.04/ + :alt: Ubuntu 20.04 + + +Installing Isaac Sim +-------------------- + +Downloading pre-built binaries +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Please follow the Isaac Sim +`documentation `__ +to install the latest Isaac Sim release. + +To check the minimum system requirements,refer to the documentation +`here `__. + +.. note:: + We have tested ORBIT with Isaac Sim 2022.2 release on Ubuntu + 20.04LTS with NVIDIA driver 515.76. + +Configuring the environment variables +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Isaac Sim is shipped with its own Python interpreter which bundles in +the extensions released with it. To simplify the setup, we recommend +using the same Python interpreter. Alternately, it is possible to setup +a virtual environment following the instructions +`here `__. + +Please locate the `Python executable in Isaac +Sim `__ +by navigating to Isaac Sim root folder. In the remaining of the +documentation, we will refer to its path as ``ISAACSIM_PYTHON_EXE``. + +.. note:: + On Linux systems, by default, this should be the executable ``python.sh`` in the directory + ``${HOME}/.local/share/ov/pkg/isaac_sim-*``, with ``*`` corresponding to the Isaac Sim version. + +To avoid the overhead of finding and locating the Isaac Sim installation +directory every time, we recommend exporting the following environment +variables to your ``~/.bashrc`` or ``~/.zshrc`` files: + +.. code:: bash + + # Isaac Sim root directory + export ISAACSIM_PATH="${HOME}/.local/share/ov/pkg/isaac_sim-2022.2.0" + # Isaac Sim python executable + export ISAACSIM_PYTHON_EXE="${ISAACSIM_PATH}/python.sh" + +Running the simulator +~~~~~~~~~~~~~~~~~~~~~ + +Once Isaac Sim is installed successfully, make sure that the simulator runs on your +system. For this, we encourage the user to try some of the introductory +tutorials on their `website `__. + +For completeness, we specify the commands here to check that everything is configured correctly. +On a new terminal (**``Ctrl+Alt+T``**), run the following: + +- Check that the simulator is runs: + + .. code:: bash + + # note: you can pass the argument `--help` to see all arguments possible. + ${ISAACSIM_PATH}/isaac-sim.sh + +- Check that the simulator runs from a standalone python script: + + .. 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/omni.isaac.core/add_cubes.py + +.. note:: + + 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: + + .. code:: bash + + ${ISAACSIM_PATH}/isaac-sim.sh --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 `__. + +Installing Orbit +---------------- + +Organizing the workspace +~~~~~~~~~~~~~~~~~~~~~~~~ + +- Clone the ``orbit`` repository into your workspace: + + .. code:: bash + + # For public users + git clone git@github.com:NVIDIA-Omniverse/orbit.git + +- Set up a symbolic link between the installed Isaac Sim root folder + and ``_isaac_sim`` in the ``orbit``` directory. This makes it convenient + to index the python modules and look for extensions shipped with + Isaac Sim. + + .. code:: bash + + # enter the cloned repository + cd orbit + # create a symbolic link + ln -s ${ISAACSIM_PATH} _isaac_sim + +Building extensions +~~~~~~~~~~~~~~~~~~~ + +We provide a helper executable ```orbit.sh`` `__ that provides +utilities to manage extensions: + +.. code:: bash + + ./orbit.sh --help + + usage: orbit.sh [-h] [-i] [-e] [-p] [-s] -- Utility to manage extensions in Isaac Orbit. + + optional arguments: + -h, --help Display the help content. + -i, --install Install the extensions inside Isaac Orbit. + -e, --extra Install extra dependencies such as the learning frameworks. + -p, --python Run the python executable (python.sh) provided by Isaac Sim. + -s, --sim Run the simulator executable (isaac-sim.sh) provided by Isaac Sim. + +The executable automatically fetches the python bundled with Isaac +Sim, using ``./orbit.sh -p`` command. To not restrict running commands +only from the top of this repository (where the README.md is located), +we recommend adding the executable to your environment variables in +your ``.bashrc`` or ``.zshrc`` file as an alias command. This can be +achieved running the following on your terminal: + + +.. code:: bash + + # note: execute the command from where the `orbit.sh` executable exists + # for bash users + echo -e "alias orbit=$(pwd)/orbit.sh" >> ${HOME}/.bashrc + # for zshell users + echo -e "alias orbit=$(pwd)/orbit.sh" >> ${HOME}/.zshrc + + +To build all the extensions, run the following commands: + +- 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/extensions`` directory and installs them using pip + (with ``--editable`` flag): + + .. code:: bash + + ./orbit.sh --install + +- For installing all other dependencies (such as learning + frameworks), execute: + + .. code:: bash + + ./orbit.sh --extra diff --git a/docs/source/setup/sample.rst b/docs/source/setup/sample.rst new file mode 100644 index 0000000000..409de40198 --- /dev/null +++ b/docs/source/setup/sample.rst @@ -0,0 +1,195 @@ +Running existing scripts +======================== + +API Demos +--------- + +The main core interface extension in Orbit ``omni.isaac.orbit`` provides +the main modules for actuators, objects, robots and sensors. We provide +a list of demo scripts. These showcase how to use the provided interfaces +within a code in a minimal way. + +A few quick demo scripts to run and checkout: + +- Spawn different quadrupeds, visualize feet markers, and make + robots stand using position commands: + + .. code:: bash + + ./orbit.sh -p source/standalone/demo/play_quadrupeds.py + +- Spawn multiple Franka arms and apply random position commands: + + .. code:: bash + + ./orbit.sh -p source/standalone/demo/play_arms.py --robot franka_panda + +- Spawn multiple robots and control them using inverse kinematics + controller: + + .. code:: bash + + ./orbit.sh -p source/standalone/demo/play_ik_control.py --robot franka_panda --num_envs 128 + +Environments +------------ + +With Orbit, we also provide a suite of benchmark environments included +in the ``omni.isaac.orbit_envs`` extension. We use the OpenAI Gym registry +to register these environments. For each environment, we provide a default +configuration file that defines the scene, observations, rewards and action spaces. + +The list of environments available registered with OpenAI Gym can be found by running: + +.. code:: bash + + ./orbit.sh -p source/standalone/environments/list_envs.py + + +Basic agents +~~~~~~~~~~~~ + +These include basic agents that output zero or random agents. They are +useful to ensure that the environments are configured correctly. + +- Zero-action agent on the Cart-pole example + + .. code:: bash + + ./orbit.sh -p source/standalone/environments/zero_agent.py --task Isaac-Cartpole-v0 --num_envs 32 + +- Random-action agent on the Cart-pole example: + + .. code:: bash + + ./orbit.sh -p source/standalone/environments/random_agent.py --task Isaac-Cartpole-v0 --num_envs 32 + +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 + + ./orbit.sh -p source/standalone/environments/teleoperation/teleop_se3_agent.py --task Isaac-Lift-Franka-v0 --num_envs 1 --cpu --device keyboard + +The script prints the teleoperation events configured. For keyboard, +these are as follows: + +.. code:: text + + Keyboard Controller for SE(3): Se3Keyboard + Reset all commands: L + 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 + +Imitation Learning +~~~~~~~~~~~~~~~~~~ + +Using the teleoperation devices, it is also possible to collect data for +learning from demonstrations (LfD). For this, we support the learning +framework `Robomimic `__ and allow saving +data in +`HDF5 `__ +format. + +1. Collect demonstrations with teleoperation for the environment + ``Isaac-Lift-Franka-v0``: + + .. code:: bash + + # step a: collect data with keyboard + ./orbit.sh -p source/standalone/workflows/robomimic/collect_demonstrations.py --task Isaac-Lift-Franka-v0 --num_envs 1 --num_demos 10 --device keyboard + # step b: inspect the collected dataset + ./orbit.sh -p source/standalone/workflows/robomimic/tools/inspect_demonstrations.py logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5 + +2. Split the dataset into train and validation set: + + .. code:: bash + + # install python module (for robomimic) + ./orbit.sh -p -m pip install -e 'source/extensions/omni.isaac.orbit_envs[robomimic]' + # split data + ./orbit.sh -p source/standalone//workflows/robomimic/tools/split_train_val.py logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5 --ratio 0.2 + +3. Train a BC agent for ``Isaac-Lift-Franka-v0`` with + `Robomimic `__: + + .. code:: bash + + ./orbit.sh -p source/standalone/workflows/robomimic/train.py --task Isaac-Lift-Franka-v0 --algo bc --dataset logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5 + +4. Play the learned model to visualize results: + + .. code:: bash + + ./orbit.sh -p source/standalone//workflows/robomimic/play.py --task Isaac-Lift-Franka-v0 --checkpoint /PATH/TO/model.pth + +Reinforcement Learning +~~~~~~~~~~~~~~~~~~~~~~ + +We provide wrappers to different reinforcement libraries. These wrappers convert the data +from the environments into the respective libraries function argument and return types. + +- Training an agent with + `Stable-Baselines3 `__ + on ``Isaac-Cartpole-v0``: + + .. code:: bash + + # install python module (for stable-baselines3) + ./orbit.sh -p -m pip install -e 'source/extensions/omni.isaac.orbit_envs[sb3]' + # run script for training + # note: we enable cpu flag since SB3 doesn't optimize for GPU anyway + ./orbit.sh -p source/standalone/workflows/sb3/train.py --task Isaac-Cartpole-v0 --headless --cpu + # run script for playing with 32 environments + ./orbit.sh -p source/standalone/workflows/sb3/play.py --task Isaac-Cartpole-v0 --num_envs 32 --checkpoint /PATH/TO/model.zip + +- Training an agent with + `RL-Games `__ on ``Isaac-Ant-v0``: + + .. code:: bash + + # install python module (for rl-games) + ./orbit.sh -p -m pip install -e 'source/extensions/omni.isaac.orbit_envs[rl_games]' + # run script for training + ./orbit.sh -p source/standalone/workflows/rl_games/train.py --task Isaac-Ant-v0 --headless + # run script for playing with 32 environments + ./orbit.sh -p source/standalone/workflows/rl_games/play.py --task Isaac-Ant-v0 --num_envs 32 --checkpoint /PATH/TO/model.pth + +- Training an agent with + `RSL-RL `__ on ``Isaac-Reach-Franka-v0``: + + .. code:: bash + + # install python module (for rsl-rl) + ./orbit.sh -p -m pip install -e 'source/extensions/omni.isaac.orbit_envs[rsl_rl]' + # run script for training + ./orbit.sh -p source/standalone/workflows/rsl_rl/train.py --task Isaac-Reach-Franka-v0 --headless + # run script for playing with 32 environments + ./orbit.sh -p source/standalone/workflows/rsl_rl/play.py --task Isaac-Reach-Franka-v0 --num_envs 32 --checkpoint /PATH/TO/model.pth + + +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 ```` +is the name of the learning framework, ```` is the task name, and ```` is the timestamp at +which the training script was executed. + +To view the logs, run: + +.. code:: bash + + # execute from the root directory of the repository + ./orbit.sh -p -m tensorboard.main --logdir=logs + +.. _Tensorboard: https://www.tensorflow.org/tensorboard diff --git a/docs/source/tutorials/00_empty.rst b/docs/source/tutorials/00_empty.rst new file mode 100644 index 0000000000..dc28166d77 --- /dev/null +++ b/docs/source/tutorials/00_empty.rst @@ -0,0 +1,162 @@ +Creating an empty scene +======================= + +This tutorial introduces how to create a standalone python script to set up a simple empty scene in Orbit. +It introduces the two main classes used in the simulator, :class:`SimulationApp` and :class:`SimulationContext`, +that help launch and control the simulation timeline respectively. + +Additionally, it introduces the :meth:`create_prim()` function that allows users to create objects in the scene. +We will use this function to create two lights in the scene. + +Please review `Isaac Sim Interface `_ +and `Isaac Sim Workflows `_ +prior to beginning this tutorial. + + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``play_empty.py`` script in the ``orbit/source/standalone/demo`` directory. + + +.. literalinclude:: ../../../source/standalone/demo/play_empty.py + :language: python + :emphasize-lines: 20-22,42-43,65-66,70-80,86-87 + :linenos: + + +The Code Explained +~~~~~~~~~~~~~~~~~~ + +Launching the simulator +----------------------- + +The first step when working with standalone python scripts is to import the ``SimulationApp`` class. This +class is used to launch the simulation app from the python script. It takes in a dictionary of configuration parameters +that can be used to configure the launched application. For more information on the configuration parameters, please +check the `SimulationApp documentation `_. + +Here, we launch the simulation app with the default configuration parameters but with the ``headless`` flag +read from the parsed command line arguments. + +.. literalinclude:: ../../../source/standalone/demo/play_empty.py + :language: python + :lines: 20-22 + :linenos: + :lineno-start: 20 + +Importing python modules +------------------------ + +It is important that the simulation app is launched at the start of the script since it loads various python modules +that are required for the rest of the script to run. Once that is done, we can import the various python modules that +we will be using in the script. + +.. literalinclude:: ../../../source/standalone/demo/play_empty.py + :language: python + :lines: 28-32 + :linenos: + :lineno-start: 28 + + +Designing the simulation scene +------------------------------ + +The next step is to design the simulation scene. This includes creating the stage, setting up the physics scene, +and adding objects to the stage. + +Isaac Sim core provides the `SimulationContext `_ that handles various timeline related events (pausing, playing, +stepping, or stopping the simulator), configures the stage (such as stage units or up-axis), and creates the +`physicsScene `_ +prim (which provides physics simulation parameters such as gravity direction and magnitude, the simulation +time-step size, and advanced solver algorithm settings). + +.. note:: + + The :class:`SimulationContext` class also takes in the ``backend`` parameter which specifies the tensor library + (such as ``"numpy"`` and ``"torch"``) in which the returned physics tensors are casted in. Currently, ``orbit`` + only supports ``"torch"`` backend. + +For this tutorial, we set the physics time step to 0.01 seconds and the rendering time step to 0.01 seconds. Rendering, +here, refers to updating a frame of the current simulation state, which includes the viewport, cameras, and other +existing UI elements. + +.. literalinclude:: ../../../source/standalone/demo/play_empty.py + :language: python + :lines: 42-45 + :linenos: + :lineno-start: 42 + +Next, we add a ground plane and some lights into the scene. These objects are referred to as primitives or prims in +the USD definition. More concretely, prims are the basic building blocks of a USD scene. They can be considered +similar to an "element" in HTML. For example, a polygonal mesh is a prim, a light is a prim, a material is a prim. +An “xform” prim stores a transform that applies to it's child prims. + +Each prim has a unique name, zero or more *properties*, and zero or more *children*. Prims can be nested to create +a hierarchy of objects that define a simulation *stage*. Using this powerful concept, it is possible to create +complex scenes with a single USD file. For more information on USD, +please refer to the `USD documentation `_. + + +In this tutorial, we create prims, we use the :meth:`create_prim()` function which takes as inputs the USD prim path, +the type of prim, prim's location and the prim's properties. The prim path is a string that specifies the prim's +unique name in the USD stage. The prim type is the type of prim (such as ``"Xform"``, ``"Sphere"``, or ``"SphereLight"``). +The prim's properties are passed as key-value pairs in a dictionary. For example, in this tutorial, the ``"radius"`` +property of a ``"SphereLight"`` prim is set to ``2.5``. + + +.. literalinclude:: ../../../source/standalone/demo/play_empty.py + :language: python + :lines: 50-63 + :linenos: + :lineno-start: 50 + + +Running the simulation loop +--------------------------- + +As mentioned earlier, the :class`SimulationContext` class provides methods to control timeline events such as resetting, +playing, pausing and stepping the simulator. An important thing to note is that the simulator is not running until +the :meth:`sim.reset()` method is called. Once the method is called, it plays the timeline that initializes all the +physics handles and tensors. + +After playing the timeline, we can start the main loop. The main loop is where we step the physics simulation, rendering +and other timeline events. The :meth:`sim.step()` method takes in a ``render`` parameter that specifies whether the +current simulation state should be rendered or not. This parameter is set to ``False`` when the ``headless`` flag is +set to ``True``. + +To ensure a safe execution, we wrap the execution loop with checks to ensure that the simulation app is running and +that the simulator is playing. If the simulator is not playing, we simply step the simulator and continue to the next +iteration of the loop. + +.. literalinclude:: ../../../source/standalone/demo/play_empty.py + :language: python + :lines: 65-80 + :linenos: + :lineno-start: 65 + +Lastly, we call the :meth:`simulation_app.stop()` method to stop the simulation application and close the window. + +.. literalinclude:: ../../../source/standalone/demo/play_empty.py + :language: python + :lines: 86-87 + :linenos: + :lineno-start: 86 + +The Code Execution +~~~~~~~~~~~~~~~~~~ + +Now that we have gone through the code, let's run the script and see the result: + +.. code-block:: bash + + ./orbit.sh -p source/standalone/demo/play_empty.py + + +This should open a stage with a ground plane and lights spawned at the specified locations. +The simulation should be playing and the stage should be rendering. To stop the simulation, +you can either close the window, or press the ``STOP`` button in the UI, or press ``Ctrl+C`` +in the terminal. + +Now that we have a basic understanding of how to run a simulation, let's move on to the next tutorial +where we will learn how to add a robot to the stage. diff --git a/docs/source/tutorials/01_arms.rst b/docs/source/tutorials/01_arms.rst new file mode 100644 index 0000000000..649acecc49 --- /dev/null +++ b/docs/source/tutorials/01_arms.rst @@ -0,0 +1,150 @@ +Adding a robotic arm to the scene +================================= + +In the previous tutorial, we explained the basic working of the standalone script and how to +play the simulator. This tutorial shows how to add a robotic arm into the stage and control the +arm by providing random joint commands. + +The tutorial will cover how to use the robot classes provided in Orbit. This includes spawning, +initializing, resetting, and controlling the robot. + + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``play_arms.py`` script in the ``orbit/source/standalone/demo`` directory. + + +.. literalinclude:: ../../../source/standalone/demo/play_arms.py + :language: python + :emphasize-lines: 45-47,92-95,100-103,130-133,144-147,152-154 + :linenos: + + +The Code Explained +~~~~~~~~~~~~~~~~~~ + +Most of the code is the same as the previous tutorial. The only difference is that we are adding a robot to the scene. +We explain the changes below. + +Designing the simulation scene +------------------------------ + +The single arm manipulators refer to robotic arms with a fixed base. These robots are at the ground height, i.e. `z=0`. +Accordingly, the robots areplaced on a table that define their workspace. In this tutorial, we spawn two robots on two +different tables. The first robot is placed on the left table ``/World/Table_1`` and the second robot is placed on the +right table ``/World/Table_2``. + +The tables are loaded from their respective USD file which is hosted on Omniverse Nucleus server + +.. literalinclude:: ../../../source/standalone/demo/play_arms.py + :language: python + :lines: 80-83 + :linenos: + :lineno-start: 80 + +Next, we create an instance of the single arm robot class. The robot class is initialized with a configuration object +that contains information on the associated USD file, default initial state, actuator models for different joints, and +other meta-information about the robot kinematics. The robot class provides method to spawn the robot in the scene at +a given position and orientation, if provided. + +In this tutorial, we disambiguate the robot configuration to load through the parsed command line argument. In Orbit, +we include pre-configured instances of the configuration class to simplify usage. After creating +the robot instance, we can spawn it at the origin defined by the table's location. + + +.. literalinclude:: ../../../source/standalone/demo/play_arms.py + :language: python + :lines: 84-95 + :linenos: + :lineno-start: 84 + +Initializing the robot +---------------------- + +Prims in the scene with physics schemas enabled on them have an associated physics handle created by the +physics engine. These handles are created only when the simulator starts playing. After that, the allocated +tensor buffers used to store the associated prim's state are accessible. This data is exposed through *physics +views* that provide a convenient interface to access the state of the prims. The physics views can be used to +group or encapsulate multiple prims and obtain their data in a batched manner. + +Using the robot class in Orbit, a user can initialize the physics views to obtain views over the articulation +and essential rigid bodies (such as the end-effector) as shown below. Multiple prims are grouped together by +specifying their paths as regex patterns. + +.. literalinclude:: ../../../source/standalone/demo/play_arms.py + :language: python + :lines: 97-103 + :linenos: + :lineno-start: 97 + +Running the simulation loop +--------------------------- + +The robot class provides a method to obtain the default state of the robot. This state is the initial state of the +robot when it is spawned in the scene. The default state is a tuple of two tensors, one for the joint positions and +the other for the joint velocities. It is used to reset the robot to its initial state at a pre-defined interval of +steps. + +.. note:: + Since the underlying physics engine in Isaac Sim does not separate the kinematics forwarding and dynamics stepping, + the robot's state does not take into affect until after stepping the simulation. + +.. literalinclude:: ../../../source/standalone/demo/play_arms.py + :language: python + :lines: 126-133 + :linenos: + :lineno-start: 126 + + +At the start of an episode, we randmly generate the joint commands for the arm. However, we toggle the gripper command +at a regular interval to simulate a grasp and release action. The robot class provides a method to apply the joint +commands. The type of command is configured in the robot configuration object. + +.. literalinclude:: ../../../source/standalone/demo/play_arms.py + :language: python + :lines: 140-147 + :linenos: + :lineno-start: 140 + +After stepping the simulator, we can obtain the current state of the robot. The robot class provides a method to +update the buffers by reading the data through the physics views. By default, the simulation engine provides all data +in the world frame. Thus, the update method also takes care of transforming quantities to other frames such as the +base frame of the robot. + +.. literalinclude:: ../../../source/standalone/demo/play_arms.py + :language: python + :lines: 151-154 + :linenos: + :lineno-start: 151 + +The Code Execution +~~~~~~~~~~~~~~~~~~ + +Now that we have gone through the code, let's run the script and see the result: + +.. code-block:: bash + + ./orbit.sh -p source/standalone/demo/play_arms.py --robot franka_panda + + +This should open a stage with a ground plane, lights, tables and robots. +The simulation should be playing with the robot arms going to random joint configurations. The +gripper, if present, should be opening or closing at regular intervals. To stop the simulation, +you can either close the window, or press the ``STOP`` button in the UI, or press ``Ctrl+C`` +in the terminal + +In addition to the demo script for playing single arm manipulators, we also provide scripts +for playing other robots such as quadrupeds or mobile manipulators. You can run these as follows: + +.. code-block:: bash + + # Quadruped -- Spawns ANYmal C, ANYmal B, Unitree A1 on one stage + ./orbit.sh -p source/standalone/demo/play_quadrupeds.py + + # Mobile manipulator -- Spawns Franka Panda on Clearpath Ridgeback + ./orbit.sh -p source/standalone/demo/play_ridgeback_franka.py + +In this tutorial, we saw how to spawn a robot multiple times and initialize the physics views to access +the simulation state of the robots. In the next tutorial, we will see how to simplify duplicating a simulation +scene multiple times by using the cloner APIs. diff --git a/docs/source/tutorials/02_cloner.rst b/docs/source/tutorials/02_cloner.rst new file mode 100644 index 0000000000..dbee7f204a --- /dev/null +++ b/docs/source/tutorials/02_cloner.rst @@ -0,0 +1,118 @@ +Duplicating a scene efficiently +=============================== + +In the previous tutorial, we needed to spawn each individual prim manually to create multiple robots in the scene. +This operation can be cumbersome and slow when defining a large scene which needs to duplicated a large number of +times, such for reinforement learning. In this tutorial we will look at duplicating the scene with the cloner APIs +provided by Isaac Sim. + +.. note:: + A more descriptive tutorial on the cloner APIs can be found in the `cloner API tutorial + `_. We introduce its main + concepts in this tutorial for convenience. + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``play_cloner.py`` script in the ``orbit/source/standalone/demo`` directory. + + +.. literalinclude:: ../../../source/standalone/demo/play_cloner.py + :language: python + :emphasize-lines: 37,61-67,69-73,94,105,107-120,126 + :linenos: + +The Code Explained +~~~~~~~~~~~~~~~~~~ + +Configuring the simulation stage +-------------------------------- + +Cloning a scene to a large number of instances can lead to slow accessibility of the physics buffers from Python. This +is because in Omniverse, all read and write happens through the intermediate USD layer. Thus, whenever a physics +step happens. the data is first written into the USD buffers from the physics tensor buffers, and then available +to the users. Thus, to avoid this overhead, we can configure the simulation stage to write the data directly into +the tensors accessed by the users. This is performed by enabling the +`PhysX flatcache `_. + +However, once flatcache is enabled, the rendering is not updated since it still reads from the USD buffers, which are +no longer being updated. Thus, we also need to enable scene graph instancing to render the scene. + +These are done by setting the following flags: + +.. literalinclude:: ../../../source/standalone/demo/play_cloner.py + :language: python + :lines: 61-67 + :linenos: + :lineno-start: 61 + + +Cloning the scene +----------------- + +The basic :class:`Cloner` class clones all the prims under a source prim path and puts them under the specified paths. +It provides a method to generate the paths for the cloned prims based on the number of clones. The generated paths +follow the pattern of the source prim path with a number appended to the end. For example, if the source prim path is +`/World/envs/env_0`, the generated paths will be `/World/envs/env_0`, `/World/envs/env_1`, `/World/envs/env_2`, etc. +These are then passed to the :meth:`clone()` method to clone the scene. + +In this tutorial, we use the :class:`GridCloner` class which clones the scene in a grid pattern. The grid spacing +defines the distance between the cloned scenes. We define the base environment path to be `/World/envs`, which is +the parent prim of all the cloned scenes. The source prim path is then defined to be `/World/envs/env_0`. All the +prims under this prim path are cloned. + +.. literalinclude:: ../../../source/standalone/demo/play_cloner.py + :language: python + :lines: 69-73 + :linenos: + :lineno-start: 69 + +Unlike the previous tutorial, in this tutorial, we only spawn one environment and clone it to multiple instances. +We spawn the table and the robot under the source prim path. + +.. literalinclude:: ../../../source/standalone/demo/play_cloner.py + :language: python + :lines: 92-105 + :linenos: + :lineno-start: 92 + +The :meth:`generate_paths()` method generates the paths for the cloned prims. The generated paths are then passed to +the :meth:`clone()` method to clone the source scene. It returns the positions of the cloned scenes relative to +the simulation world origin. + +.. literalinclude:: ../../../source/standalone/demo/play_cloner.py + :language: python + :lines: 107-120 + :linenos: + :lineno-start: 107 + + +Applying collision filtering +---------------------------- + +Collisions between the cloned environments is filtered by using the :meth:`filter_collisions()` method. This +is done by specifying the physics scene path, the collision prim path, the cloned prim paths, and the global prim paths. +The global prim paths are the prims that are not cloned and are shared between the cloned scenes. For instance, +the ground plane belongs to the global prim paths. + +.. literalinclude:: ../../../source/standalone/demo/play_cloner.py + :language: python + :lines: 116-120 + :linenos: + :lineno-start: 116 + +The Code Execution +~~~~~~~~~~~~~~~~~~ + +Now that we have gone through the code, let's run the script and see the result: + +.. code-block:: bash + + ./orbit.sh -p source/standalone/demo/play_cloner.py --robot franka_panda --num_robots 128 + +This should behave the same as the previous tutorial, except that it is much faster to spawn the robots. +To stop the simulation, you can either close the window, or press the ``STOP`` button in the UI, or +press ``Ctrl+C`` in the terminal. + +Now that we have learned how to design a scene with a robot and clone it multiple times, we can move on to +the next tutorial to learn how to control the robot with a task-space controller. diff --git a/docs/source/tutorials/03_ik_controller.rst b/docs/source/tutorials/03_ik_controller.rst new file mode 100644 index 0000000000..36a10ed913 --- /dev/null +++ b/docs/source/tutorials/03_ik_controller.rst @@ -0,0 +1,149 @@ +Using a task-space controller +============================= + +In the previous tutorials, we learned how to create a scene and control a robotic arm using a +joint-space controller. In this tutorial, we will learn how to use a task-space controller to +control the robot. More specifically, we will use the :class:`DifferentialInverseKinematics` class +to track a desired end-effector pose command. + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``play_ik_control.py`` script in the ``orbit/source/standalone/demo`` directory. + + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :emphasize-lines: 44-47,130-138,147,201-217 + :linenos: + +The Code Explained +~~~~~~~~~~~~~~~~~~ + +While using any task-space controller, it is important to ensure that the provided +quantities are in the correct frames. When parallelizing environment instances using the cloner, +each environment itself can be thought of having its own local frame. However, from the way +the physics engine is implemented in Isaac Sim, all environments exist on the same stage and +thus there is a unique global frame for the entire simulation. In summary, there are the three +main frames that are used in Orbit: + +- The simulation world frame (denoted as ``w``), which is the frame of the entire simulation. +- The local environment frame (denoted as ``e``), which is the frame of the local environment. +- The robot's base frame (denoted as ``b``), which is the frame of the robot's base link. + +In the current scenario, where the robot is mounted on the table, the base frame of the robot coincides with +the local environment frame. However, this is not always the case. For example, in a scenario where the robot +is a floating-base system. The location of the environment frames are obtained from the +:attr:`envs_positions` value returned by the cloner. + + +Creating an IK controller +------------------------- + +Computing the inverse kinematics (IK) of a robot is a common task in robotics. +The :class:`DifferentialInverseKinematics` class computes the desired joint positions +for a robot to reach a desired end-effector pose. The included implementation performs +the computation in a batched format and is optimized for speed. + +Since in many robots the end-effector is not a rigid body, the simulator does not provide +the pose and Jacobian of the end-effector directly. Instead, the obtained Jacobian +is that of the parent body and not the end-effector. Thus, the IK controller takes in +as input the end-effector offset from the parent frame. This offset is typically specified +in the robot's configuration instance and thus is obtained from there. + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :lines: 130-138 + :linenos: + :lineno-start: 130 + + +Computing robot command +----------------------- + +The IK controller separates the operation of setting the desired command and +computing the desired joint positions. This is done to allow for the user to +run the IK controller at a different frequency than the robot's control frequency. + +The :attr:`set_command` method takes in the desired end-effector pose as a single +batched array. The first three columns correspond to the desired position and the +last four columns correspond to the desired quaternion orientation in ``(w, x, y, z)`` +order. + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :lines: 201-202 + :linenos: + :lineno-start: 201 + +We can then compute the desired joint positions using the :attr:`compute` method. +The method takes in the current end-effector position, orientation, Jacobian, and +current joint positions. We read the Jacobian matrix from the robot's data, which uses +its value computed from the physics engine. + + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :lines: 203-209 + :linenos: + :lineno-start: 203 + +While the IK controller returns the desired joint positions, we need to convert +them to the robot's action space. This is done by subtracting joint positions +offsets from the desired joint positions. The joint offsets are obtained from the +robot's data which is a constant value obtained from the robot's configuration. +For more details, we suggest reading the :doc:`/source/api/orbit.actuators.group` tutorial. + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :lines: 210-215 + :linenos: + :lineno-start: 210 + +These actions can then be applied on the robot, as done in the previous tutorials. + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :lines: 216-219 + :linenos: + :lineno-start: 216 + +Using markers for displaying frames +----------------------------------- + +We will use the :class:`StaticMarker` class to display the current and desired end-effector poses. +The marker class takes as input the associated prim name, the number of markers to display, the +USD file corresponding to the marker, and the scale of the marker. By default, it uses a frame marker +to display the pose. + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :lines: 81-83 + :linenos: + :lineno-start: 81 + +We can then set the pose of the marker using the :attr:`set_world_poses` method. +It is important to ensure that the set poses are in the simulation world frame and not the +local environment frame. + +.. literalinclude:: ../../../source/standalone/demo/play_ik_control.py + :language: python + :lines: 223-229 + :linenos: + :lineno-start: 223 + + +The Code Execution +~~~~~~~~~~~~~~~~~~ + +Now that we have gone through the code, let's run the script and see the result: + +.. code-block:: bash + + ./orbit.sh -p source/standalone/demo/play_ik_control.py --robot franka_panda --num_robots 128 + +The script will start a simulation with 128 robots. The robots will be controlled using a task-space controller. +The current and desired end-effector poses should be displayed using frame markers. When the robot reaches the +desired pose, the command should cycle through to the next pose specified in the script. +To stop the simulation, you can either close the window, or press the ``STOP`` button in the UI, or +press ``Ctrl+C`` in the terminal. diff --git a/docs/source/tutorials/04_gym_env.rst b/docs/source/tutorials/04_gym_env.rst new file mode 100644 index 0000000000..23e593f2ce --- /dev/null +++ b/docs/source/tutorials/04_gym_env.rst @@ -0,0 +1,121 @@ +Running an RL environment +========================= + +In this tutorial, we will learn how to run existing learning environments provided in the ``omni.isaac.orbit_envs`` +extension. All the environments included in Orbit follow the ``gym.Env`` interface, which means that they can be used +with any reinforcement learning framework that supports OpenAI Gym. However, since the environments are implemented +in a vectorized fashion, they can only be used with frameworks that support vectorized environments. + +Many common frameworks come with their own desired definitions of a vectorized environment and require the returned data +to follow their supported data types and data structures. For example, ``stable-baselines3`` uses ``numpy`` arrays, while +``rsl-rl`` or ``rl-games`` use ``torch.Tensor``. We provide wrappers for these different frameworks, which can be found +in the ``omni.isaac.orbit_envs.utils.wrappers`` module. + + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``zero_agent.py`` script in the ``orbit/source/standalone/environments`` directory. + + +.. literalinclude:: ../../../source/standalone/environments/zero_agent.py + :language: python + :emphasize-lines: 35-36,42-45,50-57 + :linenos: + +The Code Explained +~~~~~~~~~~~~~~~~~~ + +Using gym registry for environments +----------------------------------- + +All environments are registered using the ``gym`` registry, which means that you can create an instance of +an environment by calling ``gym.make``. The environments are registered in the ``__init__.py`` file of the +``omni.isaac.orbit_envs`` extension with the following syntax: + +.. code-block:: python + + # Cartpole environment + gym.register( + id="Isaac-Cartpole-v0", + entry_point="omni.isaac.orbit_envs.classic.cartpole:CartpoleEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.classic.cartpole:cartpole_cfg.yaml"}, + ) + +The ``cfg_entry_point`` argument is used to load the default configuration for the environment. The default +configuration is loaded using the :meth:`omni.isaac.orbit_envs.utils.parse_cfg.load_default_env_cfg` function. +The configuration entry point can correspond to both a YAML file or a python configuration +class. The default configuration can be overridden by passing a custom configuration instance to the ``gym.make`` +function as shown later in the tutorial. + +To inform the ``gym`` registry with all the environments provided by the ``omni.isaac.orbit_envs`` extension, +we must import the module at the start of the script. + +.. literalinclude:: ../../../source/standalone/environments/zero_agent.py + :language: python + :lines: 32-36 + :linenos: + :lineno-start: 32 + +.. note:: + + As a convention, we name all the environments in ``omni.isaac.orbit_envs`` extension with the prefix ``Isaac-``. + For more complicated environments, we follow the pattern: ``Isaac---v``, + where `N` is used to specify different observations or action spaces within the same task definition. For example, + for legged locomotion with ANYmal C, the environment is called ``Isaac-Velocity-Anymal-C-v0``. + + +In this tutorial, the task name is read from the command line. The task name is used to load the default configuration +as well as to create the environment instance. In addition, other parsed command line arguments such as the +number of environments, the simulation device, and whether to render, are used to override the default configuration. + +.. literalinclude:: ../../../source/standalone/environments/zero_agent.py + :language: python + :lines: 42-45 + :linenos: + :lineno-start: 42 + + +Running the environment +----------------------- + +Once creating the environment, the rest of the execution follows the standard resetting and stepping. + +.. literalinclude:: ../../../source/standalone/environments/zero_agent.py + :language: python + :lines: 47-54 + :linenos: + :lineno-start: 47 + +Similar to previous tutorials, to ensure a safe exit when running the script, we need to add checks +for whether the simulation is stopped or not. + +.. literalinclude:: ../../../source/standalone/environments/zero_agent.py + :language: python + :lines: 55-57 + :linenos: + :lineno-start: 55 + + +The Code Execution +~~~~~~~~~~~~~~~~~~ + +Now that we have gone through the code, let's run the script and see the result: + +.. code-block:: bash + + ./orbit.sh -p source/standalone/environments/zero_agent.py --task Isaac-Cartpole-v0 --num_envs 32 + + +This should open a stage with a ground plane, lights and 32 cartpoles spawned in a grid. The cartpole +would be falling down since no actions are acting on them. To stop the simulation, +you can either close the window, or press the ``STOP`` button in the UI, or press ``Ctrl+C`` +in the terminal. + +.. note:: + When running environments with GPU pipeline, the states in the scene are not synced with the USD + interface. Therefore values in the UI may appear wrong when simulation is running. Although objects + may be updating in the Viewport, attribute values in the UI will not update along with them. + + To enable USD synchronization, please use the CPU pipeline with ``--cpu`` and disable flatcache by setting + ``use_flatcache`` to False in the environment configuration. diff --git a/orbit.sh b/orbit.sh new file mode 100755 index 0000000000..ee40b20e4f --- /dev/null +++ b/orbit.sh @@ -0,0 +1,165 @@ +#!/bin/bash + +#== +# Configurations +#== + +# Exits if error occurs +set -e + +# Set tab-spaces +tabs 4 + +# get source directory +export ORBIT_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +#== +# Helper functions +#== + +# extract the python from isaacsim +extract_isaacsim_python() { + # Check if IsaacSim directory manually specified + # Note: for manually build isaacsim, this: _build/linux-x86_64/release + if [ ! -z ${ISAACSIM_PATH} ]; + then + # Use local build + build_path=${ISAACSIM_PATH} + else + # Use TeamCity build + build_path=${ORBIT_PATH}/_isaac_sim + fi + # python executable to use + local python_exe=${build_path}/python.sh + # check if there is a python path available + if [ ! -f "${python_exe}" ]; then + echo "[ERROR] No python executable found at path: ${build_path}" >&2 + exit 1 + fi + # return the result + echo ${python_exe} +} + +# extract the simulator exe from isaacsim +extract_isaacsim_exe() { + # Check if IsaacSim directory manually specified + # Note: for manually build isaacsim, this: _build/linux-x86_64/release + if [ ! -z ${ISAACSIM_PATH} ]; + then + # Use local build + build_path=${ISAACSIM_PATH} + else + # Use TeamCity build + build_path=${ORBIT_PATH}/_isaac_sim + fi + # python executable to use + local isaacsim_exe=${build_path}/isaac-sim.sh + # check if there is a python path available + if [ ! -f "${isaacsim_exe}" ]; then + echo "[ERROR] No isaac-sim executable found at path: ${build_path}" >&2 + exit 1 + fi + # return the result + echo ${isaacsim_exe} +} + +# check if input directory is a python extension and install the module +install_orbit_extension() { + # retrieve the python executable + python_exe=$(extract_isaacsim_python) + # 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 + fi +} + +# print the usage description +print_help () { + echo -e "\nusage: $(basename "$0") [-h] [-i] [-e] [-p] [-s] -- Utility to manage extensions in Isaac Orbit." + echo -e "\noptional arguments:" + echo -e "\t-h, --help Display the help content." + echo -e "\t-i, --install Install the extensions inside Isaac Orbit." + echo -e "\t-e, --extra Install extra dependencies such as the learning frameworks." + echo -e "\t-p, --python Run the python executable (python.sh) provided by Isaac Sim." + echo -e "\t-s, --sim Run the simulator executable (isaac-sim.sh) provided by Isaac Sim." + echo -e "\n" >&2 +} + + +#== +# Main +#== + +# check argument provided +if [ -z "$*" ]; then + echo "[Error] No arguments provided." >&2; + print_help + exit 1 +fi + +# pass the arguments +while [[ $# -gt 0 ]]; do + # read the key + case "$1" in + # install the python packages in omni_isaac_orbit/source directory + -i|--install) + echo "[INFO] Installing extensions inside orbit repository..." + # recursively look into directories and install them + # this does not check dependencies between extensions + export -f extract_isaacsim_python + export -f install_orbit_extension + # initialize git hooks + pip install pre-commit + # source directory + find -L "${ORBIT_PATH}/source/extensions" -mindepth 1 -maxdepth 1 -type d -exec bash -c 'install_orbit_extension "{}"' \; + # unset local variables + unset install_orbit_extension + shift # past argument + ;; + # install the python packages for supported reinforcement learning frameworks + -e|--extra) + echo "[INFO] Installing extra requirements such as learning frameworks..." + python_exe=$(extract_isaacsim_python) + # install the rl-frameworks specified + ${python_exe} -m pip install -e ${ORBIT_PATH}/source/extensions/omni.isaac.orbit_envs[all] + shift # past argument + ;; + # run the python provided by isaacsim + -p|--python) + python_exe=$(extract_isaacsim_python) + echo "[INFO] Using python from: ${python_exe}" + shift # past argument + ${python_exe} $@ + # exit neatly + break + ;; + # run the simulator exe provided by isaacsim + -s|--sim) + isaacsim_exe=$(extract_isaacsim_exe) + echo "[INFO] Running isaac-sim from: ${isaacsim_exe}" + shift # past argument + ${isaacsim_exe} --ext-folder ${ORBIT_PATH}/source/extensions $@ + # exit neatly + break + ;; + # run the formatter over the repository + -f|--format) + echo "[INFO] Formatting the repository..." + pre-commit run --all-files + shift # past argument + # exit neatly + break + ;; + -h|--help) + print_help + exit 1 + ;; + *) # unknown option + echo "[Error] Invalid argument provided: $1" + print_help + exit 1 + ;; + esac +done diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000000..5abd8f97fe --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,80 @@ +[tool.isort] + +py_version = 37 +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", + "LOCALFOLDER", +] + +# Extra standard libraries considered as part of python (permissive licenses +extra_standard_library = [ + "numpy", + "h5py", + "open3d", + "torch", + "bpy", + "matplotlib", + "gym", + "scipy", + "hid", + "yaml", + "prettytable", +] +# Imports from Isaac Sim and Omniverse +known_third_party = [ + "omni.isaac.core", + "omni.replicator.isaac", + "omni.replicator.core", + "pxr", + "omni.kit.*", + "warp", + "carb", +] +# Imports from this repository +known_first_party = "omni.isaac.orbit" +known_assets_firstparty = "omni.isaac.assets" +known_extra_firstparty = [ + "omni.isaac.contrib_envs", + "omni.isaac.orbit_envs" +] +# Imports from the local folder +known_local_folder = "config" + +[tool.pyright] + +include = ["source/extensions", "source/standalone"] +exclude = [ + "**/__pycache__", + "**/_isaac_sim", + "**/docs", + "**/logs", + ".git", + ".vscode", +] + +typeCheckingMode = "basic" +pythonVersion = "3.7" +pythonPlatform = "Linux" +enableTypeIgnoreComments = true + +# This is required as the CI pre-commit does not download the module (i.e. numpy, torch, prettytable) +# Therefore, we have to ignore missing imports +reportMissingImports = "none" +# This is required to ignore for type checks of modules with stubs missing. +reportMissingModuleSource = "none" # -> most common: prettytable in mdp managers + +reportGeneralTypeIssues = "none" # -> raises 218 errors (usage of literal MISSING in dataclasses) +reportOptionalMemberAccess = "warning" # -> raises 8 errors +reportPrivateUsage = "warning" diff --git a/source/extensions/omni.isaac.contrib_envs/config/extension.toml b/source/extensions/omni.isaac.contrib_envs/config/extension.toml new file mode 100644 index 0000000000..0646d6ef61 --- /dev/null +++ b/source/extensions/omni.isaac.contrib_envs/config/extension.toml @@ -0,0 +1,22 @@ +[package] + +# Note: Semantic Versioning is used: https://semver.org/ +version = "0.0.1" + +# Description +title = "ORBIT Environments contributed by the community." +description="Extension containing environments for robot learning contributed by the community." +readme = "docs/README.md" +repository = "https://github.com/NVIDIA-Omniverse/Orbit" +category = "robotics" +keywords = ["robotics", "rl", "il", "learning"] + +[dependencies] +"omni.isaac.orbit" = {} +"omni.isaac.orbit_envs" = {} +"omni.isaac.core" = {} +"omni.isaac.gym" = {} +"omni.replicator.isaac" = {} + +[[python.module]] +name = "omni.isaac.contrib_envs" diff --git a/source/extensions/omni.isaac.contrib_envs/data/.gitkeep b/source/extensions/omni.isaac.contrib_envs/data/.gitkeep new file mode 100644 index 0000000000..e69de29bb2 diff --git a/source/extensions/omni.isaac.contrib_envs/docs/CHANGELOG.rst b/source/extensions/omni.isaac.contrib_envs/docs/CHANGELOG.rst new file mode 100644 index 0000000000..f903c90ace --- /dev/null +++ b/source/extensions/omni.isaac.contrib_envs/docs/CHANGELOG.rst @@ -0,0 +1,10 @@ +Changelog +--------- + +0.0.1 (2023-01-17) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Initial dummy extension for community to contribute environments. diff --git a/source/extensions/omni.isaac.contrib_envs/docs/README.md b/source/extensions/omni.isaac.contrib_envs/docs/README.md new file mode 100644 index 0000000000..16b080448d --- /dev/null +++ b/source/extensions/omni.isaac.contrib_envs/docs/README.md @@ -0,0 +1,26 @@ +# Orbit: Contributed Environment + +This extension serves as a platform to host contributed environments from the robotics and machine learning +community. The extension follows the same style as the `omni.isaac.orbit_envs` extension. + +The environments should follow the `gym.Env` API from OpenAI Gym version `0.21.0`. They need to be registered using +the Gym registry. + +To follow the same convention, each environment's name is composed of `Isaac-Contrib---v`, +where `` indicates the skill to learn in the environment, `` indicates the embodiment of the +acting agent, and `` represents the version of the environment (which can be used to suggest different +observation or action spaces). + +The environments can be configured using either Python classes (wrapped using `configclass` decorator) or through +YAML files. The template structure of the environment is always put at the same level as the environment file +itself. However, its various instances should be included in directories within the environment directory itself. + +The environments should then be registered in the `omni/isaac/contrib_envs/__init__.py`: + +```python +gym.register( + id="Isaac-Contrib--v0", + entry_point="omni.isaac.contrib_envs.:", + kwargs={"cfg_entry_point": "omni.isaac.contrib_envs.:"}, +) +``` diff --git a/source/extensions/omni.isaac.contrib_envs/omni/isaac/contrib_envs/__init__.py b/source/extensions/omni.isaac.contrib_envs/omni/isaac/contrib_envs/__init__.py new file mode 100644 index 0000000000..2284151215 --- /dev/null +++ b/source/extensions/omni.isaac.contrib_envs/omni/isaac/contrib_envs/__init__.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Module containing environments contributed by the community. + + +We use OpenAI Gym registry to register the environment and their default configuration file. +The default configuration file is passed to the argument "kwargs" in the Gym specification registry. +The string is parsed into respective configuration container which needs to be passed to the environment +class. This is done using the function :meth:`load_default_env_cfg` in the sub-module +:mod:`omni.isaac.orbit.utils.parse_cfg`. + +Note: + This is a slight abuse of kwargs since they are meant to be directly passed into the environment class. + Instead, we remove the key :obj:`cfg_file` from the "kwargs" dictionary and the user needs to provide + the kwarg argument :obj:`cfg` while creating the environment. + +Usage: + >>> import gym + >>> import omni.isaac.contrib_envs + >>> from omni.isaac.orbit_envs.utils.parse_cfg import load_default_env_cfg + >>> + >>> task_name = "Isaac-Contrib--v0" + >>> cfg = load_default_env_cfg(task_name) + >>> env = gym.make(task_name, cfg=cfg) +""" + + +import gym # noqa: F401 +import os + +# Conveniences to other module directories via relative paths +ORBIT_CONTRIB_ENVS_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) +"""Path to the extension source directory.""" + +ORBIT_CONTRIB_ENVS_DATA_DIR = os.path.join(ORBIT_CONTRIB_ENVS_EXT_DIR, "data") +"""Path to the extension data directory.""" diff --git a/source/extensions/omni.isaac.contrib_envs/setup.py b/source/extensions/omni.isaac.contrib_envs/setup.py new file mode 100644 index 0000000000..565c7a7f41 --- /dev/null +++ b/source/extensions/omni.isaac.contrib_envs/setup.py @@ -0,0 +1,24 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Installation script for the 'omni.isaac.contrib_envs' python package.""" + + +from setuptools import setup + +# Installation operation +setup( + name="omni-isaac-contrib_envs", + author="Community", + url="https://github.com/NVIDIA-Omniverse/orbit", + version="0.1.0", + description="Python module for contributed robotic environments built using ORBIT / Isaac Sim.", + keywords=["robotics", "rl"], + include_package_data=True, + python_requires=">=3.7.*", + packages=["omni.isaac.contrib_envs"], + classifiers=["Natural Language :: English", "Programming Language :: Python :: 3.7"], + zip_safe=False, +) diff --git a/source/extensions/omni.isaac.orbit/config/extension.toml b/source/extensions/omni.isaac.orbit/config/extension.toml new file mode 100644 index 0000000000..49bfeaf1ff --- /dev/null +++ b/source/extensions/omni.isaac.orbit/config/extension.toml @@ -0,0 +1,19 @@ +[package] + +# Note: Semantic Versioning is used: https://semver.org/ +version = "0.1.0" + +# Description +title = "ORBIT framework for Robot Learning" +description="Extension providing main framework interfaces and abstractions for robot learning." +readme = "docs/README.md" +repository = "https://github.com/NVIDIA-Omniverse/Orbit" +category = "robotics" +keywords = ["kit", "robotics", "learning", "ai"] + +[dependencies] +"omni.isaac.core" = {} +"omni.replicator.core" = {} + +[[python.module]] +name = "omni.isaac.orbit" diff --git a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst new file mode 100644 index 0000000000..032e250d17 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst @@ -0,0 +1,15 @@ +Changelog +--------- + +0.1.0 (2023-01-17) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Initial release of the extension with experimental API. +* Available robot configurations: + + * **Quadrupeds:** Unitree A1, ANYmal B, ANYmal C + * **Single-arm manipulators:** Franka Emika arm, UR5 + * **Mobile manipulators:** Clearpath Ridgeback with Franka Emika arm or UR5 diff --git a/source/extensions/omni.isaac.orbit/docs/README.md b/source/extensions/omni.isaac.orbit/docs/README.md new file mode 100644 index 0000000000..5ecc6e08fb --- /dev/null +++ b/source/extensions/omni.isaac.orbit/docs/README.md @@ -0,0 +1,11 @@ +# Orbit: Framework + +Orbit includes its own set of interfaces and wrappers around Isaac Sim classes. One of the main goals behind this +decision is to have a unified description for different systems. While isaac Sim tries to be general for a wider +variety of simulation requires, our goal has been to specialize these for learning requirements. These include +features such as augmenting simulators with non-ideal actuator models, managing different observation and reward +settings, integrate different sensors, as well as provide interfaces to features that are currently not available in +Isaac Sim but are available from the physics side (such as deformable bodies). + +We recommend the users to try out the demo scripts present in `standalone/demos` that display how different parts +of the framework can be integrated together. diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/__init__.py new file mode 100644 index 0000000000..645fb929e7 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/__init__.py @@ -0,0 +1,21 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Python module with robotic environments. +""" + + +import os + +# Conveniences to other module directories via relative paths +ORBIT_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) +"""Path to the extension source directory.""" + +ORBIT_DATA_DIR = os.path.join(ORBIT_EXT_DIR, "data") +"""Path to the extension data directory.""" + +__author__ = "Mayank Mittal" +__email__ = "mittalma@ethz.ch" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/__init__.py new file mode 100644 index 0000000000..7e4a8b4c74 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Subpackage for handling actuator models and groups.""" + +from .group import * +from .model import * diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/__init__.py new file mode 100644 index 0000000000..25a81c2113 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/__init__.py @@ -0,0 +1,19 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodule containing configuration instances for commonly used robots. +""" + +from .anydrive import ANYDRIVE_3_ACTUATOR_CFG, ANYDRIVE_SIMPLE_ACTUATOR_CFG +from .franka import PANDA_HAND_MIMIC_GROUP_CFG + +__all__ = [ + # ANYmal actuators + "ANYDRIVE_SIMPLE_ACTUATOR_CFG", + "ANYDRIVE_3_ACTUATOR_CFG", + # Franka panda actuators + "PANDA_HAND_MIMIC_GROUP_CFG", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/allegro.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/allegro.py new file mode 100644 index 0000000000..5e78d9836f --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/allegro.py @@ -0,0 +1,24 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.orbit.actuators.group import ActuatorControlCfg, GripperActuatorGroupCfg +from omni.isaac.orbit.actuators.model import ImplicitActuatorCfg + +# TODO: Fix the configuration. +ALLEGRO_HAND_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( + dof_names=[".*finger_joint", ".*_inner_knuckle_joint", ".*_inner_finger_joint", ".*right_outer_knuckle_joint"], + model_cfg=ImplicitActuatorCfg(velocity_limit=2.0, torque_limit=1000.0), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + mimic_multiplier={ + ".*finger_joint": 1.0, + ".*_inner_knuckle_joint": -1.0, + ".*_inner_finger_joint": 1.0, + ".*right_outer_knuckle_joint": -1.0, + }, + speed=0.1, + open_dof_pos=0.87, + close_dof_pos=0.0, +) +"""Configuration for Allegro hand with implicit actuator model.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/anydrive.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/anydrive.py new file mode 100644 index 0000000000..db7e1c32a8 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/anydrive.py @@ -0,0 +1,50 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Configuration instances of actuation models for ANYmal robot. +""" + +from omni.isaac.orbit.actuators.group import ActuatorControlCfg, ActuatorGroupCfg +from omni.isaac.orbit.actuators.model import ActuatorNetLSTMCfg, DCMotorCfg +from omni.isaac.orbit.utils.assets import ISAAC_ORBIT_NUCLEUS_DIR + +""" +Actuator Models. +""" + +ANYDRIVE_SIMPLE_ACTUATOR_CFG = DCMotorCfg( + peak_motor_torque=120.0, motor_torque_limit=80.0, motor_velocity_limit=7.5, gear_ratio=1.0 +) +"""Configuration for ANYdrive 3.x with DC actuator model.""" + +ANYDRIVE_3_ACTUATOR_CFG = ActuatorNetLSTMCfg( + network_file=f"{ISAAC_ORBIT_NUCLEUS_DIR}/ActuatorNets/anydrive_3_lstm_jit.pt", + peak_motor_torque=120.0, + motor_torque_limit=89.0, + motor_velocity_limit=7.5, + gear_ratio=1.0, +) +"""Configuration for ANYdrive 3.0 (used on ANYmal-C) with LSTM actuator model.""" + +""" +Actuator Groups. +""" + +ANYMAL_C_DEFAULT_GROUP_CFG = ActuatorGroupCfg( + dof_names=[".*HAA", ".*HFE", ".*KFE"], + model_cfg=ANYDRIVE_3_ACTUATOR_CFG, + control_cfg=ActuatorControlCfg( + command_types=["p_abs"], + dof_pos_offset={ + ".*HAA": 0.0, # all HAA + ".*F_HFE": 0.4, # both front HFE + ".*H_HFE": -0.4, # both hind HFE + ".*F_KFE": -0.8, + ".*H_KFE": 0.8, + }, + ), +) +"""Configuration for default ANYmal-C quadruped with LSTM actuator network.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/franka.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/franka.py new file mode 100644 index 0000000000..0abe8e67e5 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/franka.py @@ -0,0 +1,22 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.orbit.actuators.group import ActuatorControlCfg, GripperActuatorGroupCfg +from omni.isaac.orbit.actuators.model import ImplicitActuatorCfg + +""" +Actuator Groups. +""" + +PANDA_HAND_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( + dof_names=["panda_finger_joint[1-2]"], + model_cfg=ImplicitActuatorCfg(velocity_limit=0.2, torque_limit=200), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + mimic_multiplier={"panda_finger_joint.*": 1.0}, + speed=0.1, + open_dof_pos=0.04, + close_dof_pos=0.0, +) +"""Configuration for Franka Panda hand with implicit actuator model.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/kinova.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/kinova.py new file mode 100644 index 0000000000..5821cc7afc --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/kinova.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.orbit.actuators.group import ActuatorControlCfg, GripperActuatorGroupCfg +from omni.isaac.orbit.actuators.model import ImplicitActuatorCfg + +""" +Actuator Groups. +""" + +KINOVA_S300_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( + dof_names=[".*_joint_finger_[1-3]", ".*_joint_finger_tip_[1-3]"], + model_cfg=ImplicitActuatorCfg(velocity_limit=1.0, torque_limit=2.0), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + mimic_multiplier={".*_joint_finger_[1-3]": 1.0, ".*_joint_finger_tip_[1-3]": 0.0}, + speed=0.1, + open_dof_pos=0.65, + close_dof_pos=0.0, +) +"""Configuration for Kinova S300 hand with implicit actuator model.""" + +KINOVA_S200_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( + dof_names=[".*_joint_finger_[1-2]", ".*_joint_finger_tip_[1-2]"], + model_cfg=ImplicitActuatorCfg(velocity_limit=1.0, torque_limit=2.0), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + mimic_multiplier={".*_joint_finger_[1-2]": 1.0, ".*_joint_finger_tip_[1-2]": 0.0}, + speed=0.1, + open_dof_pos=0.65, + close_dof_pos=0.0, +) +"""Configuration for Kinova S200 hand with implicit actuator model.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/robotiq.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/robotiq.py new file mode 100644 index 0000000000..aa7f886b80 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/robotiq.py @@ -0,0 +1,59 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.orbit.actuators.group import ActuatorControlCfg, GripperActuatorGroupCfg +from omni.isaac.orbit.actuators.model import ImplicitActuatorCfg + +""" +Actuator Groups. +""" + +ROBOTIQ_2F140_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( + dof_names=["finger_joint", ".*_inner_knuckle_joint", ".*_inner_finger_joint", ".*right_outer_knuckle_joint"], + model_cfg=ImplicitActuatorCfg(velocity_limit=2.0, torque_limit=1000.0), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + mimic_multiplier={ + "finger_joint": 1.0, # mimicked joint + ".*_inner_knuckle_joint": -1.0, + ".*_inner_finger_joint": 1.0, + ".*right_outer_knuckle_joint": -1.0, + }, + speed=0.01, + open_dof_pos=0.7, + close_dof_pos=0.0, +) +"""Configuration for Robotiq 2F-140 gripper with implicit actuator model.""" + +ROBOTIQ_2F85_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( + dof_names=["finger_joint", ".*_inner_knuckle_joint", ".*_inner_finger_joint", ".*right_outer_knuckle_joint"], + model_cfg=ImplicitActuatorCfg(velocity_limit=2.0, torque_limit=1000.0), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + mimic_multiplier={ + "finger_joint": 1.0, # mimicked joint + ".*_inner_knuckle_joint": 1.0, + ".*_inner_finger_joint": -1.0, + ".*right_outer_knuckle_joint": 1.0, + }, + speed=0.01, + open_dof_pos=0.8, + close_dof_pos=0.0, +) +"""Configuration for Robotiq 2F-85 gripper with implicit actuator model.""" + +ROBOTIQ_C2_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( + dof_names=[".*_left_knuckle_joint", ".*_right_knuckle_joint", ".*_inner_knuckle_joint", ".*_finger_tip_joint"], + model_cfg=ImplicitActuatorCfg(velocity_limit=2.0, torque_limit=1000.0), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + mimic_multiplier={ + ".*_left_knuckle_joint": 1.0, # mimicked joint + ".*_right_knuckle_joint": 1.0, + ".*_inner_knuckle_joint": 1.0, + ".*_finger_tip_joint": -1.0, + }, + speed=0.01, + open_dof_pos=0.85, + close_dof_pos=0.0, +) +"""Configuration for Robotiq C2 gripper with implicit actuator model.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/__init__.py new file mode 100644 index 0000000000..126cf5f76c --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/__init__.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Module containing different actuator groups. + +- **default**: Direct control over the DOFs handled by the actuator group. +- **mimic**: Mimics given commands into each DOFs handled by actuator group. +- **non-holonomic**: Adds a 2D kinematics skid-steering constraint for the actuator group. +""" + +from .actuator_control_cfg import ActuatorControlCfg +from .actuator_group import ActuatorGroup +from .actuator_group_cfg import ActuatorGroupCfg, GripperActuatorGroupCfg, NonHolonomicKinematicsGroupCfg +from .gripper_group import GripperActuatorGroup +from .non_holonomic_group import NonHolonomicKinematicsGroup + +__all__ = [ + # control + "ActuatorControlCfg", + # default + "ActuatorGroupCfg", + "ActuatorGroup", + # mimic + "GripperActuatorGroupCfg", + "GripperActuatorGroup", + # non-holonomic + "NonHolonomicKinematicsGroupCfg", + "NonHolonomicKinematicsGroup", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_control_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_control_cfg.py new file mode 100644 index 0000000000..3e6a8c438a --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_control_cfg.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Dict, List, Optional + +from omni.isaac.orbit.utils import configclass + + +@configclass +class ActuatorControlCfg: + """Configuration for the joint-level controller used by the group. + + This configuration is used by the ActuatorGroup class to configure the commands types and their + respective scalings and offsets to apply on the input actions over the actuator group. If the + scales and offsets are set to :obj:`None`, then no scaling or offset is applied on the commands. + + Depending on the actuator model type, the gains are set either into the simulator (implicit) or to + the actuator model class (explicit). + + The command types are processed as a list of strings. Each string has two sub-strings joined by + underscore: + + - type of command mode: "p" (position), "v" (velocity), "t" (torque) + - type of command resolving: "abs" (absolute), "rel" (relative) + + For instance, the command type "p_abs" defines a position command in absolute mode, while "v_rel" + defines a velocity command in relative mode. For more information on the command types, see the + documentation of the :class:`ActuatorGroup` class. + """ + + command_types: List[str] = MISSING + """ + Command types applied on the DOF in the group. + + Note: + The first string in the list defines the type of DOF drive mode in simulation. It must contain either + "p" (position-controlled), "v" (velocity-controlled), or "t" (torque-controlled). + """ + + stiffness: Optional[Dict[str, float]] = None + """ + Stiffness gains of the DOFs in the group. Defaults to :obj:`None`. + + If :obj:`None`, these are loaded from the articulation prim. + """ + + damping: Optional[Dict[str, float]] = None + """ + Damping gains of the DOFs in the group. Defaults to :obj:`None`. + + If :obj:`None`, these are loaded from the articulation prim. + """ + + dof_pos_offset: Optional[Dict[str, float]] = None + """ + DOF position offsets used for processing commands. Defaults to :obj:`None`. + + If :obj:`None`, these are processed as zero, i.e. absolute commands. + """ + + dof_pos_scale: Optional[Dict[str, float]] = None + """ + DOF position scaling factor used for processing commands. Defaults to :obj:`None`. + + If :obj:`None`, these are processed as ones, i.e. absolute commands. + """ + + dof_vel_scale: Optional[Dict[str, float]] = None + """ + DOF velocity scaling factor used for processing commands. Defaults to :obj:`None`. + + If :obj:`None`, these are processed as ones, i.e. absolute commands. + """ + + dof_torque_scale: Optional[Dict[str, float]] = None + """ + DOF torque scaling factor used for processing commands. Defaults to :obj:`None`. + + If :obj:`None`, these are processed as ones, i.e. absolute commands. + """ diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_group.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_group.py new file mode 100644 index 0000000000..f2cf8def41 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_group.py @@ -0,0 +1,468 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import re +import torch +from typing import List, Optional, Sequence + +from omni.isaac.core.articulations import ArticulationView +from omni.isaac.core.utils.types import ArticulationActions + +from omni.isaac.orbit.actuators.group.actuator_group_cfg import ActuatorGroupCfg +from omni.isaac.orbit.actuators.model import * # noqa: F403, F401 +from omni.isaac.orbit.actuators.model import DCMotor, IdealActuator, ImplicitActuatorCfg + + +class ActuatorGroup: + """ + The default actuator group for applying the same actuator model over a collection of actuated joints in + an articulation. + + The joint names are specified in the configuration through a list of regular expressions. The regular + expressions are matched against the joint names in the articulation. The first match is used to determine + the joint indices in the articulation. + + It is possible to specify multiple joint-level command types (position, velocity or torque control). The + command types are applied in the order they are specified in the configuration. For each command, the + scaling and offset can be configured through the :class:`ActuatorControlCfg` class. + + In the default actuator group, no constraints or formatting is performed over the input actions. Thus, the + input actions are directly used to compute the joint actions in the :meth:`compute`. + """ + + cfg: ActuatorGroupCfg + """The configuration of the actuator group.""" + view: ArticulationView + """The simulation articulation view.""" + num_articulations: int + """Number of articulations in the view.""" + device: str + """Device used for processing.""" + dof_names: List[str] + """Articulation's DOF names that are part of the group.""" + dof_indices: List[int] + """Articulation's DOF indices that are part of the group.""" + model: Optional[IdealActuator] + """Actuator model used by the group. + + If model type is "implicit" (i.e., when :obj:`ActuatorGroupCfg.model_cfg` is instance of + :class:`ImplicitActuatorCfg`), then `model` is set to :obj:`None`. + """ + dof_pos_offset: torch.Tensor + """DOF position offsets used for processing commands.""" + dof_pos_scale: torch.Tensor + """DOF position scale used for processing commands.""" + dof_vel_scale: torch.Tensor + """DOF velocity scale used for processing commands.""" + dof_torque_scale: torch.Tensor + """DOF torque scale used for processing commands.""" + + def __init__(self, cfg: ActuatorGroupCfg, view: ArticulationView): + """Initialize the actuator group. + + Args: + cfg (ActuatorGroupCfg): The configuration of the actuator group. + view (ArticulationView): The simulation articulation view. + + Raises: + ValueError: When no command types are specified in the configuration. + RuntimeError: When the articulation view is not initialized. + ValueError: When not able to find a match for all DOF names in the configuration. + ValueError: When the actuator model configuration is invalid, i.e. not "explicit" or "implicit". + """ + # check valid inputs + if len(cfg.control_cfg.command_types) < 1: + raise ValueError("Each actuator group must have at least one command type. Received: 0.") + if not view.initialized: + raise RuntimeError("Actuator group expects an initialized articulation view.") + # save parameters + self.cfg = cfg + self.view = view + # extract useful quantities + self.num_articulation = self.view.count + self.device = self.view._device + + # TODO (@mayank): Make regex resolving safer by throwing errors for keys that didn't find a match. + # -- from articulation dof names + self.dof_names = list() + self.dof_indices = list() + for dof_name in self.view.dof_names: + # dof-names and indices + for re_key in self.cfg.dof_names: + if re.fullmatch(re_key, dof_name): + # add to group details + self.dof_names.append(dof_name) + self.dof_indices.append(self.view.get_dof_index(dof_name)) + # check that group is valid + if len(self.dof_names) == 0: + raise ValueError(f"Unable to find any joints associated with actuator group. Input: {self.cfg.dof_names}.") + + # process configuration + # -- create actuator model + if self.model_type == "explicit": + actuator_model_cls = eval(self.cfg.model_cfg.cls_name) + self.model = actuator_model_cls( + cfg=self.cfg.model_cfg, + num_actuators=self.num_actuators, + num_envs=self.num_articulation, + device=self.device, + ) + elif self.model_type == "implicit": + self.model = None + else: + raise ValueError( + f"Invalid articulation model type. Received: '{self.model_type}'. Expected: 'explicit' or 'implicit'." + ) + # -- action transforms + self._process_action_transforms_cfg() + # -- gains + self._process_actuator_gains_cfg() + # -- torque limits + self._process_actuator_torque_limit_cfg() + # -- control mode + self.view.switch_control_mode(self.control_mode, joint_indices=self.dof_indices) + + # create buffers for allocation + # -- state + self._dof_pos = torch.zeros(self.num_articulation, self.num_actuators, device=self.device) + self._dof_vel = torch.zeros_like(self._dof_pos) + # -- commands + self._computed_torques = torch.zeros_like(self._dof_pos) + self._applied_torques = torch.zeros_like(self._dof_pos) + self._gear_ratio = torch.ones_like(self._dof_pos) + + def __str__(self) -> str: + """A string representation of the actuator group.""" + msg = ( + " object:\n" + f"\tNumber of DOFs: {self.num_actuators}\n" + f"\tDOF names : {self.dof_names}\n" + f"\tDOF indices : {self.dof_indices}\n" + f"\tActuator model: {self.model_type}\n" + f"\tCommand types : {self.command_types}\n" + f"\tControl mode : {self.control_mode}\n" + f"\tControl dim : {self.control_dim}" + ) + return msg + + """ + Properties- Group. + """ + + @property + def num_actuators(self) -> int: + """Number of actuators in the group.""" + return len(self.dof_names) + + @property + def model_type(self) -> str: + """Type of actuator model: "explicit" or "implicit". + + - **explicit**: Computes joint torques to apply into the simulation. + - **implicit**: Uses the in-built PhysX joint controller. + """ + return self.cfg.model_cfg.model_type + + @property + def command_types(self) -> List[str]: + """Command type applied on the DOF in the group. + + It contains a list of strings. Each string has two sub-strings joined by underscore: + - type of command mode: "p" (position), "v" (velocity), "t" (torque) + - type of command resolving: "abs" (absolute), "rel" (relative) + """ + return self.cfg.control_cfg.command_types + + @property + def control_dim(self) -> int: + """Dimension of control actions. + + The number of control actions is a product of number of actuated joints in the group and the + number of command types provided in the control confgiruation. + """ + return self.num_actuators * len(self.command_types) + + @property + def control_mode(self) -> str: + """Simulation drive control mode. + + For explicit actuator models, the control mode is always "effort". For implicit actuators, the + control mode is prioritized by the first command type in the control configuration. It is either: + + - "position" (position-controlled) + - "velocity" (velocity-controlled) + - "effort" (torque-controlled). + + """ + if self.model_type == "explicit": + return "effort" + elif self.model_type == "implicit": + # get first command type. + drive_type = self.command_types[0] + # check valid drive type + if "p" in drive_type: + return "position" + elif "v" in drive_type: + return "velocity" + elif "t" in drive_type: + return "effort" + else: + raise ValueError(f"Invalid primary control mode '{drive_type}'. Expected substring: 'p', 'v', 't'.") + else: + raise ValueError( + f"Invalid actuator model in group '{self.model_type}'. Expected: 'explicit' or 'implicit'." + ) + + """ + Properties- Actuator Model. + """ + + @property + def gear_ratio(self) -> torch.Tensor: + """Gear-box conversion factor from motor axis to joint axis.""" + return self._gear_ratio + + @property + def computed_torques(self) -> torch.Tensor: + """DOF torques computed from the actuator model (before clipping). + + Note: The torques are zero for implicit actuator models. + """ + return self._computed_torques + + @property + def applied_torques(self) -> torch.Tensor: + """DOF torques applied from the actuator model to simulation (after clipping). + + Note: The torques are zero for implicit actuator models. + """ + return self._applied_torques + + @property + def velocity_limit(self) -> Optional[torch.Tensor]: + """DOF velocity limits from actuator. + + Returns :obj:`None` for implicit and ideal actuator. + """ + if isinstance(self.model, DCMotor): + return self.model.cfg.motor_velocity_limit / self.model.gear_ratio + else: + return None + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int]): + """Reset the internals within the group. + + Args: + env_ids (Sequence[int]): List of environment IDs to reset. + """ + # actuator + if self.model is not None: + self.model.reset(env_ids) + + def compute(self, group_actions: torch.Tensor, dof_pos: torch.Tensor, dof_vel: torch.Tensor) -> ArticulationActions: + """Process the actuator group actions and compute the articulation actions. + + It performs the following operations: + + 1. formats the input actions to apply any extra constraints + 2. splits the formatted actions into individual tensors corresponding to each command type + 3. applies offset and scaling to individual commands based on absolute or relative command + 4. computes the articulation actions based on the actuator model type + + Args: + group_actions (torch.Tensor): The actuator group actions of shape (num_articulation, control_dim). + dof_pos (torch.Tensor): The current joint positions of the joints in the group. + dof_vel (torch.Tensor): The current joint velocities of the joints in the group. + + Raises: + ValueError: When the group actions has the wrong shape. Expected shape: (num_articulation, control_dim). + ValueError: Invalid command type. Valid: 'p_abs', 'p_rel', 'v_abs', 'v_rel', 't_abs'. + ValueError: Invalid actuator model type in group. Expected: 'explicit' or 'implicit'. + + Returns: + ArticulationActions: An instance of the articulation actions. + a. for explicit actuator models, it returns the computed joint torques (after clipping) + b. for implicit actuator models, it returns the actions with the processed desired joint + positions, velocities, and efforts (based on the command types) + """ + # check that group actions has the right dimension + control_shape = (self.num_articulation, self.control_dim) + if tuple(group_actions.shape) != control_shape: + raise ValueError( + f"Invalid actuator group actions shape '{group_actions.shape}'. Expected: '{control_shape}'." + ) + # store current dof state + self._dof_pos[:] = dof_pos + self._dof_vel[:] = dof_vel + # buffers for applying actions. + group_des_dof_pos = None + group_des_dof_vel = None + group_des_dof_torque = None + # pre-processing of commands based on group. + group_actions = self._format_command(group_actions) + # reshape actions for sub-groups + group_actions = group_actions.view(self.num_articulation, self.num_actuators, -1) + # pre-process relative commands + for index, command_type in enumerate(self.command_types): + if command_type == "p_rel": + group_des_dof_pos = self.dof_pos_scale * group_actions[..., index] + self._dof_pos + elif command_type == "p_abs": + group_des_dof_pos = self.dof_pos_scale * group_actions[..., index] + self.dof_pos_offset + elif command_type == "v_rel": + group_des_dof_vel = self.dof_vel_scale * group_actions[..., index] + self._dof_vel + elif command_type == "v_abs": + group_des_dof_vel = self.dof_vel_scale * group_actions[..., index] # offset = 0 + elif command_type == "t_abs": + group_des_dof_torque = self.dof_torque_scale * group_actions[..., index] # offset = 0 + else: + raise ValueError(f"Invalid action command type for actuators: '{command_type}'.") + + # process commands based on actuators + if self.model_type == "explicit": + # assert that model is created + assert self.model is not None, "Actuator model is not created." + # update state and command + self.model.set_command(dof_pos=group_des_dof_pos, dof_vel=group_des_dof_vel, torque_ff=group_des_dof_torque) + # compute torques + self._computed_torques = self.model.compute_torque(dof_pos=self._dof_pos, dof_vel=self._dof_vel) + self._applied_torques = self.model.clip_torques( + self._computed_torques, dof_pos=self._dof_pos, dof_vel=self._dof_vel + ) + # store updated quantities + self._gear_ratio[:] = self.model.gear_ratio + # wrap into isaac-sim command + control_action = ArticulationActions(joint_efforts=self._applied_torques, joint_indices=self.dof_indices) + elif self.model_type == "implicit": + # wrap into isaac-sim command + control_action = ArticulationActions( + joint_positions=group_des_dof_pos, + joint_velocities=group_des_dof_vel, + joint_efforts=group_des_dof_torque, + joint_indices=self.dof_indices, + ) + else: + raise ValueError( + f"Invalid articulation model type. Received: '{self.model_type}'. Expected: 'explicit' or 'implicit'." + ) + # return the computed actions + return control_action + + """ + Implementation specifics. + """ + + def _format_command(self, command: torch.Tensor) -> torch.Tensor: + """Formatting of commands given to the group. + + In the default actuator group, an identity mapping is performed between the input + commands and the joint commands. + + Returns: + torch.Tensor: Desired commands for the DOFs in the group. + Shape is ``(num_articulations, num_actuators * len(command_types))``. + """ + return command + + """ + Helper functions. + """ + + def _process_action_transforms_cfg(self): + """Resolve the action scaling and offsets for different command types.""" + # default values + # -- scale + self.dof_pos_scale = torch.ones(self.num_actuators, device=self.device) + self.dof_vel_scale = torch.ones(self.num_actuators, device=self.device) + self.dof_torque_scale = torch.ones(self.num_actuators, device=self.device) + # -- offset + self.dof_pos_offset = torch.zeros(self.num_actuators, device=self.device) + # parse configuration + for index, dof_name in enumerate(self.dof_names): + # dof pos scale + if self.cfg.control_cfg.dof_pos_scale is not None: + for re_key, value in self.cfg.control_cfg.dof_pos_scale.items(): + if re.fullmatch(re_key, dof_name): + self.dof_pos_scale[index] = value + # dof vel scale + if self.cfg.control_cfg.dof_vel_scale is not None: + for re_key, value in self.cfg.control_cfg.dof_vel_scale.items(): + if re.fullmatch(re_key, dof_name): + self.dof_vel_scale[index] = value + # dof torque scale + if self.cfg.control_cfg.dof_torque_scale is not None: + for re_key, value in self.cfg.control_cfg.dof_torque_scale.items(): + if re.fullmatch(re_key, dof_name): + self.dof_torque_scale[index] = value + # dof pos offset + if self.cfg.control_cfg.dof_pos_offset is not None: + for re_key, value in self.cfg.control_cfg.dof_pos_offset.items(): + if re.fullmatch(re_key, dof_name): + self.dof_pos_offset[index] = value + + def _process_actuator_gains_cfg(self): + """Resolve the PD gains for the actuators and set them into actuator model. + + If the actuator model is implicit, then the gains are applied into the simulator. + If the actuator model is explicit, then the gains are applied into the actuator model. + """ + # get the default values from simulator/USD + stiffness, damping = self.view.get_gains(joint_indices=self.dof_indices, clone=False) + # parse configuration + for index, dof_name in enumerate(self.dof_names): + # stiffness + if self.cfg.control_cfg.stiffness is not None: + for re_key, value in self.cfg.control_cfg.stiffness.items(): + if re.fullmatch(re_key, dof_name): + if value is not None: + stiffness[:, index] = value + # damping + if self.cfg.control_cfg.damping is not None: + for re_key, value in self.cfg.control_cfg.damping.items(): + if re.fullmatch(re_key, dof_name): + if value is not None: + damping[:, index] = value + # set values into model + if self.model_type == "explicit": + # assert that model is created + assert self.model is not None, "Actuator model is not created." + # set values into the explicit models + self.model.set_command(p_gains=stiffness, d_gains=damping) + elif self.model_type == "implicit": + # set gains into simulation (implicit) + self.view.set_gains(kps=stiffness, kds=damping, joint_indices=self.dof_indices) + else: + raise ValueError( + f"Invalid articulation model type. Received: '{self.model_type}'. Expected: 'explicit' or 'implicit'." + ) + + def _process_actuator_torque_limit_cfg(self): + """Process the torque limit of the actuators and set them into simulation. + + If the actuator model is implicit, then the torque limits are applied into the simulator based on the parsed + model configuration. If the actuator model is explicit, the torque limits are set to high values since the + torque range is handled by the saturation model. + """ + # get the default values from simulator/USD + torque_limit = self.view.get_max_efforts(joint_indices=self.dof_indices, clone=False) + # parse configuration + if self.model_type == "explicit": + # for explicit actuators, clipping is handled by the forward model. + # so we set this high for PhysX to not do anything. + torque_limit[:] = 1.0e9 + elif self.model_type == "implicit": + # for implicit actuators, we let PhysX handle the actuator limits. + if isinstance(self.cfg.model_cfg, ImplicitActuatorCfg): + if self.cfg.model_cfg.torque_limit is not None: + torque_limit[:] = self.cfg.model_cfg.torque_limit + else: + raise ValueError( + f"Invalid articulation model type. Received: '{self.model_type}'. Expected: 'explicit' or 'implicit'." + ) + # set values into simulator + self.view.set_max_efforts(torque_limit, joint_indices=self.dof_indices) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_group_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_group_cfg.py new file mode 100644 index 0000000000..2267cf1d7b --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_group_cfg.py @@ -0,0 +1,76 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This submodule contains the configuration for the ActuatorGroup classes. + +Currently, the following configuration classes are supported: + +* ActuatorGroupCfg: standard actuator group with joint-level controllers +* GripperActuatorGroupCfg: actuator group for grippers (mimics transmission across joints) +* NonHolonomicKinematicsGroupCfg: actuator group for a non-holonomic kinematic constraint + +""" + + +from dataclasses import MISSING +from typing import ClassVar, Dict, List + +from omni.isaac.orbit.actuators.model.actuator_cfg import BaseActuatorCfg +from omni.isaac.orbit.utils import configclass + +from .actuator_control_cfg import ActuatorControlCfg + + +@configclass +class ActuatorGroupCfg: + """Configuration for default group of actuators in an articulation.""" + + cls_name: ClassVar[str] = "ActuatorGroup" + """Name of the associated actuator group class. Used to construct the actuator group.""" + + dof_names: List[str] = MISSING + """Articulation's DOF names that are part of the group.""" + + model_cfg: BaseActuatorCfg = MISSING + """Actuator model configuration used by the group.""" + + control_cfg: ActuatorControlCfg = MISSING + """Actuator control configuration used by the group.""" + + +@configclass +class GripperActuatorGroupCfg(ActuatorGroupCfg): + """Configuration for mimicking actuators in a gripper.""" + + cls_name: ClassVar[str] = "GripperActuatorGroup" + + speed: float = MISSING + """The speed at which gripper should close. (used with velocity command type.)""" + + mimic_multiplier: Dict[str, float] = MISSING + """ + Mapping of command from DOF names to command axis [-1, 1]. + + The first joint in the dictionary is considered the joint to mimic. + + For convention purposes: + + - :obj:`+1` -> opening direction + - :obj:`-1` -> closing direction + """ + + open_dof_pos: float = MISSING + """The DOF position at *open* configuration. (used with position command type.)""" + + close_dof_pos: float = MISSING + """The DOF position at *close* configuration. (used with position command type.)""" + + +@configclass +class NonHolonomicKinematicsGroupCfg(ActuatorGroupCfg): + """Configuration for formulating non-holonomic base constraint.""" + + cls_name: ClassVar[str] = "NonHolonomicKinematicsGroup" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/gripper_group.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/gripper_group.py new file mode 100644 index 0000000000..22426bd473 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/gripper_group.py @@ -0,0 +1,141 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import re +import torch +from typing import Sequence + +from omni.isaac.core.articulations import ArticulationView + +from .actuator_group import ActuatorGroup +from .actuator_group_cfg import GripperActuatorGroupCfg + + +class GripperActuatorGroup(ActuatorGroup): + """ + A mimicking actuator group to format a binary open/close command into the joint commands. + + The input actions are processed as a scalar which sign determines whethter to open or close the gripper. + We consider the following convention: + + 1. Positive value (> 0): open command + 2. Negative value (< 0): close command + + The mimicking actuator group has only two valid command types: absolute positions (``"p_abs"``) or absolute + velocity (``"v_abs"``). Based on the chosen command type, the joint commands are computed by multiplying the + reference command with the mimicking multipler. + + * **position mode:** The reference command is resolved as the joint position target for opening or closing the + gripper. These targets are read from the :class:`GripperActuatorGroupCfg` class. + * **velocity mode:** The reference command is resolved as the joint velocity target based on the configured speed + of the gripper. The reference commands are added to the previous command and clipped to the range of (-1.0, 1.0). + + Tip: + In general, we recommend using the velocity mode, as it simulates the delay in opening or closing the gripper. + + """ + + cfg: GripperActuatorGroupCfg + """The configuration of the actuator group.""" + + def __init__(self, cfg: GripperActuatorGroupCfg, view: ArticulationView): + """Initialize the actuator group. + + Args: + cfg (GripperActuatorGroupCfg): The configuration of the actuator group. + view (ArticulationView): The simulation articulation view. + + Raises: + ValueError: When command type is not "p_abs" or "v_abs". + RuntimeError: When the articulation view is not initialized. + ValueError: When not able to find a match for all DOF names in the configuration. + ValueError: When the actuator model configuration is invalid, i.e. not "explicit" or "implicit". + """ + # check valid config + if cfg.control_cfg.command_types[0] not in ["p_abs", "v_abs"] or len(cfg.control_cfg.command_types) > 1: + raise ValueError(f"Gripper mimicking does not support command types: '{cfg.control_cfg.command_types}'.") + # initialize parent + super().__init__(cfg, view) + + # --from actuator dof names + self._mimic_multiplier = torch.ones(self.num_articulation, self.num_actuators, device=self.device) + for index, dof_name in enumerate(self.dof_names): + # multiplier command + for re_key, value in self.cfg.mimic_multiplier.items(): + if re.fullmatch(re_key, dof_name): + self._mimic_multiplier[:, index] = value + # -- dof positions + self._open_dof_pos = self._mimic_multiplier * self.cfg.open_dof_pos + self._close_dof_pos = self._mimic_multiplier * self.cfg.close_dof_pos + + # create buffers + self._previous_dof_targets = torch.zeros(self.num_articulation, self.num_actuators, device=self.device) + + def __str__(self) -> str: + msg = super().__str__() + "\n" + msg = msg.replace("ActuatorGroup", "GripperActuatorGroup") + msg += ( + f"\tNumber of DOFs: {self.num_actuators}\n" + f"\tMimic multiply: {self._mimic_multiplier}\n" + f"\tOpen position : {self.cfg.open_dof_pos}\n" + f"\tClose position: {self.cfg.close_dof_pos}" + ) + return msg + + """ + Properties + """ + + @property + def control_dim(self) -> int: + """Dimension of control actions.""" + return 1 + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int]): + # reset super + super().reset(env_ids=env_ids) + # buffers + self._previous_dof_targets[env_ids] = 0.0 + + def _format_command(self, command: torch.Tensor) -> torch.Tensor: + """Pre-processing of the commands given to actuators. + + We consider the following convention: + + * Positive command -> open grippers + * Negative command -> close grippers + + Returns: + torch.Tensor: The target joint commands for the gripper. + """ + # FIXME: mimic joint positions -- Gazebo plugin seems to do this. + # The following is commented out because Isaac Sim doesn't support setting joint positions + # of particular dof indices properly. It sets joint positions and joint position targets for + # the whole robot, i.e. all previous position targets is lost. + # mimic_dof_pos = self._dof_pos[:, 0] * self._mimic_multiplier + # self.view.set_joint_positions(mimic_dof_pos, joint_indices=self.dof_indices) + + # process actions + if self.control_mode == "velocity": + # compute new command + dof_vel_targets = torch.sign(command) * self.cfg.speed * self._mimic_multiplier + dof_vel_targets = self._previous_dof_targets[:] + dof_vel_targets + dof_vel_targets = torch.clamp(dof_vel_targets, -1.0, 1.0) + # store new command + self._previous_dof_targets[:] = dof_vel_targets + # return command + return dof_vel_targets + else: + # compute new command + dof_pos_targets = torch.where(command > 0, self._open_dof_pos, self._close_dof_pos) + dof_pos_targets = dof_pos_targets * self._mimic_multiplier + # store new command + self._previous_dof_targets[:] = dof_pos_targets + # return command + return dof_pos_targets diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/non_holonomic_group.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/non_holonomic_group.py new file mode 100644 index 0000000000..2826d09d77 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/non_holonomic_group.py @@ -0,0 +1,101 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch + +from omni.isaac.core.articulations import ArticulationView + +from omni.isaac.orbit.utils.math import euler_xyz_from_quat + +from .actuator_group import ActuatorGroup +from .actuator_group_cfg import NonHolonomicKinematicsGroupCfg + + +class NonHolonomicKinematicsGroup(ActuatorGroup): + """ + An actuator group that formulates the 2D-base constraint for vehicle kinematics control. + + In simulation, it is easier to consider the mobile base as a floating link controlled by three dummy joints + (prismatic joints along x and y, and revolute joint along z), in comparison to simulating wheels which is at times + is tricky because of friction settings. Thus, this class implements the non-holonomic kinematic constraint for + translating the input velocity command into joint velocity commands. + + A skid-steering base is under-actuated, i.e. the commands are forward velocity :math:`v_{B,x}` and the turning rate + :\\omega_{B,z}: in the base frame. Using the current base orientation, the commands are transformed into dummy + joint velocity targets as: + + .. math:: + + \\dot{q}_{0, des} &= v_{B,x} \\cos(\\theta) \\\\ + \\dot{q}_{1, des} &= v_{B,x} \\sin(\\theta) \\\\ + \\dot{q}_{2, des} &= \\omega_{B,z} + + where :math:`\\theta` is the yaw of the 2-D base. Since the base is simulated as a dummy joint, the yaw is directly + the value of the revolute joint along z, i.e., :math:`q_2 = \\theta`. + + Tip: + For velocity control of the base with dummy mechanism, we recommed setting high damping gains to the joints. + This ensures that the base remains unperturbed from external disturbances, such as an arm mounted on the base. + + """ + + cfg: NonHolonomicKinematicsGroupCfg + """The configuration of the actuator group.""" + + def __init__(self, cfg: NonHolonomicKinematicsGroupCfg, view: ArticulationView): + """Initialize the actuator group. + + Args: + cfg (NonHolonomicKinematicsGroupCfg): The configuration of the actuator group. + view (ArticulationView): The simulation articulation view. + + Raises: + ValueError: When command type is not "v_abs". + RuntimeError: When the articulation view is not initialized. + ValueError: When not able to find a match for all DOF names in the configuration. + ValueError: When the actuator model configuration is invalid, i.e. not "explicit" or "implicit". + """ + # check valid config + if cfg.control_cfg.command_types[0] not in ["v_abs"] or len(cfg.control_cfg.command_types) > 1: + raise ValueError( + f"Non-holonomic kinematics group does not support command types: '{cfg.control_cfg.command_types}'." + ) + # initialize parent + super().__init__(cfg, view) + # check that the encapsulated joints are three + if self.num_actuators != 3: + raise ValueError(f"Non-holonomic kinematics group requires three joints, but got {self.num_actuators}.") + + """ + Properties + """ + + @property + def control_dim(self) -> int: + """Dimension of control actions.""" + return 2 + + """ + Operations. + """ + + def _format_command(self, command: torch.Tensor) -> torch.Tensor: + """Pre-processing of commands given to actuators. + + The input command is the base velocity and turning rate command. + + Returns: + torch.Tensor: The target joint commands for the gripper. + """ + # obtain current heading + quat_w = self.view.get_world_poses(clone=False)[1] + yaw_w = euler_xyz_from_quat(quat_w)[2] + # compute new command + dof_vel_targets = torch.zeros(self.num_articulation, 3, device=self.device) + dof_vel_targets[:, 0] = torch.cos(yaw_w) * command[:, 0] + dof_vel_targets[:, 1] = torch.sin(yaw_w) * command[:, 0] + dof_vel_targets[:, 2] = command[:, 1] + # return command + return dof_vel_targets diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/__init__.py new file mode 100644 index 0000000000..cde0eb5ef6 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/__init__.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This module contains the configuration classes and explicit models for actuators.""" + +from .actuator_cfg import ( + ActuatorNetLSTMCfg, + ActuatorNetMLPCfg, + DCMotorCfg, + IdealActuatorCfg, + ImplicitActuatorCfg, + VariableGearRatioDCMotorCfg, +) +from .actuator_net import ActuatorNetLSTM, ActuatorNetMLP +from .actuator_physics import DCMotor, IdealActuator, VariableGearRatioDCMotor + +__all__ = [ + # implicit + "ImplicitActuatorCfg", + # ideal actuator + "IdealActuatorCfg", + "IdealActuator", + # dc motor + "DCMotorCfg", + "DCMotor", + # variable gear + "VariableGearRatioDCMotorCfg", + "VariableGearRatioDCMotor", + # mlp + "ActuatorNetMLPCfg", + "ActuatorNetMLP", + # lstm + "ActuatorNetLSTMCfg", + "ActuatorNetLSTM", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_cfg.py new file mode 100644 index 0000000000..39bf1f1122 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_cfg.py @@ -0,0 +1,144 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import MISSING +from typing import Callable, ClassVar, Iterable, Optional, Union + +from omni.isaac.orbit.utils import configclass + + +@configclass +class BaseActuatorCfg: + """Base configuration for actuator model. + + Note: + This class is not meant to be instantiated directly, i.e., it should + only by used as a base class for other actuator model configurations. + """ + + cls_name: ClassVar[Optional[str]] = MISSING + """ + Name of the associated actuator class. + + This is used to construct the actuator model. If an "implicit" model, then the class name + is :obj:`None`. Otherwise, it is the name of the actuator model class. + """ + + model_type: ClassVar[str] = MISSING + """Type of actuator model: "explicit" or "implicit".""" + + +""" +Actuator model configurations. +""" + + +@configclass +class ImplicitActuatorCfg(BaseActuatorCfg): + """Configuration for implicit actuator model. + + Note: + There are no classes associated to this model. It is handled directly by the physics + simulation. The provided configuration is used to set the physics simulation parameters. + """ + + cls_name: ClassVar[Optional[str]] = None # implicit actuators are handled directly. + model_type: ClassVar[str] = "implicit" + + torque_limit: Optional[float] = None + """ + Torque saturation (in N-m). Defaults to :obj:`None`. + + This is used by the physics simulation. If :obj:`None`, then default values from USD is used. + """ + + # TODO (@mayank): Check if we can remove this parameter and use the default values? + velocity_limit: Optional[float] = None + """ + Velocity saturation (in rad/s or m/s). Defaults to :obj:`None`. + + This is used by the physics simulation. If :obj:`None`, then default values from USD is used. + + Tip: + Setting this parameter low may result in undesired behaviors. Keep it high in general. + """ + + +@configclass +class IdealActuatorCfg(BaseActuatorCfg): + """Configuration for ideal actuator model.""" + + cls_name: ClassVar[str] = "IdealActuator" + model_type: ClassVar[str] = "explicit" + + motor_torque_limit: float = MISSING + """Effort limit on the motor controlling the actuator (in N-m).""" + + gear_ratio: float = 1.0 + """The gear ratio of the gear box from motor to joint axel. Defaults to 1.0.""" + + +@configclass +class DCMotorCfg(IdealActuatorCfg): + """Configuration for direct control (DC) motor actuator model.""" + + cls_name: ClassVar[str] = "DCMotor" + + peak_motor_torque: float = MISSING + """Peak motor torque of the electric DC motor (in N-m).""" + + motor_velocity_limit: float = MISSING + """Maximum velocity of the motor controlling the actuated joint (in rad/s).""" + + +@configclass +class VariableGearRatioDCMotorCfg(DCMotorCfg): + """Configuration for variable gear-ratio DC motor actuator model.""" + + cls_name: ClassVar[str] = "VariableGearRatioDCMotor" + + # gear ratio is a function of dof positions + gear_ratio: Union[str, Callable[[torch.Tensor], torch.Tensor]] = MISSING + """ + Gear ratio function of the gear box connecting the motor to actuated joint. + + Note: + The gear ratio function takes as input the joint positions. + """ + + +@configclass +class ActuatorNetMLPCfg(DCMotorCfg): + """Configuration for MLP-based actuator model.""" + + cls_name: ClassVar[str] = "ActuatorNetMLP" + + network_file: str = MISSING + """Path to the file containing network weights.""" + + pos_scale: float = MISSING # DOF position input scaling + """Scaling of the joint position errors input to the network.""" + vel_scale: float = MISSING # DOF velocity input scaling + """Scaling of the joint velocities input to the network.""" + torque_scale: float = MISSING # DOF torque output scaling + """Scaling of the joint efforts output from the network.""" + input_idx: Iterable[int] = MISSING # Indices from the actuator history buffer to pass as inputs. + """ + 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 ActuatorNetLSTMCfg(DCMotorCfg): + """Configuration for LSTM-based actuator model.""" + + cls_name: ClassVar[str] = "ActuatorNetLSTM" + + network_file: str = MISSING + """Path to the file containing network weights.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_net.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_net.py new file mode 100644 index 0000000000..e32d26c57f --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_net.py @@ -0,0 +1,160 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Neural network models for actuators. + +Currently, the following models are supported: +* Multi-Layer Perceptron (MLP) +* Long Short-Term Memory (LSTM) + +""" + +import torch +from typing import Sequence + +from omni.isaac.orbit.utils.assets import read_file + +from .actuator_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg +from .actuator_physics import DCMotor + + +class ActuatorNetMLP(DCMotor): + """Actuator model based on multi-layer perceptron and joint history. + + Many times the analytical model is not sufficient to capture the actuator dynamics, the + delay in the actuator response, or the non-linearities in the actuator. In these cases, + a neural network model can be used to approximate the actuator dynamics. This model is + trained using data collected from the physical actuator and maps the joint state and the + desired joint command to the produced torque by the actuator. + + This class implements the learned model as a neural network based on the work from + :cite:t:`hwangbo2019learning`. The class stores the history of the joint positions errors + and velocities which are used to provide input to the neural network. The model is loaded + as a TorchScript. + + Note: + The class only supports desired joint positions commands as inputs in the method: + :meth:`ActuatorNetMLP.set_command`. + + """ + + cfg: ActuatorNetMLPCfg + """The configuration of the actuator model.""" + + def __init__(self, cfg: ActuatorNetMLPCfg, num_actuators: int, num_envs: int, device: str): + """Initializes the actuator net model. + + Args: + cfg (ActuatorNetMLPCfg): The configuration for the actuator model. + num_actuators (int): The number of actuators using the model. + num_envs (int): The number of instances of the articulation. + device (str): The computation device. + """ + super().__init__(cfg, num_actuators, num_envs, device) + # save config locally + self.cfg = cfg + # load the model from JIT file + file_bytes = read_file(self.cfg.network_file) + self.network = torch.jit.load(file_bytes).to(self.device) + + # create buffers for MLP history + history_length = max(self.cfg.input_idx) + 1 + self._dof_pos_error_history = torch.zeros(self.num_envs, history_length, self.num_actuators, device=self.device) + self._dof_vel_history = torch.zeros(self.num_envs, history_length, self.num_actuators, device=self.device) + + def reset(self, env_ids: Sequence[int]): + self._dof_pos_error_history[env_ids] = 0.0 + self._dof_vel_history[env_ids] = 0.0 + + def compute_torque(self, dof_pos: torch.Tensor, dof_vel: torch.Tensor) -> torch.Tensor: + # move history queue by 1 and update top of history + # -- positions + self._dof_pos_error_history = self._dof_pos_error_history.roll(1, 1) + self._dof_pos_error_history[:, 0] = self._des_dof_pos - dof_pos + # -- velocity + self._dof_vel_history = self._dof_vel_history.roll(1, 1) + self._dof_vel_history[:, 0] = dof_vel + + # compute network inputs + # -- positions + pos_input = torch.cat([self._dof_pos_error_history[:, i].unsqueeze(2) for i in self.cfg.input_idx], dim=2) + pos_input = pos_input.reshape(self.num_envs * self.num_actuators, -1) + # -- velocity + vel_input = torch.cat([self._dof_vel_history[:, i].unsqueeze(2) for i in self.cfg.input_idx], dim=2) + vel_input = vel_input.reshape(self.num_envs * self.num_actuators, -1) + # -- scale and concatenate inputs + network_input = torch.cat([vel_input * self.cfg.vel_scale, pos_input * self.cfg.pos_scale], dim=1) + + # run network inference + desired_torques = self.network(network_input).reshape(self.num_envs, self.num_actuators) + desired_torques = self.cfg.torque_scale * desired_torques + # return torques + return desired_torques + + +class ActuatorNetLSTM(DCMotor): + """Actuator model based on recurrent neural network (LSTM). + + Unlike the MLP implementation :cite:t:`hwangbo2019learning`, this class implements + the learned model as a temporal neural network (LSTM) based on the work from + :cite:t:`rudin2022learning`. This removes the need of storing a history as the + hidden states of the recurrent network captures the history. + + Note: + The class only supports desired joint positions commands as inputs in the method: + :meth:`ActuatorNetLSTM.set_command`. + + """ + + cfg: ActuatorNetLSTMCfg + """The configuration of the actuator model.""" + + def __init__(self, cfg: ActuatorNetLSTMCfg, num_actuators: int, num_envs: int, device: str): + """Initializes the actuator net model. + + Args: + cfg (ActuatorNetLSTMCfg): The configuration for the actuator model. + num_actuators (int): The number of actuators using the model. + num_envs (int): The number of instances of the articulation. + device (str): The computation device. + """ + super().__init__(cfg, num_actuators, num_envs, device) + # load the model from JIT file + file_bytes = read_file(self.cfg.network_file) + self.network = torch.jit.load(file_bytes).to(self.device) + + # extract number of lstm layers and hidden dim from the shape of weights + num_layers = len(self.network.lstm.state_dict()) // 4 + hidden_dim = self.network.lstm.state_dict()["weight_hh_l0"].shape[1] + # create buffers for storing LSTM inputs + self.sea_input = torch.zeros(self.num_envs * self.num_actuators, 1, 2, device=self.device) + self.sea_hidden_state = torch.zeros( + num_layers, self.num_envs * self.num_actuators, hidden_dim, device=self.device + ) + self.sea_cell_state = torch.zeros( + num_layers, self.num_envs * self.num_actuators, hidden_dim, device=self.device + ) + # reshape via views (doesn't change the actual memory layout) + layer_shape_per_env = (num_layers, self.num_envs, self.num_actuators, hidden_dim) + self.sea_hidden_state_per_env = self.sea_hidden_state.view(layer_shape_per_env) + self.sea_cell_state_per_env = self.sea_cell_state.view(layer_shape_per_env) + + def reset(self, env_ids: Sequence[int]): + with torch.no_grad(): + self.sea_hidden_state_per_env[:, env_ids] = 0.0 + self.sea_cell_state_per_env[:, env_ids] = 0.0 + + def compute_torque(self, dof_pos: torch.Tensor, dof_vel: torch.Tensor) -> torch.Tensor: + # compute network inputs + self.sea_input[:, 0, 0] = (self._des_dof_pos - dof_pos).flatten() + self.sea_input[:, 0, 1] = dof_vel.flatten() + # run network inference + with torch.inference_mode(): + torques, (self.sea_hidden_state[:], self.sea_cell_state[:]) = self.network( + self.sea_input, (self.sea_hidden_state, self.sea_cell_state) + ) + # return torques + return torques.view(dof_pos.shape) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_physics.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_physics.py new file mode 100644 index 0000000000..a4b119df67 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/model/actuator_physics.py @@ -0,0 +1,316 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Physics-based models for actuators. + +Currently the following models are supported: +* Ideal actuator +* DC motor +* Variable gear ratio DC motor +""" + + +import torch +from typing import Optional, Sequence, Union + +from .actuator_cfg import DCMotorCfg, IdealActuatorCfg, VariableGearRatioDCMotorCfg + + +class IdealActuator: + """Ideal torque-controlled actuator model with a simple saturation model. + + It employs the following model for computing torques for the actuated joint :math:`j`: + + .. math:: + + \\tau_{j, computed} = k_p * (q - q_{des}) + k_d * (\\dot{q} - \\dot{q}_{des}) + \\tau_{ff} + + where, :math:`k_p` and :math:`k_d` are joint stiffness and damping gains, :math:`q` and :math:`\\dot{q}` + are the current joint positions and velocities, :math:`q_{des}`, :math:`\\dot{q}_{des}` and :math:`\tau_{ff}` + are the desired joint positions, velocities and torques commands. + + The clipping model is based on the maximum torque applied by the motor. It is implemented as: + + .. math:: + + \\tau_{j, max} & = \\gamma \\times \\tau_{motor, max} \\\\ + \\tau_{j, applied} & = clip(\\tau_{computed}, -\\tau_{j, max}, \\tau_{j, max}) + + where the clipping function is defined as :math:`clip(x, x_{min}, x_{max}) = min(max(x, x_{min}), x_{max})`. + The parameters :math:`\\gamma` is the gear ratio of the gear box connecting the motor and the actuated joint ends, + and :math:`\\tau_{motor, max}` is the maximum motor effort possible. These parameters are read from + the configuration instance passed to the class. + """ + + cfg: IdealActuatorCfg + """The configuration for the actuator model.""" + num_actuators: int + """The number of actuators using the model.""" + num_envs: int + """The number of instances of the articulation.""" + device: str + """The computation device.""" + + def __init__(self, cfg: IdealActuatorCfg, num_actuators: int, num_envs: int, device: str): + """Initializes the ideal actuator model. + + Args: + cfg (IdealActuatorCfg): The configuration for the actuator model. + num_actuators (int): The number of actuators using the model. + num_envs (int): The number of instances of the articulation. + device (str): The computation device. + """ + # store inputs to class + self.cfg = cfg + self.num_actuators = num_actuators + self.num_envs = num_envs + self.device = device + + # create buffers for allocation + # -- joint commands + self._des_dof_pos = torch.zeros(self.num_envs, self.num_actuators, device=self.device) + self._des_dof_vel = torch.zeros_like(self._des_dof_pos) + # -- PD gains + self._p_gains = torch.zeros_like(self._des_dof_pos) + self._d_gains = torch.zeros_like(self._des_dof_pos) + # -- feed-forward torque + self._torque_ff = torch.zeros_like(self._des_dof_pos) + + """ + Properties + """ + + @property + def gear_ratio(self) -> float: + """Gear-box conversion factor from motor axis to joint axis.""" + return self.cfg.gear_ratio + + """ + Operations- State. + """ + + def set_command( + self, + dof_pos: Optional[Union[torch.Tensor, float]] = None, + dof_vel: Optional[Union[torch.Tensor, float]] = None, + p_gains: Optional[Union[torch.Tensor, float]] = None, + d_gains: Optional[Union[torch.Tensor, float]] = None, + torque_ff: Optional[Union[torch.Tensor, float]] = None, + ): + """Sets the desired joint positions, velocities, gains and feed-forward torques. + + If the values are :obj:`None`, the previous values are retained. + + Args: + dof_pos (Optional[Union[torch.Tensor, float]], optional): The desired joint positions. Defaults to None. + dof_vel (Optional[Union[torch.Tensor, float]], optional): The desired joint velocities. Defaults to None. + p_gains (Optional[Union[torch.Tensor, float]], optional): The stiffness gains of the drive. Defaults to None. + d_gains (Optional[Union[torch.Tensor, float]], optional): The damping gains of the drive. Defaults to None. + torque_ff (Optional[Union[torch.Tensor, float]], optional): The desired joint torque. Defaults to None. + """ + if dof_pos is not None: + self._des_dof_pos[:] = dof_pos + if dof_vel is not None: + self._des_dof_vel[:] = dof_vel + if p_gains is not None: + self._p_gains[:] = p_gains + if d_gains is not None: + self._d_gains[:] = d_gains + if torque_ff is not None: + self._torque_ff[:] = torque_ff + + """ + Operations- Main. + """ + + def reset(self, env_ids: Sequence[int]): + """Resets the internal buffers or state of the actuator model. + + Args: + env_ids (Sequence[int]): The ids to reset. + """ + # reset desired joint positions and velocities + self._des_dof_pos[env_ids] = 0.0 + self._des_dof_vel[env_ids] = 0.0 + # reset feed-forward torque + self._torque_ff[env_ids] = 0.0 + + def compute_torque(self, dof_pos: torch.Tensor, dof_vel: torch.Tensor) -> torch.Tensor: + """Computes the desired joint torques using the input commands and current joint states. + + Args: + dof_pos (torch.Tensor): The joint positions of the actuators. + dof_vel (torch.Tensor): The joint velocities of the actuators. + + Returns: + torch.Tensor: The desired joint torques to achieve the input commands. + """ + # compute errors + dof_pos_error = self._des_dof_pos - dof_pos + dof_vel_error = self._des_dof_vel - dof_vel + # compute torques + desired_torques = self._p_gains * dof_pos_error + self._d_gains * dof_vel_error + self._torque_ff + # return torques + return desired_torques + + def clip_torques(self, desired_torques: torch.Tensor, **kwargs) -> torch.Tensor: + """Clip the desired torques based on the motor limits. + + Args: + desired_torques (torch.Tensor): The desired torques to clip. + + Returns: + torch.Tensor: The clipped torques. + """ + # evaluate parameters from motor axel to dof axel + torque_limit = self.cfg.motor_torque_limit * self.gear_ratio + # saturate torques + return torch.clip(desired_torques, -torque_limit, torque_limit) + + +class DCMotor(IdealActuator): + """ + Direct control (DC) motor actuator model with velocity-based saturation model. + + It uses the same model as the :class:`IdealActuator` for computing the torques from input commands. + However, it implements a saturation model defined by DC motor characteristics. + + A DC motor is a type of electric motor that is powered by direct current electricity. In most cases, + the motor is connected to a constant source of voltage supply, and the current is controlled by a rheostat. + Depending on various design factors such as windings and materials, the motor can draw a limited maximum power + from the electronic source, which limits the produced motor torque and speed. + + A DC motor characteristics are defined by the following parameters: + + * Continuous-rated speed (:math:`\\dot{q}_{motor, max}`) : The maximum-rated speed of the motor. + * Continuous-stall torque (:math:`\\tau_{motor, max}`): The maximum-rated torque produced at 0 speed. + * Peak torque (:math:`\\tau_{motor, peak}`): The maximum torque that can be outputted for a short period. + + Based on these parameters, the instantaneous minimum and maximum torques are defined as follows: + + .. math:: + + \\tau_{j, max}(\\dot{q}) & = clip \\left (\\tau_{j, peak} \\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, peak} \\times \\left( -1 - + \\frac{\\dot{q}}{\\dot{q}_{j, max}}\\right), - \\tau_{j, max}, 0.0 \\right) + + where :math:`\\gamma` is the gear ratio of the gear box connecting the motor and the actuated joint ends, + :math:`\\dot{q}_{j, max} = \\gamma^{-1} \\times \\dot{q}_{motor, max}`, :math:`\\tau_{j, max} = + \\gamma \\times \\tau_{motor, max}` and :math:`\\tau_{j, peak} = \\gamma \\times \\tau_{motor, peak}` + are the maximum joint velocity, maximum joint torque and peak torque, respectively. These parameters + are read from the configuration instance passed to the class. + + Using these values, the computed torques are clipped to the minimum and maximum values based on the + instantaneous joint velocity: + + .. math:: + + \\tau_{j, applied} = clip(\\tau_{computed}, \\tau_{j, min}(\\dot{q}), \\tau_{j, max}(\\dot{q})) + + """ + + cfg: DCMotorCfg + """The configuration for the actuator model.""" + + def __init__(self, cfg: DCMotorCfg, num_actuators: int, num_envs: int, device: str): + """Initializes the DC motor actuator model. + + Args: + cfg (DCMotorCfg): The configuration for the actuator model. + num_actuators (int): The number of actuators using the model. + num_envs (int): The number of instances of the articulation. + device (str): The computation device. + """ + super().__init__(cfg, num_actuators, num_envs, device) + + def clip_torques(self, desired_torques: torch.Tensor, dof_vel: torch.Tensor, **kwargs) -> torch.Tensor: + """Clip the desired torques based on the motor limits. + + Args: + desired_torques (torch.Tensor): The desired torques to clip. + dof_vel (torch.Tensor): The current joint velocities. + + Returns: + torch.Tensor: The clipped torques. + """ + # evaluate parameters from motor axel to dof axel + peak_torque = self.cfg.peak_motor_torque * self.gear_ratio + torque_limit = self.cfg.motor_torque_limit * self.gear_ratio + velocity_limit = self.cfg.motor_velocity_limit / self.gear_ratio + # compute torque limits + # -- max limit + max_torques = peak_torque * (1.0 - dof_vel / velocity_limit) + max_torques = torch.clip(max_torques, min=0.0, max=torque_limit) + # -- min limit + min_torques = peak_torque * (-1.0 - dof_vel / velocity_limit) + min_torques = torch.clip(min_torques, min=-torque_limit, max=0.0) + # saturate torques + return torch.clip(desired_torques, min_torques, max_torques) + + +class VariableGearRatioDCMotor(DCMotor): + """Torque-controlled actuator with variable gear-ratio based saturation model. + + Instead of using a fixed gear box, some motors are equiped with variators that allow the gear-ratio + to be changed continuously (instead of steps). This model implements a DC motor with a variable + gear ratio function that computes the gear-ratio as a function of the joint position, i.e.: + + .. math:: + + \\gamma = \\gamma(q) + + where :math:`\\gamma(\\cdot)` is read from the configuration instance passed to the class. The gear-ratio function is evaluated at + every time step and the motor parameters are computed accordingly. + + """ + + cfg: VariableGearRatioDCMotorCfg + """The configuration for the actuator model.""" + + def __init__(self, cfg: VariableGearRatioDCMotorCfg, num_actuators: int, num_envs: int, device: str): + """Initializes the variable gear ratio DC actuator model. + + Args: + cfg (VariableGearRatioDCMotorCfg): The configuration for the actuator model. + num_actuators (int): The number of actuators using the model. + num_envs (int): The number of instances of the articulation. + device (str): The computation device. + """ + super().__init__(cfg, num_actuators, num_envs, device) + # parse the configuration + if isinstance(self.cfg.gear_ratio, str): + self._gear_ratio_fn = eval(self.cfg.gear_ratio) + else: + self._gear_ratio_fn = self.cfg.gear_ratio + # check configuration + if not callable(self._gear_ratio_fn): + raise ValueError(f"Expected a callable gear ratio function. Received: {self.cfg.gear_ratio}.") + # create buffers + self._gear_ratio = torch.ones(self.num_envs, self.num_actuators, device=self.device) + + @property + def gear_ratio(self) -> torch.Tensor: + """Gear-box conversion factor from motor axis to joint axis.""" + return self._gear_ratio + + def clip_torques( + self, desired_torques: torch.Tensor, dof_pos: torch.Tensor, dof_vel: torch.Tensor, **kwargs + ) -> torch.Tensor: + """Clip the desired torques based on the motor limits. + + Args: + desired_torques (torch.Tensor): The desired torques to clip. + dof_pos (torch.Tensor): The current joint positions. + dof_vel (torch.Tensor): The current joint velocities. + + Returns: + torch.Tensor: The clipped torques. + """ + # compute gear ratio + self._gear_ratio = self._gear_ratio_fn(dof_pos) + # clip torques using model from parent + super().clip_torques(desired_torques, dof_vel=dof_vel) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/__init__.py new file mode 100644 index 0000000000..140137a7d9 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# enable motion generation extension +from omni.isaac.core.utils.extensions import enable_extension + +# TODO: Maybe keep this extension enabled by default? -- Fix the app experience. +enable_extension("omni.isaac.motion_generation") + + +# get module libraries +from .differential_inverse_kinematics import DifferentialInverseKinematics +from .rmp_flow import RmpFlowController + +__all__ = ["DifferentialInverseKinematics", "RmpFlowController"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/__init__.py new file mode 100644 index 0000000000..de5a6ee413 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/rmp_flow.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/rmp_flow.py new file mode 100644 index 0000000000..17950dbb18 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/rmp_flow.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os + +from omni.isaac.core.utils.extensions import get_extension_path_from_name + +from omni.isaac.orbit.controllers.rmp_flow import RmpFlowControllerCfg + +# Note: RMP-Flow config files for supported robots are stored in the motion_generation extension +_RMP_CONFIG_DIR = os.path.join(get_extension_path_from_name("omni.isaac.motion_generation"), "motion_policy_configs") + + +FRANKA_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "franka_rmpflow_common.yaml"), + urdf_file=os.path.join(_RMP_CONFIG_DIR, "franka", "lula_franka_gen.urdf"), + collision_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "robot_descriptor.yaml"), + frame_name="right_gripper", + evaluations_per_frame=5, +) +"""Configuration of RMPFlow for Franka arm (default from `omni.isaac.motion_generation`).""" + + +UR10_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join(_RMP_CONFIG_DIR, "ur10", "rmpflow", "ur10_rmpflow_config.yaml"), + urdf_file=os.path.join(_RMP_CONFIG_DIR, "ur10", "ur10_robot.urdf"), + collision_file=os.path.join(_RMP_CONFIG_DIR, "ur10", "rmpflow", "ur10_robot_description.yaml"), + frame_name="ee_link", + evaluations_per_frame=5, +) +"""Configuration of RMPFlow for UR10 arm (default from `omni.isaac.motion_generation`).""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/differential_inverse_kinematics.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/differential_inverse_kinematics.py new file mode 100644 index 0000000000..067366d0cb --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/differential_inverse_kinematics.py @@ -0,0 +1,271 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import MISSING +from typing import Dict, Optional, Tuple + +from omni.isaac.orbit.utils import configclass +from omni.isaac.orbit.utils.math import ( + apply_delta_pose, + combine_frame_transforms, + compute_pose_error, + quat_apply, + quat_inv, +) + + +@configclass +class DifferentialInverseKinematicsCfg: + """Configuration for inverse differential kinematics controller.""" + + command_type: str = MISSING + """Type of command: "position_abs", "position_rel", "pose_abs", "pose_rel".""" + + ik_method: str = MISSING + """Method for computing inverse of Jacobian: "pinv", "svd", "trans", "dls".""" + + ik_params: Optional[Dict[str, float]] = None + """Parameters for the inverse-kinematics method. (default: obj:`None`). + + - Moore-Penrose pseudo-inverse ("pinv"): + - "k_val": Scaling of computed delta-dof positions (default: 1.0). + - Adaptive Singular Value Decomposition ("svd"): + - "k_val": Scaling of computed delta-dof positions (default: 1.0). + - "min_singular_value": Single values less than this are suppressed to zero (default: 1e-5). + - Jacobian transpose ("trans"): + - "k_val": Scaling of computed delta-dof positions (default: 1.0). + - Damped Moore-Penrose pseudo-inverse ("dls"): + - "lambda_val": Damping coefficient (default: 0.1). + """ + + position_offset: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position offset from parent body to end-effector frame in parent body frame.""" + rotation_offset: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Rotational offset from parent body to end-effector frame in parent body frame.""" + + position_command_scale: Tuple[float, float, float] = (1.0, 1.0, 1.0) + """Scaling of the position command received. Used only in relative mode.""" + rotation_command_scale: Tuple[float, float, float] = (1.0, 1.0, 1.0) + """Scaling of the rotation command received. Used only in relative mode.""" + + +class DifferentialInverseKinematics: + """Inverse differential kinematics controller. + + This controller uses the Jacobian mapping from joint-space velocities to end-effector velocities + to compute the delta-change in the joint-space that moves the robot closer to a desired end-effector + position. + + To deal with singularity in Jacobian, the following methods are supported for computing inverse of the Jacobian: + - "pinv": Moore-Penrose pseudo-inverse + - "svd": Adaptive singular-value decomposition (SVD) + - "trans": Transpose of matrix + - "dls": Damped version of Moore-Penrose pseudo-inverse (also called Levenberg-Marquardt) + + Note: We use the quaternions in the convention: [w, x, y, z]. + + Reference: + [1] https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf + [2] https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf + """ + + _DEFAULT_IK_PARAMS = { + "pinv": {"k_val": 1.0}, + "svd": {"k_val": 1.0, "min_singular_value": 1e-5}, + "trans": {"k_val": 1.0}, + "dls": {"lambda_val": 0.1}, + } + """Default parameters for different inverse kinematics approaches.""" + + def __init__(self, cfg: DifferentialInverseKinematicsCfg, num_robots: int, device: str): + # store inputs + self.cfg = cfg + self.num_robots = num_robots + self._device = device + # check valid input + if self.cfg.ik_method not in ["pinv", "svd", "trans", "dls"]: + raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}.") + if self.cfg.command_type not in ["position_abs", "position_rel", "pose_abs", "pose_rel"]: + raise ValueError(f"Unsupported inverse-kinematics command: {self.cfg.command_type}.") + + # update parameters for IK-method + self._ik_params = self._DEFAULT_IK_PARAMS[self.cfg.ik_method].copy() + if self.cfg.ik_params is not None: + self._ik_params.update(self.cfg.ik_params) + # end-effector offsets + # -- position + tool_child_link_pos = torch.tensor(self.cfg.position_offset, device=self._device) + self._tool_child_link_pos = tool_child_link_pos.repeat(self.num_robots, 1) + # -- orientation + tool_child_link_rot = torch.tensor(self.cfg.rotation_offset, device=self._device) + self._tool_child_link_rot = tool_child_link_rot.repeat(self.num_robots, 1) + # transform from tool -> parent frame + self._tool_parent_link_rot = quat_inv(self._tool_child_link_rot) + self._tool_parent_link_pos = -quat_apply(self._tool_parent_link_rot, self._tool_child_link_pos) + # scaling of command + self._position_command_scale = torch.diag(torch.tensor(self.cfg.position_command_scale, device=self._device)) + self._rotation_command_scale = torch.diag(torch.tensor(self.cfg.rotation_command_scale, device=self._device)) + + # create buffers + self.desired_ee_pos = torch.zeros(self.num_robots, 3, device=self._device) + self.desired_ee_rot = torch.zeros(self.num_robots, 4, device=self._device) + # -- input command + self._command = torch.zeros(self.num_robots, self.num_actions, device=self._device) + + """ + Properties. + """ + + @property + def num_actions(self) -> int: + """Dimension of the action space of controller.""" + if "position" in self.cfg.command_type: + return 3 + elif self.cfg.command_type == "pose_rel": + return 6 + elif self.cfg.command_type == "pose_abs": + return 7 + else: + raise ValueError(f"Invalid control command: {self.cfg.command_type}.") + + """ + Operations. + """ + + def reset_idx(self, robot_ids: torch.Tensor = None): + """Reset the internals.""" + pass + + def set_command(self, command: torch.Tensor): + """Set target end-effector pose command.""" + # check input size + if command.shape != (self.num_robots, self.num_actions): + raise ValueError( + f"Invalid command shape '{command.shape}'. Expected: '{(self.num_robots, self.num_actions)}'." + ) + # store command + self._command[:] = command + + def compute( + self, + current_ee_pos: torch.Tensor, + current_ee_rot: torch.Tensor, + jacobian: torch.Tensor, + joint_positions: torch.Tensor, + ) -> torch.Tensor: + """Performs inference with the controller. + + Returns: + torch.Tensor: The target joint positions commands. + """ + # compute the desired end-effector pose + if "position_rel" in self.cfg.command_type: + # scale command + self._command @= self._position_command_scale + # compute targets + self.desired_ee_pos = current_ee_pos + self._command + self.desired_ee_rot = current_ee_rot + elif "position_abs" in self.cfg.command_type: + # compute targets + self.desired_ee_pos = self._command + self.desired_ee_rot = current_ee_rot + elif "pose_rel" in self.cfg.command_type: + # scale command + self._command[:, 0:3] @= self._position_command_scale + self._command[:, 3:6] @= self._rotation_command_scale + # compute targets + self.desired_ee_pos, self.desired_ee_rot = apply_delta_pose(current_ee_pos, current_ee_rot, self._command) + elif "pose_abs" in self.cfg.command_type: + # compute targets + self.desired_ee_pos = self._command[:, 0:3] + self.desired_ee_rot = self._command[:, 3:7] + else: + raise ValueError(f"Invalid control command: {self.cfg.command_type}.") + + # transform from ee -> parent + # TODO: Make this optional to reduce overhead? + desired_parent_pos, desired_parent_rot = combine_frame_transforms( + self.desired_ee_pos, self.desired_ee_rot, self._tool_parent_link_pos, self._tool_parent_link_rot + ) + # transform from ee -> parent + # TODO: Make this optional to reduce overhead? + current_parent_pos, current_parent_rot = combine_frame_transforms( + current_ee_pos, current_ee_rot, self._tool_parent_link_pos, self._tool_parent_link_rot + ) + # compute pose error between current and desired + position_error, axis_angle_error = compute_pose_error( + current_parent_pos, current_parent_rot, desired_parent_pos, desired_parent_rot, rot_error_type="axis_angle" + ) + # compute the delta in joint-space + if "position" in self.cfg.command_type: + jacobian_pos = jacobian[:, 0:3] + delta_joint_positions = self._compute_delta_dof_pos(delta_pose=position_error, jacobian=jacobian_pos) + else: + pose_error = torch.cat((position_error, axis_angle_error), dim=1) + delta_joint_positions = self._compute_delta_dof_pos(delta_pose=pose_error, jacobian=jacobian) + # return the desired joint positions + return joint_positions + delta_joint_positions + + """ + Helper functions. + """ + + def _compute_delta_dof_pos(self, delta_pose: torch.Tensor, jacobian: torch.Tensor) -> torch.Tensor: + """Computes the change in dos-position that yields the desired change in pose. + + The method uses the Jacobian mapping from joint-space velocities to end-effector velocities + to compute the delta-change in the joint-space that moves the robot closer to a desired end-effector + position. + + Args: + delta_pose (torch.Tensor): The desired delta pose in shape [N, 3 or 6]. + jacobian (torch.Tensor): The geometric jacobian matrix in shape [N, 3 or 6, num-dof] + + Returns: + torch.Tensor: The desired delta in joint space. + """ + if self.cfg.ik_method == "pinv": # Jacobian pseudo-inverse + # parameters + k_val = self._ik_params["k_val"] + # computation + 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 self.cfg.ik_method == "svd": # adaptive SVD + # parameters + k_val = self._ik_params["k_val"] + min_singular_value = self._ik_params["min_singular_value"] + # computation + # U: 6xd, S: dxd, V: d x num-dof + U, S, Vh = torch.linalg.svd(jacobian) + S_inv = 1.0 / S + S_inv = torch.where(S > min_singular_value, S_inv, torch.zeros_like(S_inv)) + jacobian_pinv = ( + torch.transpose(Vh, dim0=1, dim1=2)[:, :, :6] + @ torch.diag_embed(S_inv) + @ torch.transpose(U, dim0=1, dim1=2) + ) + delta_dof_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + elif self.cfg.ik_method == "trans": # Jacobian transpose + # parameters + k_val = self._ik_params["k_val"] + # computation + 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 self.cfg.ik_method == "dls": # damped least squares + # parameters + lambda_val = self._ik_params["lambda_val"] + # computation + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + lambda_matrix = (lambda_val**2) * torch.eye(n=jacobian.shape[1], device=self._device) + delta_dof_pos = jacobian_T @ torch.inverse(jacobian @ jacobian_T + lambda_matrix) @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + else: + raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}") + + return delta_dof_pos diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/joint_impedance.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/joint_impedance.py new file mode 100644 index 0000000000..7a3b3de87c --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/joint_impedance.py @@ -0,0 +1,199 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import MISSING +from typing import Optional, Sequence, Tuple, Union + +from omni.isaac.orbit.utils import configclass + + +@configclass +class JointImpedanceControllerCfg: + """Configuration for joint impedance regulation controller.""" + + command_type: str = "p_abs" + """Type of command: p_abs (absolute) or p_rel (relative).""" + + dof_pos_offset: Optional[Sequence[float]] = None + """Offset to DOF position command given to controller. (default: :obj:`None`). + + If :obj:`None` then position offsets are set to zero. + """ + + impedance_mode: str = MISSING + """Type of gains: "fixed", "variable", "variable_kp".""" + + inertial_compensation: bool = False + """Whether to perform inertial compensation (inverse dynamics).""" + + gravity_compensation: bool = False + """Whether to perform gravity compensation.""" + + stiffness: Union[float, Sequence[float]] = MISSING + """The positional gain for determining desired torques based on joint position error.""" + + damping_ratio: Optional[Union[float, Sequence[float]]] = None + """The damping ratio is used in-conjunction with positional gain to compute desired torques + based on joint velocity error. + + The following math operation is performed for computing velocity gains: + :math:`d_gains = 2 * sqrt(p_gains) * damping_ratio`. + """ + + stiffness_limits: Tuple[float, float] = (0, 300) + """Minimum and maximum values for positional gains. + + Note: Used only when :obj:`impedance_mode` is "variable" or "variable_kp". + """ + + damping_ratio_limits: Tuple[float, float] = (0, 100) + """Minimum and maximum values for damping ratios used to compute velocity gains. + + Note: Used only when :obj:`impedance_mode` is "variable". + """ + + +class JointImpedanceController: + """Joint impedance regulation control. + + Reference: + [1] https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf + """ + + def __init__(self, cfg: JointImpedanceControllerCfg, num_robots: int, dof_pos_limits: torch.Tensor, device: str): + # check valid inputs + if len(dof_pos_limits.shape) != 3: + raise ValueError(f"Joint position limits has shape '{dof_pos_limits.shape}'. Expected length of shape = 3.") + # store inputs + self.cfg = cfg + self.num_robots = num_robots + self.num_dof = dof_pos_limits.shape[1] # (num_robots, num_dof, 2) + self._device = device + + # create buffers + # -- commands + self._dof_pos_target = torch.zeros(self.num_robots, self.num_dof, device=self._device) + # -- offsets + self._dof_pos_offset = torch.zeros(self.num_robots, self.num_dof, device=self._device) + # -- limits + self._dof_pos_limits = dof_pos_limits + # -- positional gains + self._p_gains = torch.zeros(self.num_robots, self.num_dof, device=self._device) + self._p_gains[:] = torch.tensor(self.cfg.stiffness, device=self._device) + # -- velocity gains + self._d_gains = torch.zeros(self.num_robots, self.num_dof, device=self._device) + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) * torch.tensor(self.cfg.damping_ratio, device=self._device) + # -- position offsets + if self.cfg.dof_pos_offset is not None: + self._dof_pos_offset[:] = torch.tensor(self.cfg.dof_pos_offset, device=self._device) + # -- position gain limits + self._p_gains_limits = torch.zeros_like(self._dof_pos_limits) + self._p_gains_limits[..., 0] = self.cfg.stiffness_limits[0] + self._p_gains_limits[..., 1] = self.cfg.stiffness_limits[1] + # -- damping ratio limits + self._damping_ratio_limits = torch.zeros_like(self._dof_pos_limits) + self._damping_ratio_limits[..., 0] = self.cfg.damping_ratio_limits[0] + self._damping_ratio_limits[..., 1] = self.cfg.damping_ratio_limits[1] + + """ + Properties. + """ + + @property + def num_actions(self) -> int: + """Dimension of the action space of controller.""" + # impedance mode + if self.cfg.impedance_mode == "fixed": + # joint positions + return self.num_dof + elif self.cfg.impedance_mode == "variable_kp": + # joint positions + stiffness + return self.num_dof * 2 + elif self.cfg.impedance_mode == "variable": + # joint positions + stiffness + damping + return self.num_dof * 3 + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + """ + Operations. + """ + + def reset_idx(self, robot_ids: torch.Tensor = None): + """Reset the internals.""" + pass + + def set_command(self, command: torch.Tensor): + """Set target end-effector pose command.""" + # check input size + if command.shape != (self.num_robots, self.num_actions): + raise ValueError( + f"Invalid command shape '{command.shape}'. Expected: '{(self.num_robots, self.num_actions)}'." + ) + # impedance mode + if self.cfg.impedance_mode == "fixed": + # joint positions + self._dof_pos_target[:] = command + elif self.cfg.impedance_mode == "variable_kp": + # split input command + dof_pos_command, stiffness = torch.tensor_split(command, 2, dim=-1) + # format command + stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) + # joint positions + stiffness + self._dof_pos_target[:] = dof_pos_command + self._p_gains[:] = stiffness + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) # critically damped + elif self.cfg.impedance_mode == "variable": + # split input command + dof_pos_command, stiffness, damping_ratio = torch.tensor_split(command, 3, dim=-1) + # format command + stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) + damping_ratio = damping_ratio.clip_(min=self._damping_ratio_limits[0], max=self._damping_ratio_limits[1]) + # joint positions + stiffness + damping + self._dof_pos_target[:] = dof_pos_command + self._p_gains[:] = stiffness + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) * damping_ratio + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + def compute( + self, + dof_pos: torch.Tensor, + dof_vel: torch.Tensor, + mass_matrix: Optional[torch.Tensor] = None, + gravity: Optional[torch.Tensor] = None, + ) -> torch.Tensor: + """Performs inference with the controller. + + Returns: + torch.Tensor: The target joint torques commands. + """ + # resolve the command type + if self.cfg.command_type == "p_abs": + desired_dof_pos = self._dof_pos_target + self._dof_pos_offset + elif self.cfg.command_type == "p_rel": + desired_dof_pos = self._dof_pos_target + dof_pos + else: + raise ValueError(f"Invalid dof position command mode: {self.cfg.command_type}.") + # compute errors + desired_dof_pos = desired_dof_pos.clip_(min=self._dof_pos_limits[..., 0], max=self._dof_pos_limits[..., 1]) + dof_pos_error = desired_dof_pos - dof_pos + dof_vel_error = -dof_vel + # compute acceleration + des_dof_acc = self._p_gains * dof_pos_error + self._d_gains * dof_vel_error + # compute torques + # -- inertial compensation + if self.cfg.inertial_compensation: + # inverse dynamics control + desired_torques = mass_matrix @ des_dof_acc + else: + # decoupled spring-mass control + desired_torques = des_dof_acc + # -- gravity compensation (bias correction) + if self.cfg.gravity_compensation: + desired_torques += gravity + + return desired_torques diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/operational_space.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/operational_space.py new file mode 100644 index 0000000000..beb0d40e7d --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/operational_space.py @@ -0,0 +1,327 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import MISSING +from typing import Optional, Sequence, Tuple, Union + +from omni.isaac.orbit.utils import configclass +from omni.isaac.orbit.utils.math import apply_delta_pose, compute_pose_error + + +@configclass +class OperationSpaceControllerCfg: + """Configuration for operation-space controller.""" + + command_types: Sequence[str] = MISSING + """Type of command. + + It has two sub-strings joined by underscore: + - type of command mode: "position", "pose", "force" + - type of command resolving: "abs" (absolute), "rel" (relative) + """ + + impedance_mode: str = MISSING + """Type of gains for motion control: "fixed", "variable", "variable_kp".""" + + uncouple_motion_wrench: bool = False + """Whether to decouple the wrench computation from task-space pose (motion) error.""" + + motion_control_axes: Sequence[int] = (1, 1, 1, 1, 1, 1) + """Motion direction to control. Mark as 0/1 for each axis.""" + force_control_axes: Sequence[int] = (0, 0, 0, 0, 0, 0) + """Force direction to control. Mark as 0/1 for each axis.""" + + inertial_compensation: bool = False + """Whether to perform inertial compensation for motion control (inverse dynamics).""" + + gravity_compensation: bool = False + """Whether to perform gravity compensation.""" + + stiffness: Union[float, Sequence[float]] = MISSING + """The positional gain for determining wrenches based on task-space pose error.""" + + damping_ratio: Optional[Union[float, Sequence[float]]] = None + """The damping ratio is used in-conjunction with positional gain to compute wrenches + based on task-space velocity error. + + The following math operation is performed for computing velocity gains: + :math:`d_gains = 2 * sqrt(p_gains) * damping_ratio`. + """ + + stiffness_limits: Tuple[float, float] = (0, 300) + """Minimum and maximum values for positional gains. + + Note: Used only when :obj:`impedance_mode` is "variable" or "variable_kp". + """ + + damping_ratio_limits: Tuple[float, float] = (0, 100) + """Minimum and maximum values for damping ratios used to compute velocity gains. + + Note: Used only when :obj:`impedance_mode` is "variable". + """ + + force_stiffness: Union[float, Sequence[float]] = None + """The positional gain for determining wrenches for closed-loop force control. + + If obj:`None`, then open-loop control of desired forces is performed. + """ + + position_command_scale: Tuple[float, float, float] = (1.0, 1.0, 1.0) + """Scaling of the position command received. Used only in relative mode.""" + rotation_command_scale: Tuple[float, float, float] = (1.0, 1.0, 1.0) + """Scaling of the rotation command received. Used only in relative mode.""" + + +class OperationSpaceController: + """Operation-space controller. + + Reference: + [1] https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf + """ + + def __init__(self, cfg: OperationSpaceControllerCfg, num_robots: int, num_dof: int, device: str): + # store inputs + self.cfg = cfg + self.num_robots = num_robots + self.num_dof = num_dof + self._device = device + + # resolve tasks-pace target dimensions + self.target_list = list() + for command_type in self.cfg.command_types: + if "position" in command_type: + self.target_list.append(3) + elif command_type == "pose_rel": + self.target_list.append(6) + elif command_type == "pose_abs": + self.target_list.append(7) + elif command_type == "force_abs": + self.target_list.append(6) + else: + raise ValueError(f"Invalid control command: {command_type}.") + self.target_dim = sum(self.target_list) + + # create buffers + # -- selection matrices + self._selection_matrix_motion = torch.diag( + torch.tensor(self.cfg.motion_control_axes, dtype=torch.float, device=self._device) + ) + self._selection_matrix_force = torch.diag( + torch.tensor(self.cfg.force_control_axes, dtype=torch.float, device=self._device) + ) + # -- commands + self._task_space_target = torch.zeros(self.num_robots, self.target_dim, device=self._device) + # -- scaling of command + self._position_command_scale = torch.diag(torch.tensor(self.cfg.position_command_scale, device=self._device)) + self._rotation_command_scale = torch.diag(torch.tensor(self.cfg.rotation_command_scale, device=self._device)) + # -- motion control gains + self._p_gains = torch.zeros(self.num_robots, 6, device=self._device) + self._p_gains[:] = torch.tensor(self.cfg.stiffness, device=self._device) + self._d_gains = 2 * torch.sqrt(self._p_gains) * torch.tensor(self.cfg.damping_ratio, device=self._device) + # -- force control gains + if self.cfg.force_stiffness is not None: + self._p_wrench_gains = torch.zeros(self.num_robots, 6, device=self._device) + self._p_wrench_gains[:] = torch.tensor(self.cfg.force_stiffness, device=self._device) + else: + self._p_wrench_gains = None + # -- position gain limits + self._p_gains_limits = torch.zeros(self.num_robots, 6, device=self._device) + self._p_gains_limits[..., 0], self._p_gains_limits[..., 1] = ( + self.cfg.stiffness_limits[0], + self.cfg.stiffness_limits[1], + ) + # -- damping ratio limits + self._damping_ratio_limits = torch.zeros_like(self._p_gains_limits) + self._damping_ratio_limits[..., 0], self._damping_ratio_limits[..., 1] = ( + self.cfg.damping_ratio_limits[0], + self.cfg.damping_ratio_limits[1], + ) + # -- storing outputs + self._desired_torques = torch.zeros(self.num_robots, self.num_dof, self.num_dof, device=self._device) + + """ + Properties. + """ + + @property + def num_actions(self) -> int: + """Dimension of the action space of controller.""" + # impedance mode + if self.cfg.impedance_mode == "fixed": + # task-space pose + return self.target_dim + elif self.cfg.impedance_mode == "variable_kp": + # task-space pose + stiffness + return self.target_dim + 6 + elif self.cfg.impedance_mode == "variable": + # task-space pose + stiffness + damping + return self.target_dim + 6 + 6 + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + """ + Operations. + """ + + def reset_idx(self, robot_ids: torch.Tensor = None): + """Reset the internals.""" + pass + + def set_command(self, command: torch.Tensor): + """Set target end-effector pose command.""" + # check input size + if command.shape != (self.num_robots, self.num_actions): + raise ValueError( + f"Invalid command shape '{command.shape}'. Expected: '{(self.num_robots, self.num_actions)}'." + ) + # impedance mode + if self.cfg.impedance_mode == "fixed": + # joint positions + self._task_space_target[:] = command + elif self.cfg.impedance_mode == "variable_kp": + # split input command + task_space_command, stiffness = torch.tensor_split(command, (self.target_dim, 6), dim=-1) + # format command + stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) + # joint positions + stiffness + self._task_space_target[:] = task_space_command.squeeze(dim=-1) + self._p_gains[:] = stiffness + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) # critically damped + elif self.cfg.impedance_mode == "variable": + # split input command + task_space_command, stiffness, damping_ratio = torch.tensor_split(command, 3, dim=-1) + # format command + stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) + damping_ratio = damping_ratio.clip_(min=self._damping_ratio_limits[0], max=self._damping_ratio_limits[1]) + # joint positions + stiffness + damping + self._task_space_target[:] = task_space_command + self._p_gains[:] = stiffness + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) * damping_ratio + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + def compute( + self, + jacobian: torch.Tensor, + ee_pose: Optional[torch.Tensor] = None, + ee_vel: Optional[torch.Tensor] = None, + ee_force: Optional[torch.Tensor] = None, + mass_matrix: Optional[torch.Tensor] = None, + gravity: Optional[torch.Tensor] = None, + ) -> torch.Tensor: + """Performs inference with the controller. + + Returns: + torch.Tensor: The target joint torques commands. + """ + # buffers for motion/force control + desired_ee_pos = None + desired_ee_rot = None + desired_ee_force = None + # resolve the commands + target_groups = torch.split(self._task_space_target, self.target_list) + for command_type, target in zip(self.cfg.command_types, target_groups): + if command_type == "position_rel": + # check input is provided + if ee_pose is None: + raise ValueError("End-effector pose is required for 'position_rel' command.") + # scale command + target @= self._position_command_scale + # compute targets + desired_ee_pos = ee_pose[:, :3] + target + desired_ee_rot = ee_pose[:, 3:] + elif command_type == "position_abs": + # check input is provided + if ee_pose is None: + raise ValueError("End-effector pose is required for 'position_abs' command.") + # compute targets + desired_ee_pos = target + desired_ee_rot = ee_pose[:, 3:] + elif command_type == "pose_rel": + # check input is provided + if ee_pose is None: + raise ValueError("End-effector pose is required for 'pose_rel' command.") + # scale command + target[:, 0:3] @= self._position_command_scale + target[:, 3:6] @= self._rotation_command_scale + # compute targets + desired_ee_pos, desired_ee_rot = apply_delta_pose(ee_pose[:, :3], ee_pose[:, 3:], target) + elif command_type == "pose_abs": + # compute targets + desired_ee_pos = target[:, 0:3] + desired_ee_rot = target[:, 3:7] + elif command_type == "force_abs": + # compute targets + desired_ee_force = target + else: + raise ValueError(f"Invalid control command: {self.cfg.command_type}.") + + # reset desired joint torques + self._desired_torques[:] = 0 + # compute for motion-control + if desired_ee_pos is not None: + # check input is provided + if ee_pose is None or ee_vel is None: + raise ValueError("End-effector pose and velocity are required for motion control.") + # -- end-effector tracking error + pose_error = compute_pose_error( + ee_pose[:, :3], ee_pose[:, 3:], desired_ee_pos, desired_ee_rot, rot_error_type="axis_angle" + ) + velocity_error = -ee_vel # zero target velocity + # -- desired end-effector acceleration (spring damped system) + des_ee_acc = self._p_gains * pose_error + self._d_gains * velocity_error + # -- inertial compensation + if self.cfg.inertial_compensation: + # check input is provided + if mass_matrix is None: + raise ValueError("Mass matrix is required for inertial compensation.") + # compute task-space dynamics quantities + # wrench = (J M^(-1) J^T)^(-1) * \ddot(x_des) + mass_matrix_inv = torch.inverse(mass_matrix) + if self.cfg.uncouple_motion_wrench: + # decoupled-mass matrices + lambda_pos = torch.inverse(jacobian[:, 0:3] @ mass_matrix_inv * jacobian[:, 0:3].T) + lambda_ori = torch.inverse(jacobian[:, 3:6] @ mass_matrix_inv * jacobian[:, 3:6].T) + # desired end-effector wrench (from pseudo-dynamics) + decoupled_force = lambda_pos @ des_ee_acc[:, 0:3] + decoupled_torque = lambda_ori @ des_ee_acc[:, 3:6] + des_motion_wrench = torch.cat(decoupled_force, decoupled_torque) + else: + # coupled dynamics + lambda_full = torch.inverse(jacobian @ mass_matrix_inv * jacobian.T) + # desired end-effector wrench (from pseudo-dynamics) + des_motion_wrench = lambda_full @ des_ee_acc + else: + # task-space impedance control + # wrench = \ddot(x_des) + des_motion_wrench = des_ee_acc + # -- joint-space wrench + self._desired_torques += jacobian.T @ self._selection_matrix_motion @ des_motion_wrench + + # compute for force control + if desired_ee_force is not None: + # -- task-space wrench + if self.cfg.stiffness is not None: + # check input is provided + if ee_force is None: + raise ValueError("End-effector force is required for closed-loop force control.") + # closed-loop control + des_force_wrench = desired_ee_force + self._p_wrench_gains * (desired_ee_force - ee_force) + else: + # open-loop control + des_force_wrench = desired_ee_force + # -- joint-space wrench + self._desired_torques += jacobian.T @ self._selection_matrix_force @ des_force_wrench + + # add gravity compensation (bias correction) + if self.cfg.gravity_compensation: + # check input is provided + if gravity is None: + raise ValueError("Gravity vector is required for gravity compensation.") + # add gravity compensation + self._desired_torques += gravity + + return self._desired_torques diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow.py new file mode 100644 index 0000000000..bbffa79d9b --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow.py @@ -0,0 +1,125 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import MISSING +from typing import Tuple + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.articulations import Articulation +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.motion_generation import ArticulationMotionPolicy +from omni.isaac.motion_generation.lula import RmpFlow + +from omni.isaac.orbit.utils import configclass + + +@configclass +class RmpFlowControllerCfg: + """Configuration for RMP-Flow controller (provided through LULA library).""" + + config_file: str = MISSING + """Path to the configuration file for the controller.""" + urdf_file: str = MISSING + """Path to the URDF model of the robot.""" + collision_file: str = MISSING + """Path to collision model description of the robot.""" + frame_name: str = MISSING + """Name of the robot frame for task space (must be present in the URDF).""" + evaluations_per_frame: int = MISSING + """Number of substeps during Euler integration inside LULA world model.""" + ignore_robot_state_updates: bool = False + """If true, then state of the world model inside controller is rolled out. (default: False).""" + + +class RmpFlowController: + """Wraps around RMP-Flow from IsaacSim for batched environments.""" + + def __init__(self, cfg: RmpFlowControllerCfg, prim_paths_expr: str, device: str): + # store input + self.cfg = cfg + self._device = device + + print(f"[INFO]: Loading controller URDF from: {self.cfg.urdf_file}") + # obtain the simulation time + physics_dt = SimulationContext.instance().get_physics_dt() + # find all prims + self._prim_paths = prim_utils.find_matching_prim_paths(prim_paths_expr) + self.num_robots = len(self._prim_paths) + # create all franka robots references and their controllers + self.articulation_policies = list() + for prim_path in self._prim_paths: + # add robot reference + robot = Articulation(prim_path) + robot.initialize() + # add controller + rmpflow = RmpFlow( + rmpflow_config_path=self.cfg.config_file, + urdf_path=self.cfg.urdf_file, + robot_description_path=self.cfg.collision_file, + end_effector_frame_name=self.cfg.frame_name, + evaluations_per_frame=self.cfg.evaluations_per_frame, + ignore_robot_state_updates=self.cfg.ignore_robot_state_updates, + ) + # wrap rmpflow to connect to the Franka robot articulation + articulation_policy = ArticulationMotionPolicy(robot, rmpflow, physics_dt) + self.articulation_policies.append(articulation_policy) + # get number of active joints + self.active_dof_names = self.articulation_policies[0].get_motion_policy().get_active_joints() + self.num_dof = len(self.active_dof_names) + # create buffers + # -- for storing command + self._command = torch.zeros(self.num_robots, self.num_actions, device=self._device) + # -- for policy output + self.dof_pos_target = torch.zeros((self.num_robots, self.num_dof), device=self._device) + self.dof_vel_target = torch.zeros((self.num_robots, self.num_dof), device=self._device) + + """ + Properties. + """ + + @property + def num_actions(self) -> int: + """Dimension of the action space of controller.""" + return 7 + + """ + Operations. + """ + + def reset_idx(self, robot_ids: torch.Tensor = None): + """Reset the internals.""" + pass + + def set_command(self, command: torch.Tensor): + """Set target end-effector pose command.""" + # store command + self._command[:] = command + + def compute(self) -> Tuple[torch.Tensor, torch.Tensor]: + """Performs inference with the controller. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: The target joint positions and velocity commands. + """ + # convert command to numpy + command = self._command.cpu().numpy() + # compute control actions + for i, policy in enumerate(self.articulation_policies): + # enable type-hinting + policy: ArticulationMotionPolicy + # set rmpflow target to be the current position of the target cube. + policy.get_motion_policy().set_end_effector_target( + target_position=command[i, 0:3], target_orientation=command[i, 3:7] + ) + # apply action on the robot + action = policy.get_next_articulation_action() + # copy actions into buffer + # TODO: Make this more efficient? + for dof_index in range(self.num_dof): + self.dof_pos_target[i, dof_index] = action.joint_positions[dof_index] + self.dof_vel_target[i, dof_index] = action.joint_velocities[dof_index] + + return self.dof_pos_target, self.dof_vel_target diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow_smoothed.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow_smoothed.py new file mode 100644 index 0000000000..a25b585abf --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow_smoothed.py @@ -0,0 +1,121 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import torch +from typing import Tuple + +from omni.isaac.core.articulations import Articulation +from omni.isaac.core.utils.extensions import get_extension_path_from_name +from omni.isaac.core.utils.prims import find_matching_prim_paths +from omni.isaac.motion_generation import ArticulationMotionPolicy +from omni.isaac.motion_generation.lula import RmpFlow + + +class RmpFlowController: + """Wraps around RMP-Flow from IsaacSim for batched environments.""" + + def __init__(self, prim_paths_expr: str, robot_name: str, dt: float, device: str): + # store input + self._device = device + # Load configuration for the controller + # Note: RMP-Flow config files for supported robots are stored in the motion_generation extension + mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation") + # configuration for the robot + if robot_name == "franka": + rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs", "franka") + robot_description_path = os.path.join(rmp_config_dir, "rmpflow", "robot_descriptor.yaml") + robot_urdf_path = os.path.join(rmp_config_dir, "lula_franka_gen.urdf") + rmpflow_config_path = os.path.join(rmp_config_dir, "rmpflow", "franka_rmpflow_common.yaml") + self.arm_dof_num = 7 + self.ee_frame_name = "right_gripper" + elif robot_name == "ur10": + rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs", "ur10") + robot_description_path = os.path.join(rmp_config_dir, "rmpflow", "ur10_robot_description.yaml") + robot_urdf_path = os.path.join(rmp_config_dir, "ur10_robot.urdf") + rmpflow_config_path = os.path.join(rmp_config_dir, "rmpflow", "ur10_rmpflow_config.yaml") + self.arm_dof_num = 6 + self.ee_frame_name = "ee_link" + else: + raise NotImplementedError(f"Input robot has no configuration: {robot_name}") + + print(f"[INFO]: Loading controller URDF from: {robot_urdf_path}") + # find all prims + self._prim_paths = find_matching_prim_paths(prim_paths_expr) + self.num_robots = len(self._prim_paths) + # create all franka robots references and their controllers + self.articulation_policies = list() + for prim_path in self._prim_paths: + # add robot reference + robot = Articulation(prim_path) + robot.initialize() + # add controller + rmpflow = RmpFlow( + robot_description_path=robot_description_path, + urdf_path=robot_urdf_path, + rmpflow_config_path=rmpflow_config_path, + end_effector_frame_name=self.ee_frame_name, + evaluations_per_frame=5, + ) + # wrap rmpflow to connect to the Franka robot articulation + articulation_policy = ArticulationMotionPolicy(robot, rmpflow, dt) + self.articulation_policies.append(articulation_policy) + # create buffers + # -- for storing command + self.desired_ee_pos = None + self.desired_ee_rot = None + # -- for policy output + self.joint_positions = torch.zeros((self.num_robots, self.arm_dof_num), device=self._device) + self.joint_velocities = torch.zeros((self.num_robots, self.arm_dof_num), device=self._device) + # update timer + self._update_timer = torch.zeros(self.num_robots, device=self._device) + + """ + Operations. + """ + + def reset_idx(self, robot_ids: torch.Tensor = None): + """Reset the internals.""" + if robot_ids is None: + robot_ids = ... + self._update_timer[robot_ids] = 0.0 + + def set_command(self, desired_ee_pos: torch.Tensor, desired_ee_rot: torch.Tensor): + """Set target end-effector pose command.""" + # convert pose to numpy + self.desired_ee_pos = desired_ee_pos.cpu().numpy() + self.desired_ee_rot = desired_ee_rot.cpu().numpy() + + def advance(self, dt: float) -> Tuple[torch.Tensor, torch.Tensor]: + """Performs inference with the controller. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: The target joint positions and velocity commands. + """ + # check valid + if self.desired_ee_pos is None or self.desired_ee_rot is None: + raise RuntimeError("Target command has not been set. Please call `set_command()` first.") + # update timer + self._update_timer += dt + # compute control actions + for i, policy in enumerate(self.articulation_policies): + # set rmpflow target to be the current position of the target cube. + policy.get_motion_policy().set_end_effector_target( + target_position=self.desired_ee_pos[i], target_orientation=self.desired_ee_rot[i] + ) + # apply action on the robot + action = policy.get_next_articulation_action() + # copy actions into buffer + # arm-action + for dof_index in range(self.arm_dof_num): + if action.joint_positions[dof_index] is not None: + self.joint_positions[i, dof_index] = torch.tensor( + action.joint_positions[dof_index], device=self._device + ) + if action.joint_velocities[dof_index] is not None: + self.joint_velocities[i, dof_index] = torch.tensor( + action.joint_velocities[dof_index], device=self._device + ) + return self.joint_positions, self.joint_velocities diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/__init__.py new file mode 100644 index 0000000000..9498159120 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/__init__.py @@ -0,0 +1,76 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Module providing interfaces to different teleoperation devices. + +Currently, the module supports two categories of devices: + +* Keyboard: Standard keyboard with WASD and arrow keys. +* Spacemouse: 3D mouse with 6 degrees of freedom. + +All device interfaces inherit from the :class:`DeviceBase` class, which provides a +common interface for all devices. The device interface reads the input data when +the :meth:`advance()` method is called. It also provides the function :meth:`add_callback()` +to add user-defined callback functions to be called when a particular input is pressed from +the peripheral device. + +Example usage showing the keyboard interface: + .. code-block:: python + + from omni.isaac.kit import SimulationApp + + # launch the simulator + simulation_app = SimulationApp({"headless": False}) + + from omni.isaac.core.simulation_context import SimulationContext + + from omni.isaac.orbit.devices import Se3Keyboard + + + def print_cb(): + print("Print callback") + + + def quit_cb(): + print("Quit callback") + simulation_app.close() + + + # Load kit helper + sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01) + + # Create teleoperation interface + teleop_interface = Se3Keyboard(pos_sensitivity=0.1, rot_sensitivity=0.1) + # Add teleoperation callbacks + teleop_interface.add_callback("L", print_cb) + teleop_interface.add_callback("ESCAPE", quit_cb) + + # print instructions + print(teleoperation_interface) + print("Press 'L' to print a message. Press 'ESC' to quit.") + + # Reset interface internals + teleop_interface.reset() + + # Play simulation + sim.reset() + + # Run simulation + while simulation_app.is_running(): + # get keyboard command + delta_pose, gripper_command = teleop_interface.advance() + # step simulation + sim.step() + # check if simulator is stopped + if sim.is_stopped(): + break + +""" + +from .keyboard import Se2Keyboard, Se3Keyboard +from .spacemouse import Se2SpaceMouse, Se3SpaceMouse + +__all__ = ["Se2Keyboard", "Se3Keyboard", "Se2SpaceMouse", "Se3SpaceMouse"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/device_base.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/device_base.py new file mode 100644 index 0000000000..3adf91daf0 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/device_base.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Base class for tele-operation interface. +""" + + +from abc import ABC, abstractmethod +from typing import Any, Callable + + +class DeviceBase(ABC): + """An interface class for teleoperation devices.""" + + def __init__(self): + """Initialize the teleoperation interface.""" + pass + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + pass + + """ + Operations + """ + + @abstractmethod + def reset(self): + """Reset the internals.""" + raise NotImplementedError + + @abstractmethod + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind keyboard. + + Args: + key (str): The keyboard button to check against. + func (Callable): The function to call when key is pressed. + """ + raise NotImplementedError + + @abstractmethod + def advance(self) -> Any: + """Provides the joystick event state. + + Returns: + Any -- The processed output form the joystick. + """ + raise NotImplementedError diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/__init__.py new file mode 100644 index 0000000000..6e23e75fd8 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# 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 + +__all__ = ["Se2Keyboard", "Se3Keyboard"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se2_keyboard.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se2_keyboard.py new file mode 100644 index 0000000000..b0f9142eae --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se2_keyboard.py @@ -0,0 +1,157 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Keyboard controller for SE(2) control. +""" + + +import numpy as np +from typing import Callable + +import carb +import omni + +from ..device_base import DeviceBase + + +class Se2Keyboard(DeviceBase): + """A keyboard controller for sending SE(2) commands as velocity commands. + + This class is designed to provide a keyboard controller for mobile base (such as quadrupeds). + It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + task-space commands. + + The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \\omega_z)`. + + Key bindings: + ====================== ========================= ======================== + Command Key (+ve axis) Key (-ve axis) + ====================== ========================= ======================== + Move along x-axis Numpad 8 / Arrow Up Numpad 2 / Arrow Down + Move along y-axis Numpad 4 / Arrow Right Numpad 6 / Arrow Left + Rotate along z-axis Numpad 7 / X Numpad 9 / Y + ====================== ========================= ======================== + + Reference: + https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.Keyboard + """ + + def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, omega_z_sensitivity: float = 1.0): + """Initialize the keyboard layer. + + Args: + v_x_sensitivity (float): Magnitude of linear velocity along x-direction scaling. Defaults to 0.8. + v_y_sensitivity (float): Magnitude of linear velocity along y-direction scaling. Defaults to 0.4. + omega_z_sensitivity (float): Magnitude of angular velocity along z-direction scaling. Defaults to 1.0. + """ + # store inputs + self.v_x_sensitivity = v_x_sensitivity + self.v_y_sensitivity = v_y_sensitivity + self.omega_z_sensitivity = omega_z_sensitivity + # acquire omniverse interfaces + self._appwindow = omni.appwindow.get_default_app_window() + self._input = carb.input.acquire_input_interface() + self._keyboard = self._appwindow.get_keyboard() + self._keyboard_sub = self._input.subscribe_to_keyboard_events(self._keyboard, self._on_keyboard_event) + # bindings for keyboard to command + self._create_key_bindings() + # command buffers + self._base_command = np.zeros(3) + # dictionary for additional callbacks + self._additional_callbacks = dict() + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Keyboard Controller for SE(2): {self.__class__.__name__}\n" + msg += "\tReset all commands: L\n" + msg += "\tMove forward (along x-axis): Numpad 8 / Arrow Up\n" + msg += "\tMove backward (along x-axis): Numpad 2 / Arrow Down\n" + msg += "\tMove right (along y-axis): Numpad 4 / Arrow Right\n" + msg += "\tMove left (along y-axis): Numpad 6 / Arrow Left\n" + msg += "\tYaw positively (along z-axis): Numpad 7 / X\n" + msg += "\tYaw negatively (along z-axis): Numpad 9 / Y" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._base_command.fill(0.0) + + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind keyboard. + + A list of available keys are present in the + `carb documentation `_. + + The callback function should not take any arguments. + + Args: + key (str): The keyboard button to check against. + func (Callable): The function to call when key is pressed. + """ + self._additional_callbacks[key] = func + + def advance(self) -> np.ndarray: + """Provides the result from keyboard event state. + + Returns: + np.ndarray -- A 3D array containing the linear (x,y) and angular velocity (z). + """ + return self._base_command + + """ + Internal helpers. + """ + + def _on_keyboard_event(self, event, *args, **kwargs): + """Subscriber callback to when kit is updated. + + Reference: + https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.KeyboardInput + """ + # apply the command when pressed + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name == "L": + self.reset() + elif event.input.name in self._INPUT_KEY_MAPPING: + self._base_command += self._INPUT_KEY_MAPPING[event.input.name] + # remove the command when un-pressed + if event.type == carb.input.KeyboardEventType.KEY_RELEASE: + if event.input.name in self._INPUT_KEY_MAPPING: + self._base_command -= self._INPUT_KEY_MAPPING[event.input.name] + # additional callbacks + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name in self._additional_callbacks: + self._additional_callbacks[event.input.name]() + + # since no error, we are fine :) + return True + + def _create_key_bindings(self): + """Creates default key binding.""" + self._INPUT_KEY_MAPPING = { + # forward command + "NUMPAD_8": np.asarray([1.0, 0.0, 0.0]) * self.v_x_sensitivity, + "UP": np.asarray([1.0, 0.0, 0.0]) * self.v_x_sensitivity, + # back command + "NUMPAD_2": np.asarray([-1.0, 0.0, 0.0]) * self.v_x_sensitivity, + "DOWN": np.asarray([-1.0, 0.0, 0.0]) * self.v_x_sensitivity, + # right command + "NUMPAD_4": np.asarray([0.0, 1.0, 0.0]) * self.v_y_sensitivity, + "LEFT": np.asarray([0.0, 1.0, 0.0]) * self.v_y_sensitivity, + # left command + "NUMPAD_6": np.asarray([0.0, -1.0, 0.0]) * self.v_y_sensitivity, + "RIGHT": np.asarray([0.0, -1.0, 0.0]) * self.v_y_sensitivity, + # yaw command (positive) + "NUMPAD_7": np.asarray([0.0, 0.0, 1.0]) * self.omega_z_sensitivity, + "X": np.asarray([0.0, 0.0, 1.0]) * self.omega_z_sensitivity, + # yaw command (negative) + "NUMPAD_9": np.asarray([0.0, 0.0, -1.0]) * self.omega_z_sensitivity, + "Z": np.asarray([0.0, 0.0, -1.0]) * self.omega_z_sensitivity, + } diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se3_keyboard.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se3_keyboard.py new file mode 100644 index 0000000000..c54b4b7fd6 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se3_keyboard.py @@ -0,0 +1,177 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Keyboard controller for SE(3) control. +""" + + +import numpy as np +from scipy.spatial.transform.rotation import Rotation +from typing import Callable, Tuple + +import carb +import omni + +from ..device_base import DeviceBase + + +class Se3Keyboard(DeviceBase): + """A keyboard controller for sending SE(3) commands as delta poses and binary command (open/close). + + This class is designed to provide a keyboard controller for a robotic arm with a gripper. + It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + task-space commands. + + The command comprises of two parts: + + * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. + * gripper: a binary command to open or close the gripper. + + Key bindings: + ============================== ================= ================= + Description Key (+ve axis) Key (-ve axis) + ============================== ================= ================= + Toggle gripper (open/close) K + Move along x-axis W S + Move along y-axis A D + Move along z-axis Q E + Rotate along x-axis Z X + Rotate along y-axis T G + Rotate along z-axis C V + ============================== ================= ================= + + Reference: + https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.Keyboard + """ + + def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): + """Initialize the keyboard layer. + + Args: + pos_sensitivity (float): Magnitude of input position command scaling. Defaults to 0.05. + rot_sensitivity (float): Magnitude of scale input rotation commands scaling. Defaults to 0.5. + """ + # store inputs + self.pos_sensitivity = pos_sensitivity + self.rot_sensitivity = rot_sensitivity + # acquire omniverse interfaces + self._appwindow = omni.appwindow.get_default_app_window() + self._input = carb.input.acquire_input_interface() + self._keyboard = self._appwindow.get_keyboard() + self._keyboard_sub = self._input.subscribe_to_keyboard_events(self._keyboard, self._on_keyboard_event) + # bindings for keyboard to command + self._create_key_bindings() + # command buffers + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + # dictionary for additional callbacks + self._additional_callbacks = dict() + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Keyboard Controller for SE(3): {self.__class__.__name__}\n" + msg += "\tToggle gripper (open/close): K\n" + msg += "\tMove arm along x-axis: W/S\n" + msg += "\tMove arm along y-axis: A/D\n" + msg += "\tMove arm along z-axis: Q/E\n" + msg += "\tRotate arm along x-axis: Z/X\n" + msg += "\tRotate arm along y-axis: T/G\n" + msg += "\tRotate arm along z-axis: C/V" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind keyboard. + + A list of available keys are present in the + `carb documentation `_. + + The callback function should not take any arguments. + + Args: + key (str): The keyboard button to check against. + func (Callable): The function to call when key is pressed. + """ + self._additional_callbacks[key] = func + + def advance(self) -> Tuple[np.ndarray, bool]: + """Provides the result from keyboard event state. + + Returns: + Tuple[np.ndarray, bool] -- A tuple containing the delta pose command and gripper commands. + """ + rot_vec = Rotation.from_euler("XYZ", self._delta_rot).as_rotvec() + # if new command received, reset event flag to False until keyboard updated. + return np.concatenate([self._delta_pos, rot_vec]), self._close_gripper + + """ + Internal helpers. + """ + + def _on_keyboard_event(self, event, *args, **kwargs): + """Subscriber callback to when kit is updated. + + Reference: + https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.KeyboardInput + """ + # apply the command when pressed + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name == "L": + self.reset() + if event.input.name == "K": + self._close_gripper = not self._close_gripper + elif event.input.name in ["W", "S", "A", "D", "Q", "E"]: + self._delta_pos += self._INPUT_KEY_MAPPING[event.input.name] + elif event.input.name in ["Z", "X", "T", "G", "C", "V"]: + self._delta_rot += self._INPUT_KEY_MAPPING[event.input.name] + # remove the command when un-pressed + if event.type == carb.input.KeyboardEventType.KEY_RELEASE: + if event.input.name in ["W", "S", "A", "D", "Q", "E"]: + self._delta_pos -= self._INPUT_KEY_MAPPING[event.input.name] + elif event.input.name in ["Z", "X", "T", "G", "C", "V"]: + self._delta_rot -= self._INPUT_KEY_MAPPING[event.input.name] + # additional callbacks + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name in self._additional_callbacks: + self._additional_callbacks[event.input.name]() + + # since no error, we are fine :) + return True + + def _create_key_bindings(self): + """Creates default key binding.""" + self._INPUT_KEY_MAPPING = { + # toggle: gripper command + "K": True, + # x-axis (forward) + "W": np.asarray([1.0, 0.0, 0.0]) * self.pos_sensitivity, + "S": np.asarray([-1.0, 0.0, 0.0]) * self.pos_sensitivity, + # y-axis (right-left) + "D": np.asarray([0.0, 1.0, 0.0]) * self.pos_sensitivity, + "A": np.asarray([0.0, -1.0, 0.0]) * self.pos_sensitivity, + # z-axis (up-down) + "Q": np.asarray([0.0, 0.0, 1.0]) * self.pos_sensitivity, + "E": np.asarray([0.0, 0.0, -1.0]) * self.pos_sensitivity, + # roll (around x-axis) + "Z": np.asarray([1.0, 0.0, 0.0]) * self.rot_sensitivity, + "X": np.asarray([-1.0, 0.0, 0.0]) * self.rot_sensitivity, + # pitch (around y-axis) + "T": np.asarray([0.0, 1.0, 0.0]) * self.rot_sensitivity, + "G": np.asarray([0.0, -1.0, 0.0]) * self.rot_sensitivity, + # yaw (around z-axis) + "C": np.asarray([0.0, 0.0, 1.0]) * self.rot_sensitivity, + "V": np.asarray([0.0, 0.0, -1.0]) * self.rot_sensitivity, + } diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/__init__.py new file mode 100644 index 0000000000..3912c204e9 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# 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 + +__all__ = ["Se2SpaceMouse", "Se3SpaceMouse"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se2_spacemouse.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se2_spacemouse.py new file mode 100644 index 0000000000..b3f7092d89 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se2_spacemouse.py @@ -0,0 +1,154 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Spacemouse controller for SE(2) control.""" + +import hid +import numpy as np +import threading +import time +from typing import Callable + +from ..device_base import DeviceBase +from .utils import convert_buffer + + +class Se2SpaceMouse(DeviceBase): + """A space-mouse controller for sending SE(2) commands as delta poses. + + This class implements a space-mouse controller to provide commands to mobile base. + It uses the `HID-API`_ which interfaces with USD and Bluetooth HID-class devices across multiple platforms. + + The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \\omega_z)`. + + Note: + The interface finds and uses the first supported device connected to the computer. + + Currently tested for following devices: + + - SpaceMouse Compact: https://3dconnexion.com/de/product/spacemouse-compact/ + + .. _HID-API: https://github.com/libusb/hidapi + + """ + + def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, omega_z_sensitivity: float = 1.0): + """Initialize the spacemouse layer. + + Args: + v_x_sensitivity (float): Magnitude of linear velocity along x-direction scaling. Defaults to 0.8. + v_y_sensitivity (float): Magnitude of linear velocity along y-direction scaling. Defaults to 0.4. + omega_z_sensitivity (float): Magnitude of angular velocity along z-direction scaling. Defaults to 1.0. + """ + # store inputs + self.v_x_sensitivity = v_x_sensitivity + self.v_y_sensitivity = v_y_sensitivity + self.omega_z_sensitivity = omega_z_sensitivity + # acquire device interface + self._device = hid.device() + self._find_device() + # command buffers + self._base_command = np.zeros(3) + # dictionary for additional callbacks + self._additional_callbacks = dict() + # run a thread for listening to device updates + self._thread = threading.Thread(target=self._run_device) + self._thread.daemon = True + self._thread.start() + + def __del__(self): + """Destructor for the class.""" + self._thread.join() + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Spacemouse Controller for SE(2): {self.__class__.__name__}\n" + msg += f"\tManufacturer: {self._device.get_manufacturer_string()}\n" + msg += f"\tProduct: {self._device.get_product_string()}\n" + msg += "----------------------------------------------\n" + msg += "\tRight button: reset command\n" + msg += "\tMove mouse laterally: move base horizontally in x-y plane\n" + msg += "\tTwist mouse about z-axis: yaw base about a corresponding axis" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._base_command.fill(0.0) + + def add_callback(self, key: str, func: Callable): + # check keys supported by callback + if key not in ["L", "R"]: + raise ValueError(f"Only left (L) and right (R) buttons supported. Provided: {key}.") + # TODO: Improve this to allow multiple buttons on same key. + self._additional_callbacks[key] = func + + def advance(self) -> np.ndarray: + """Provides the result from spacemouse event state. + + Returns: + np.ndarray -- A 3D array containing the linear (x,y) and angular velocity (z). + """ + return self._base_command + + """ + Internal helpers. + """ + + def _find_device(self): + """Find the device connected to computer.""" + found = False + # implement a timeout for device search + for _ in range(5): + for device in hid.enumerate(): + if device["product_string"] == "SpaceMouse Compact": + # set found flag + found = True + vendor_id = device["vendor_id"] + product_id = device["product_id"] + # connect to the device + self._device.open(vendor_id, product_id) + # check if device found + if not found: + time.sleep(1.0) + else: + break + # no device found: return false + if not found: + raise OSError("No device found by SpaceMouse. Is the device connected?") + + def _run_device(self): + """Listener thread that keeps pulling new messages.""" + # keep running + while True: + # read the device data + data = self._device.read(13) + if data is not None: + # readings from 6-DoF sensor + if data[0] == 1: + # along y-axis + self._base_command[1] = self.v_y_sensitivity * convert_buffer(data[1], data[2]) + # along x-axis + self._base_command[0] = self.v_x_sensitivity * convert_buffer(data[3], data[4]) + elif data[0] == 2: + # along z-axis + self._base_command[2] = self.omega_z_sensitivity * convert_buffer(data[3], data[4]) + # readings from the side buttons + elif data[0] == 3: + # press left button + if data[1] == 1: + # additional callbacks + if "L" in self._additional_callbacks: + self._additional_callbacks["L"] + # right button is for reset + if data[1] == 2: + # reset layer + self.reset() + # additional callbacks + if "R" in self._additional_callbacks: + self._additional_callbacks["R"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se3_spacemouse.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se3_spacemouse.py new file mode 100644 index 0000000000..be1469c734 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se3_spacemouse.py @@ -0,0 +1,171 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Spacemouse controller for SE(3) control.""" + +import hid +import numpy as np +import threading +import time +from scipy.spatial.transform.rotation import Rotation +from typing import Callable, Tuple + +from ..device_base import DeviceBase +from .utils import convert_buffer + + +class Se3SpaceMouse(DeviceBase): + """A space-mouse controller for sending SE(3) commands as delta poses. + + This class implements a space-mouse controller to provide commands to a robotic arm with a gripper. + It uses the `HID-API`_ which interfaces with USD and Bluetooth HID-class devices across multiple platforms [1]. + + The command comprises of two parts: + + * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. + * gripper: a binary command to open or close the gripper. + + Note: + The interface finds and uses the first supported device connected to the computer. + + Currently tested for following devices: + + - SpaceMouse Compact: https://3dconnexion.com/de/product/spacemouse-compact/ + + .. _HID-API: https://github.com/libusb/hidapi + + """ + + def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): + """Initialize the space-mouse layer. + + Args: + pos_sensitivity (float): Magnitude of input position command scaling. Defaults to 0.4. + rot_sensitivity (float): Magnitude of scale input rotation commands scaling. Defaults to 0.8. + """ + # store inputs + self.pos_sensitivity = pos_sensitivity + self.rot_sensitivity = rot_sensitivity + # acquire device interface + self._device = hid.device() + self._find_device() + # read rotations + self._read_rotation = False + + # command buffers + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + # dictionary for additional callbacks + self._additional_callbacks = dict() + # run a thread for listening to device updates + self._thread = threading.Thread(target=self._run_device) + self._thread.daemon = True + self._thread.start() + + def __del__(self): + """Destructor for the class.""" + self._thread.join() + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Spacemouse Controller for SE(3): {self.__class__.__name__}\n" + msg += f"\tManufacturer: {self._device.get_manufacturer_string()}\n" + msg += f"\tProduct: {self._device.get_product_string()}\n" + msg += "----------------------------------------------\n" + msg += "\tRight button: reset command\n" + msg += "\tLeft button: toggle gripper command (open/close)\n" + msg += "\tMove mouse laterally: move arm horizontally in x-y plane\n" + msg += "\tMove mouse vertically: move arm vertically\n" + msg += "\tTwist mouse about an axis: rotate arm about a corresponding axis" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + + def add_callback(self, key: str, func: Callable): + # check keys supported by callback + if key not in ["L", "R"]: + raise ValueError(f"Only left (L) and right (R) buttons supported. Provided: {key}.") + # TODO: Improve this to allow multiple buttons on same key. + self._additional_callbacks[key] = func + + def advance(self) -> Tuple[np.ndarray, bool]: + """Provides the result from spacemouse event state. + + Returns: + Tuple[np.ndarray, bool] -- A tuple containing the delta pose command and gripper commands. + """ + rot_vec = Rotation.from_euler("XYZ", self._delta_rot).as_rotvec() + # if new command received, reset event flag to False until keyboard updated. + return np.concatenate([self._delta_pos, rot_vec]), self._close_gripper + + """ + Internal helpers. + """ + + def _find_device(self): + """Find the device connected to computer.""" + found = False + # implement a timeout for device search + for _ in range(5): + for device in hid.enumerate(): + if device["product_string"] == "SpaceMouse Compact": + # set found flag + found = True + vendor_id = device["vendor_id"] + product_id = device["product_id"] + # connect to the device + self._device.open(vendor_id, product_id) + # check if device found + if not found: + time.sleep(1.0) + else: + break + # no device found: return false + if not found: + raise OSError("No device found by SpaceMouse. Is the device connected?") + + def _run_device(self): + """Listener thread that keeps pulling new messages.""" + # keep running + while True: + # read the device data + data = self._device.read(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]) + # readings from the side buttons + elif data[0] == 3: + # press left button + if data[1] == 1: + # close gripper + self._close_gripper = not self._close_gripper + # additional callbacks + if "L" in self._additional_callbacks: + self._additional_callbacks["L"] + # right button is for reset + if data[1] == 2: + # reset layer + self.reset() + # additional callbacks + if "R" in self._additional_callbacks: + self._additional_callbacks["R"] + if data[1] == 3: + self._read_rotation = not self._read_rotation diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/utils.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/utils.py new file mode 100644 index 0000000000..941853052c --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/utils.py @@ -0,0 +1,79 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Helper functions for SpaceMouse.""" + +# MIT License +# +# Copyright (c) 2022 Stanford Vision and Learning Lab and UT Robot Perception and Learning Lab +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + + +__all__ = ["convert_buffer"] + + +def convert_buffer(b1, b2): + """Converts raw SpaceMouse readings to commands. + + Args: + b1 (int): 8-bit byte + b2 (int): 8-bit byte + Returns: + float: Scaled value from Space-mouse message + """ + return _scale_to_control(_to_int16(b1, b2)) + + +""" +Private methods. +""" + + +def _to_int16(y1, y2): + """Convert two 8 bit bytes to a signed 16 bit integer. + + Args: + y1 (int): 8-bit byte + y2 (int): 8-bit byte + Returns: + int: 16-bit integer + """ + x = (y1) | (y2 << 8) + if x >= 32768: + x = -(65536 - x) + return x + + +def _scale_to_control(x, axis_scale=350.0, min_v=-1.0, max_v=1.0): + """Normalize raw HID readings to target range. + + Args: + x (int): Raw reading from HID + axis_scale (float): (Inverted) scaling factor for mapping raw input value + min_v (float): Minimum limit after scaling + max_v (float): Maximum limit after scaling + Returns: + float: Clipped, scaled input from HID + """ + x = x / axis_scale + x = min(max(x, min_v), max_v) + return x diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/__init__.py new file mode 100644 index 0000000000..bb390d444b --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/__init__.py @@ -0,0 +1,28 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This submodule provides marker utilities for simplifying creation of UI elements in the GUI. + +Currently, the module provides two classes: + +* :class:`StaticMarker` for creating a group of markers from a single USD file. +* :class:`PointMarker` for creating a group of spheres. + + +.. note:: + + For some simple usecases, it may be sufficient to use the debug drawing utilities from Isaac Sim. + The debug drawing API is available in the `omni.isaac.debug_drawing`_ module. It allows drawing of + points and splines efficiently on the UI. + + .. _omni.isaac.debug_drawing: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/ext_omni_isaac_debug_drawing.html + +""" + +from .point_marker import PointMarker +from .static_marker import StaticMarker + +__all__ = ["StaticMarker", "PointMarker"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/point_marker.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/point_marker.py new file mode 100644 index 0000000000..17a59f6ac1 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/point_marker.py @@ -0,0 +1,183 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""A class to coordinate groups of visual markers (loaded from USD).""" + +import numpy as np +import torch +from typing import List, Optional, Sequence, Union + +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +from pxr import Gf, UsdGeom + + +class PointMarker: + """A class to coordinate groups of visual sphere markers for goal-conditioned tasks. + + This class allows visualization of multiple spheres. These can be used to represent a + goal-conditioned task. For instance, if a robot is performing a task of reaching a target, the + class can be used to display a red sphere when the target is far away and a green sphere when + the target is achieved. Otherwise, the class can be used to display spheres, for example, to + mark contact points. + + The class uses `UsdGeom.PointInstancer`_ for efficient handling of multiple markers in the stage. + It creates two spherical markers of different colors. Based on the indices provided the referenced + marker is activated: + + - :obj:`0` corresponds to unachieved target (red sphere). + - :obj:`1` corresponds to achieved target (green sphere). + + Usage: + To create 24 point target markers of radius 0.2 and show them as achieved targets: + + .. code-block:: python + + from omni.isaac.orbit.utils.markers import PointMarker + + # create a point marker + marker = PointMarker("/World/Visuals/goal", 24, radius=0.2) + + # set position of the marker + marker_positions = np.random.uniform(-1.0, 1.0, (24, 3)) + marker.set_world_poses(marker_positions) + # set status of the marker to show achieved targets + marker.set_status([1] * 24) + + .. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html + + """ + + def __init__(self, prim_path: str, count: int, radius: float = 1.0): + """Initialize the class. + + Args: + prim_path (str): The prim path where the PointInstancer will be created. + count (int): The number of marker duplicates to create. + radius (float, optional): The radius of the sphere. Defaults to 1.0. + + Raises: + ValueError: When a prim already exists at the :obj:`prim_path` and it is not a :class:`UsdGeom.PointInstancer`. + """ + # check inputs + stage = stage_utils.get_current_stage() + # -- prim path + if prim_utils.is_prim_path_valid(prim_path): + prim = prim_utils.get_prim_at_path(prim_path) + if not prim.IsA(UsdGeom.PointInstancer): + raise ValueError(f"The prim at path {prim_path} cannot be parsed as a `PointInstancer` object") + self._instancer_manager = UsdGeom.PointInstancer(prim) + else: + self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + # store inputs + self.prim_path = prim_path + self.count = count + self._radius = radius + + # create manager for handling instancing of frame markers + self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + # create a child prim for the marker + # -- target not achieved + prim = prim_utils.create_prim(f"{prim_path}/target_far", "Sphere", attributes={"radius": self._radius}) + geom = UsdGeom.Sphere(prim) + geom.GetDisplayColorAttr().Set([(1.0, 0.0, 0.0)]) + # -- target achieved + prim = prim_utils.create_prim(f"{prim_path}/target_close", "Sphere", attributes={"radius": self._radius}) + geom = UsdGeom.Sphere(prim) + geom.GetDisplayColorAttr().Set([(0.0, 1.0, 0.0)]) + # -- target invisible + prim = prim_utils.create_prim(f"{prim_path}/target_invisible", "Sphere", attributes={"radius": self._radius}) + geom = UsdGeom.Sphere(prim) + geom.GetDisplayColorAttr().Set([(0.0, 0.0, 1.0)]) + prim_utils.set_prim_visibility(prim, visible=False) + # add child reference to point instancer + relation_manager = self._instancer_manager.GetPrototypesRel() + relation_manager.AddTarget(f"{prim_path}/target_far") # target index: 0 + relation_manager.AddTarget(f"{prim_path}/target_close") # target index: 1 + relation_manager.AddTarget(f"{prim_path}/target_invisible") # target index: 2 + + # buffers for storing data in pixar Gf format + # FUTURE: Make them very far away from the scene? + self._proto_indices = [0] * self.count + self._gf_positions = [Gf.Vec3f() for _ in range(self.count)] + self._gf_orientations = [Gf.Quath() for _ in range(self.count)] + # FUTURE: add option to set scales + + # specify that all initial prims are related to same geometry + self._instancer_manager.GetProtoIndicesAttr().Set(self._proto_indices) + # set initial positions of the targets + self._instancer_manager.GetPositionsAttr().Set(self._gf_positions) + self._instancer_manager.GetOrientationsAttr().Set(self._gf_orientations) + + def set_visibility(self, visible: bool): + """Sets the visibility of the markers. + + The method does this through the USD API. + + Args: + visible (bool): flag to set the visibility. + """ + imageable = UsdGeom.Imageable(self._instancer_manager) + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + def set_world_poses( + self, + positions: Optional[Union[np.ndarray, torch.Tensor]] = None, + orientations: Optional[Union[np.ndarray, torch.Tensor]] = None, + indices: Optional[Sequence[int]] = None, + ): + """Update marker poses in the simulation world frame. + + Args: + positions (Optional[Union[np.ndarray, torch.Tensor]], optional): + Positions in the world frame. Shape: (M, 3). Defaults to :obj:`None`, which means left unchanged. + orientations (Optional[Union[np.ndarray, torch.Tensor]], optional): + Quaternion orientations (w, x, y, z) in the world frame of the prims. Shape: (M, 4). + Defaults to :obj:`None`, which means left unchanged. + indices (Optional[Sequence[int]], optional): Indices to specify which alter poses. + Shape: (M,), where M <= total number of markers. Defaults to :obj:`None` (i.e: all markers). + """ + # resolve inputs + if positions is not None: + positions = positions.tolist() + if orientations is not None: + orientations = orientations.tolist() + if indices is None: + indices = range(self.count) + # change marker locations + for i, marker_index in enumerate(indices): + if positions is not None: + self._gf_positions[marker_index][:] = positions[i] + if orientations is not None: + self._gf_orientations[marker_index].SetReal(orientations[i][0]) + self._gf_orientations[marker_index].SetImaginary(orientations[i][1:]) + # apply to instance manager + self._instancer_manager.GetPositionsAttr().Set(self._gf_positions) + self._instancer_manager.GetOrientationsAttr().Set(self._gf_orientations) + + def set_status(self, status: Union[List[int], np.ndarray, torch.Tensor], indices: Optional[Sequence[int]] = None): + """Updates the marker activated by the instance manager. + + Args: + status (Union[List[int], np.ndarray, torch.Tensor]): Decides which prototype marker to visualize. Shape: (M) + indices (Optional[Sequence[int]], optional): Indices to specify which alter poses. + Shape: (M,), where M <= total number of markers. Defaults to :obj:`None` (i.e: all markers). + """ + # default values + if indices is None: + indices = range(self.count) + # resolve input + if status is not list: + proto_indices = status.tolist() + else: + proto_indices = status + # change marker locations + for i, marker_index in enumerate(indices): + self._proto_indices[marker_index] = int(proto_indices[i]) + # apply to instance manager + self._instancer_manager.GetProtoIndicesAttr().Set(self._proto_indices) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/static_marker.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/static_marker.py new file mode 100644 index 0000000000..0c71f1a4bd --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/static_marker.py @@ -0,0 +1,196 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""A class to coordinate groups of visual markers (loaded from USD).""" + +import numpy as np +import torch +from typing import Optional, Sequence, Tuple, Union + +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +from omni.isaac.core.materials import PreviewSurface +from omni.isaac.core.prims import GeometryPrim +from pxr import Gf, UsdGeom + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR, check_file_path + + +class StaticMarker: + """A class to coordinate groups of visual markers (loaded from USD). + + This class allows visualization of different UI elements in the scene, such as points and frames. + The class uses `UsdGeom.PointInstancer`_ for efficient handling of the element in the stage + via instancing of the marker. + + Usage: + To create 24 default frame markers with a scale of 0.5: + + .. code-block:: python + + from omni.isaac.orbit.utils.markers import StaticMarker + + # create a static marker + marker = StaticMarker("/World/Visuals/frames", 24, scale=(0.5, 0.5, 0.5)) + + # set position of the marker + marker_positions = np.random.uniform(-1.0, 1.0, (24, 3)) + marker.set_world_poses(marker_positions) + + .. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html + + """ + + def __init__( + self, + prim_path: str, + count: int, + usd_path: Optional[str] = None, + scale: Tuple[float, float, float] = (1.0, 1.0, 1.0), + color: Optional[Tuple[float, float, float]] = None, + ): + """Initialize the class. + + When the class is initialized, the :class:`UsdGeom.PointInstancer` is created into the stage + and the marker prim is registered into it. + + Args: + prim_path (str): The prim path where the PointInstancer will be created. + count (int): The number of marker duplicates to create. + usd_path (Optional[str], optional): The USD file path to the marker. Defaults to the USD path + for the RGB frame axis marker. + scale (Tuple[float, float, float], optional): The scale of the marker. Defaults to (1.0, 1.0, 1.0). + color (Optional[Tuple[float, float, float]], optional): The color of the marker. + If provided, it overrides the existing color on all the prims of the marker. + Defaults to None. + + Raises: + ValueError: When a prim already exists at the :obj:`prim_path` and it is not a :class:`UsdGeom.PointInstancer`. + FileNotFoundError: When the USD file at :obj:`usd_path` does not exist. + """ + # resolve default markers in the UI elements + # -- USD path + if usd_path is None: + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd" + else: + if not check_file_path(usd_path): + raise FileNotFoundError(f"USD file for the marker not found at: {usd_path}") + # -- prim path + stage = stage_utils.get_current_stage() + if prim_utils.is_prim_path_valid(prim_path): + # retrieve prim if it exists + prim = prim_utils.get_prim_at_path(prim_path) + if not prim.IsA(UsdGeom.PointInstancer): + raise ValueError(f"The prim at path {prim_path} cannot be parsed as a `PointInstancer` object") + self._instancer_manager = UsdGeom.PointInstancer(prim) + else: + # create a new prim + self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + # store inputs + self.prim_path = prim_path + self.count = count + self._usd_path = usd_path + + # create manager for handling instancing of frame markers + self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + # create a child prim for the marker + prim_utils.create_prim(f"{prim_path}/marker", usd_path=self._usd_path) + # disable any physics on the marker + # FIXME: Also support disabling rigid body properties on the marker. + # Currently, it is not possible on GPU pipeline. + # kit_utils.set_nested_rigid_body_properties(f"{prim_path}/marker", rigid_body_enabled=False) + kit_utils.set_nested_collision_properties(f"{prim_path}/marker", collision_enabled=False) + # apply material to marker + if color is not None: + prim = GeometryPrim(f"{prim_path}/marker") + material = PreviewSurface(f"{prim_path}/markerColor", color=np.asarray(color)) + prim.apply_visual_material(material, weaker_than_descendants=False) + + # add child reference to point instancer + # FUTURE: Add support for multiple markers in the same instance manager? + relation_manager = self._instancer_manager.GetPrototypesRel() + relation_manager.AddTarget(f"{prim_path}/marker") # target index: 0 + + # buffers for storing data in pixar Gf format + # FUTURE: Make them very far away from the scene? + self._gf_positions = [Gf.Vec3f() for _ in range(self.count)] + self._gf_orientations = [Gf.Quath() for _ in range(self.count)] + self._gf_scales = [Gf.Vec3f(*tuple(scale)) for _ in range(self.count)] + + # specify that all vis prims are related to same geometry + self._instancer_manager.GetProtoIndicesAttr().Set([0] * self.count) + # set initial positions of the targets + self._instancer_manager.GetScalesAttr().Set(self._gf_scales) + self._instancer_manager.GetPositionsAttr().Set(self._gf_positions) + self._instancer_manager.GetOrientationsAttr().Set(self._gf_orientations) + + def set_visibility(self, visible: bool): + """Sets the visibility of the markers. + + The method does this through the USD API. + + Args: + visible (bool): flag to set the visibility. + """ + imageable = UsdGeom.Imageable(self._instancer_manager) + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + def set_world_poses( + self, + positions: Optional[Union[np.ndarray, torch.Tensor]] = None, + orientations: Optional[Union[np.ndarray, torch.Tensor]] = None, + indices: Optional[Sequence[int]] = None, + ): + """Update marker poses in the simulation world frame. + + Args: + positions (Optional[Union[np.ndarray, torch.Tensor]], optional): + Positions in the world frame. Shape: (M, 3). Defaults to :obj:`None`, which means left unchanged. + orientations (Optional[Union[np.ndarray, torch.Tensor]], optional): + Quaternion orientations (w, x, y, z) in the world frame of the prims. Shape: (M, 4). + Defaults to :obj:`None`, which means left unchanged. + indices (Optional[Sequence[int]], optional): Indices to specify which alter poses. + Shape: (M,), where M <= total number of markers. Defaults to :obj:`None` (i.e: all markers). + """ + # resolve inputs + if positions is not None: + positions = positions.tolist() + if orientations is not None: + orientations = orientations.tolist() + if indices is None: + indices = range(self.count) + # change marker locations + for i, marker_index in enumerate(indices): + if positions is not None: + self._gf_positions[marker_index][:] = positions[i] + if orientations is not None: + self._gf_orientations[marker_index].SetReal(orientations[i][0]) + self._gf_orientations[marker_index].SetImaginary(orientations[i][1:]) + # apply to instance manager + self._instancer_manager.GetPositionsAttr().Set(self._gf_positions) + self._instancer_manager.GetOrientationsAttr().Set(self._gf_orientations) + + def set_scales(self, scales: Union[np.ndarray, torch.Tensor], indices: Optional[Sequence[int]] = None): + """Update marker poses in the simulation world frame. + + Args: + scales (Union[np.ndarray, torch.Tensor]): Scale applied before any rotation is applied. Shape: (M, 3). + indices (Optional[Sequence[int]], optional): Indices to specify which alter poses. + Shape: (M,), where M <= total number of markers. Defaults to :obj:`None` (i.e: all markers). + """ + # default arguments + if indices is None: + indices = range(self.count) + # resolve inputs + scales = scales.tolist() + # change marker locations + for i, marker_index in enumerate(indices): + self._gf_scales[marker_index][:] = scales[i] + # apply to instance manager + self._instancer_manager.GetScalesAttr().Set(self._gf_scales) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/__init__.py new file mode 100644 index 0000000000..d7fcb9d008 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/__init__.py @@ -0,0 +1,22 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodule containing all objects abstractions. +""" + +from .articulated import ArticulatedObject, ArticulatedObjectCfg, ArticulatedObjectData +from .rigid import RigidObject, RigidObjectCfg, RigidObjectData + +__all__ = [ + # rigid objects + "RigidObjectCfg", + "RigidObjectData", + "RigidObject", + # articulated objects + "ArticulatedObjectCfg", + "ArticulatedObjectData", + "ArticulatedObject", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/__init__.py new file mode 100644 index 0000000000..d3d4b2d14f --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodule for handling articulated objects. +""" + +from .articulated_object import ArticulatedObject +from .articulated_object_cfg import ArticulatedObjectCfg +from .articulated_object_data import ArticulatedObjectData + +__all__ = ["ArticulatedObjectCfg", "ArticulatedObject", "ArticulatedObjectData"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py new file mode 100644 index 0000000000..fe045bc54b --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py @@ -0,0 +1,444 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import re +import torch +from typing import Dict, List, Optional, Sequence, Tuple + +import carb +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.articulations import ArticulationView +from omni.isaac.core.prims import RigidPrimView + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.actuators.group import * # noqa: F403, F401 +from omni.isaac.orbit.utils.math import quat_rotate_inverse, sample_uniform, subtract_frame_transforms + +from .articulated_object_cfg import ArticulatedObjectCfg +from .articulated_object_data import ArticulatedObjectData + + +class ArticulatedObject: + """Class for handling articulated objects. + + This class wraps around :class:`ArticulationView` and :class:`RigidPrimView` classes + from Isaac Sim to support the following: + + * Configuring the object using a single dataclass (struct). + * Applying settings to the articulated object from the configuration class. + * Handling different rigid body views inside the articulated object. + * Storing data related to the articulated object. + + """ + + cfg: ArticulatedObjectCfg + """Configuration class for the articulated object.""" + articulations: ArticulationView = None + """Articulation view for the articulated object.""" + site_bodies: Dict[str, RigidPrimView] = None + """Rigid body view for sites in the articulated object. + + Dictionary with keys as the site names and values as the corresponding rigid body view + in the articulated object. + """ + + def __init__(self, cfg: ArticulatedObjectCfg): + """Initialize the articulated object. + + Args: + cfg (ArticulatedObjectCfg): An instance of the configuration class. + """ + # store inputs + self.cfg = cfg + # container for data access + self._data = ArticulatedObjectData() + # buffer variables (filled during spawn and initialize) + self._spawn_prim_path: str = None + + """ + Properties + """ + + @property + def count(self) -> int: + """Number of prims encapsulated.""" + return self.articulations.count + + @property + def device(self) -> str: + """Memory device for computation.""" + return self.articulations._device + + @property + def body_names(self) -> List[str]: + """Ordered names of links/bodies in articulation.""" + return self.articulations.body_names + + @property + def dof_names(self) -> List[str]: + """Ordered names of DOFs in articulation.""" + return self.articulations.dof_names + + @property + def num_dof(self) -> int: + """Total number of DOFs in articulation.""" + return self.articulations.num_dof + + @property + def data(self) -> ArticulatedObjectData: + """Data related to articulation.""" + return self._data + + """ + Operations. + """ + + def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): + """Spawn an articulated object into the stage (loaded from its USD file). + + Note: + If inputs `translation` or `orientation` are not :obj:`None`, then they override the initial root + state specified through the configuration class at spawning. + + Args: + prim_path (str): The prim path for spawning object at. + translation (Sequence[float], optional): The local position of prim from its parent. Defaults to None. + orientation (Sequence[float], optional): The local rotation (as quaternion `(w, x, y, z)` + of the prim from its parent. Defaults to None. + """ + # use default arguments + if translation is None: + translation = self.cfg.init_state.pos + if orientation is None: + orientation = self.cfg.init_state.rot + scale = self.cfg.meta_info.scale + + # -- save prim path for later + self._spawn_prim_path = prim_path + # -- spawn asset if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + # add prim as reference to stage + prim_utils.create_prim( + self._spawn_prim_path, + usd_path=self.cfg.meta_info.usd_path, + translation=translation, + orientation=orientation, + scale=scale, + ) + else: + carb.log_warn(f"A prim already exists at prim path: '{prim_path}'. Skipping...") + + # TODO: What if prim already exists in the stage and spawn isn't called? + # apply rigid body properties + kit_utils.set_nested_rigid_body_properties( + prim_path, + linear_damping=self.cfg.rigid_props.linear_damping, + angular_damping=self.cfg.rigid_props.angular_damping, + max_linear_velocity=self.cfg.rigid_props.max_linear_velocity, + max_angular_velocity=self.cfg.rigid_props.max_angular_velocity, + max_depenetration_velocity=self.cfg.rigid_props.max_depenetration_velocity, + disable_gravity=self.cfg.rigid_props.disable_gravity, + retain_accelerations=self.cfg.rigid_props.retain_accelerations, + ) + # articulation root settings + kit_utils.set_articulation_properties( + prim_path, + enable_self_collisions=self.cfg.articulation_props.enable_self_collisions, + solver_position_iteration_count=self.cfg.articulation_props.solver_position_iteration_count, + solver_velocity_iteration_count=self.cfg.articulation_props.solver_velocity_iteration_count, + ) + + def initialize(self, prim_paths_expr: Optional[str] = None): + """Initializes the PhysX handles and internal buffers. + + Note: + PhysX handles are only enabled once the simulator starts playing. Hence, this function needs to be + called whenever the simulator "plays" from a "stop" state. + + Args: + prim_paths_expr (Optional[str], optional): The prim path expression for the prims. Defaults to None. + + Raises: + RuntimeError: When input `prim_paths_expr` is :obj:`None`, the method defaults to using the last + prim path set when calling the :meth:`spawn()` function. In case, the object was not spawned + and no valid `prim_paths_expr` is provided, the function throws an error. + """ + # default prim path if not cloned + if prim_paths_expr is None: + if self._is_spawned is not None: + self._prim_paths_expr = self._spawn_prim_path + else: + raise RuntimeError( + "Initialize the articulated object failed! Please provide a valid argument for `prim_paths_expr`." + ) + else: + self._prim_paths_expr = prim_paths_expr + # create handles + # -- articulation views + self.articulations = ArticulationView(prim_paths_expr, reset_xform_properties=False) + self.articulations.initialize() + # set the default state + self.articulations.post_reset() + # set properties over all instances + # -- meta-information + self._process_info_cfg() + # create buffers + self._create_buffers() + # tracked sites + if self.sites_indices is not None: + self.site_bodies: Dict[str, RigidPrimView] = dict() + for name in self.sites_indices: + # create rigid body view to track + site_body = RigidPrimView(prim_paths_expr=f"{prim_paths_expr}/{name}", reset_xform_properties=False) + site_body.initialize() + # add to list + self.site_bodies[name] = site_body + else: + self.site_bodies = None + + def reset_buffers(self, env_ids: Optional[Sequence[int]] = None): + """Resets all internal buffers. + + Args: + env_ids (Optional[Sequence[int]], optional): The indices of the object to reset. + Defaults to None (all instances). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + # reset history + self._previous_dof_vel[env_ids] = 0 + + def update_buffers(self, dt: float): + """Update the internal buffers. + + Args: + dt (float): The amount of time passed from last `update_buffers` call. + + This is used to compute numerical derivatives of quantities such as joint accelerations + which are not provided by the simulator. + """ + # frame states + # -- root frame in world + position_w, quat_w = self.articulations.get_world_poses(indices=self._ALL_INDICES, clone=False) + self._data.root_state_w[:, 0:3] = position_w + self._data.root_state_w[:, 3:7] = quat_w + self._data.root_state_w[:, 7:] = self.articulations.get_velocities(indices=self._ALL_INDICES, clone=False) + # -- tracked sites states + if self.site_bodies is not None: + for index, body in enumerate(self.site_bodies.values()): + # world frame + position_w, quat_w = body.get_world_poses(indices=self._ALL_INDICES, clone=False) + self._data.sites_state_w[:, index, 0:3] = position_w + self._data.sites_state_w[:, index, 3:7] = quat_w + self._data.sites_state_w[:, index, 7:] = body.get_velocities(indices=self._ALL_INDICES, clone=False) + # base frame + position_b, quat_b = subtract_frame_transforms( + self._data.root_state_w[:, 0:3], + self._data.root_state_w[:, 3:7], + self._data.sites_state_w[:, index, 0:3], + self._data.sites_state_w[:, index, 3:7], + ) + self._data.sites_state_b[:, index, 0:3] = position_b + self._data.sites_state_b[:, index, 3:7] = quat_b + self._data.sites_state_b[:, index, 7:10] = quat_rotate_inverse( + self._data.root_quat_w, self._data.sites_state_w[:, index, 7:10] + ) + self._data.sites_state_b[:, index, 10:13] = quat_rotate_inverse( + self._data.root_quat_w, self._data.sites_state_w[:, index, 10:13] + ) + + # dof states + self._data.dof_pos[:] = self.articulations.get_joint_positions(indices=self._ALL_INDICES, clone=False) + self._data.dof_vel[:] = self.articulations.get_joint_velocities(indices=self._ALL_INDICES, clone=False) + self._data.dof_acc[:] = (self._data.dof_vel - self._previous_dof_vel) / dt + # update history buffers + self._previous_dof_vel[:] = self._data.dof_vel[:] + + """ + Operations - State. + """ + + def set_root_state(self, root_states: torch.Tensor, env_ids: Optional[Sequence[int]] = None): + """Sets the root state (pose and velocity) of the actor over selected environment indices. + + Args: + root_states (torch.Tensor): Input root state for the actor, shape: (len(env_ids), 13). + env_ids (Optional[Sequence[int]]): Environment indices. + If :obj:`None`, then all indices are used. + """ + # resolve all indices + if env_ids is None: + env_ids = self._ALL_INDICES + # set into simulation + self.articulations.set_world_poses(root_states[:, 0:3], root_states[:, 3:7], indices=env_ids) + self.articulations.set_velocities(root_states[:, 7:], indices=env_ids) + + # TODO: Move these to reset_buffers call. + # 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] = root_states.clone() + + def get_default_root_state(self, env_ids: Optional[Sequence[int]] = None, clone=True) -> torch.Tensor: + """Returns the default/initial root state of actor. + + Args: + env_ids (Optional[Sequence[int]], optional): Environment indices. + Defaults to None (all environment indices). + clone (bool, optional): Whether to return a copy or not. Defaults to True. + + Returns: + torch.Tensor: The default/initial root state of the actor, shape: (len(env_ids), 13). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + # return copy + if clone: + return torch.clone(self._default_root_states[env_ids]) + else: + return self._default_root_states[env_ids] + + def set_dof_state(self, dof_pos: torch.Tensor, dof_vel: torch.Tensor, env_ids: Optional[Sequence[int]] = None): + """Sets the DOF state (position and velocity) of the actor over selected environment indices. + + Args: + dof_pos (torch.Tensor): Input DOF position for the actor, shape: (len(env_ids), 1). + dof_vel (torch.Tensor): Input DOF velocity for the actor, shape: (len(env_ids), 1). + env_ids (torch.Tensor): Environment indices. + If :obj:`None`, then all indices are used. + """ + # resolve all indices + if env_ids is None: + env_ids = self._ALL_INDICES + # set into simulation + self.articulations.set_joint_positions(dof_pos, indices=env_ids) + self.articulations.set_joint_velocities(dof_vel, indices=env_ids) + + # TODO: Move these to reset_buffers call. + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.dof_pos[env_ids] = dof_pos.clone() + self._data.dof_vel[env_ids] = dof_vel.clone() + self._data.dof_acc[env_ids] = 0.0 + + def get_default_dof_state( + self, env_ids: Optional[Sequence[int]] = None, clone=True + ) -> Tuple[torch.Tensor, torch.Tensor]: + """Returns the default/initial DOF state (position and velocity) of actor. + + Args: + env_ids (Optional[Sequence[int]], optional): Environment indices. + Defaults to None (all environment indices). + clone (bool, optional): Whether to return a copy or not. Defaults to True. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: The default/initial DOF position and velocity of the actor. + Each tensor has shape: (len(env_ids), 1). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + # return copy + if clone: + return torch.clone(self._default_dof_pos[env_ids]), torch.clone(self._default_dof_vel[env_ids]) + else: + return self._default_dof_pos[env_ids], self._default_dof_vel[env_ids] + + def get_random_dof_state( + self, env_ids: Optional[Sequence[int]] = None, lower: float = 0.5, upper: float = 1.5 + ) -> Tuple[torch.Tensor, torch.Tensor]: + """Returns randomly sampled DOF state (position and velocity) of actor. + + Currently, the following sampling is supported: + + - DOF positions: + + - uniform sampling between `(lower, upper)` times the default DOF position. + + - DOF velocities: + + - zero. + + Args: + env_ids (Optional[Sequence[int]], optional): Environment indices. + Defaults to None (all environment indices). + lower (float, optional): Minimum value for uniform sampling. Defaults to 0.5. + upper (float, optional): Maximum value for uniform sampling. Defaults to 1.5. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: The sampled DOF position and velocity of the actor. + Each tensor has shape: (len(env_ids), 1). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + actor_count = self.count + else: + actor_count = len(env_ids) + # sample DOF position + dof_pos = self._default_dof_pos[env_ids] * sample_uniform( + lower, upper, (actor_count, self.num_dof), device=self.device + ) + dof_vel = self._default_dof_vel[env_ids] + # return sampled dof state + return dof_pos, dof_vel + + """ + Internal helper. + """ + + def _process_info_cfg(self) -> None: + """Post processing of configuration parameters.""" + # default state + # -- root state + default_root_state = ( + self.cfg.init_state.pos + + self.cfg.init_state.rot + + self.cfg.init_state.lin_vel + + self.cfg.init_state.ang_vel + ) + self._default_root_states = torch.tensor(default_root_state, device=self.device).repeat(self.count, 1) + # -- dof state + self._default_dof_pos = torch.zeros(self.count, self.num_dof, device=self.device) + self._default_dof_vel = torch.zeros(self.count, self.num_dof, device=self.device) + for index, dof_name in enumerate(self.articulations.dof_names): + # dof pos + for re_key, value in self.cfg.init_state.dof_pos.items(): + if re.match(re_key, dof_name): + self._default_dof_pos[:, index] = value + # dof vel + for re_key, value in self.cfg.init_state.dof_vel.items(): + if re.match(re_key, dof_name): + self._default_dof_vel[:, index] = value + # -- tracked sites + if self.cfg.meta_info.sites_names: + sites_names = list() + sites_indices = list() + for body_index, body_name in enumerate(self.body_names): + for re_key in self.cfg.meta_info.sites_names: + if re.fullmatch(re_key, body_name): + sites_names.append(body_name) + sites_indices.append(body_index) + self.sites_indices: Dict[str, int] = dict(zip(sites_names, sites_indices)) + else: + self.sites_indices = None + + def _create_buffers(self): + """Create buffers for storing data.""" + # history buffers + self._previous_dof_vel = torch.zeros(self.count, self.num_dof, dtype=torch.float, device=self.device) + # constants + self._ALL_INDICES = torch.arange(self.count, dtype=torch.long, device=self.device) + + # -- frame states + self._data.root_state_w = torch.zeros(self.count, 13, dtype=torch.float, device=self.device) + if self.sites_indices is not None: + self._data.sites_state_w = torch.zeros(self.count, len(self.sites_indices), 13, device=self.device) + self._data.sites_state_b = torch.zeros_like(self._data.sites_state_w) + # -- dof states + self._data.dof_pos = torch.zeros(self.count, self.num_dof, dtype=torch.float, device=self.device) + self._data.dof_vel = torch.zeros_like(self._data.dof_pos) + self._data.dof_acc = torch.zeros_like(self._data.dof_pos) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py new file mode 100644 index 0000000000..048beee653 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py @@ -0,0 +1,89 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Dict, Optional, Sequence, Tuple + +from omni.isaac.orbit.utils import configclass + + +@configclass +class ArticulatedObjectCfg: + """Configuration parameters for an articulated object.""" + + @configclass + class MetaInfoCfg: + """Meta-information about the manipulator.""" + + usd_path: str = MISSING + """USD file to spawn asset from.""" + scale: Tuple[float] = (1.0, 1.0, 1.0) + """Scale of the object. Default to (1.0, 1.0, 1.0).""" + sites_names: Optional[Sequence[str]] = None + """Name of the sites to track (added to :obj:`data`). Defaults to :obj:`None`.""" + + @configclass + class RigidBodyPropertiesCfg: + """Properties to apply to all rigid bodies in the articulation.""" + + linear_damping: Optional[float] = None + """Linear damping coefficient.""" + angular_damping: Optional[float] = None + """Angular damping coefficient.""" + max_linear_velocity: Optional[float] = 1000.0 + """Maximum linear velocity for rigid bodies. Defaults to 1000.0.""" + max_angular_velocity: Optional[float] = 1000.0 + """Maximum angular velocity for rigid bodies. Defaults to 1000.0.""" + max_depenetration_velocity: Optional[float] = 10.0 + """Maximum depenetration velocity permitted to be introduced by the solver. Defaults to 10.0.""" + disable_gravity: Optional[bool] = False + """Disable gravity for the actor. Defaults to False.""" + retain_accelerations: Optional[bool] = None + """Carries over forces/accelerations over sub-steps.""" + + @configclass + class ArticulationRootPropertiesCfg: + """Properties to apply to articulation.""" + + enable_self_collisions: Optional[bool] = None + """Whether to enable or disable self-collisions.""" + solver_position_iteration_count: Optional[int] = None + """Solver position iteration counts for the body.""" + solver_velocity_iteration_count: Optional[int] = None + """Solver position iteration counts for the body.""" + + @configclass + class InitialStateCfg: + """Initial state of the robot.""" + + # root state + pos: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + rot: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation ``(w, x, y, z)`` of the root in simulation world frame. + Defaults to (1.0, 0.0, 0.0, 0.0). + """ + lin_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Linear velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + ang_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Angular velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + # dof state + dof_pos: Dict[str, float] = MISSING + """DOF positions of all joints.""" + dof_vel: Dict[str, float] = MISSING + """DOF velocities of all joints.""" + + ## + # Initialize configurations. + ## + + meta_info: MetaInfoCfg = MetaInfoCfg() + """Meta-information about the articulated object.""" + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the articulated object.""" + rigid_props: RigidBodyPropertiesCfg = RigidBodyPropertiesCfg() + """Properties to apply to all rigid bodies in the articulation.""" + articulation_props: ArticulationRootPropertiesCfg = ArticulationRootPropertiesCfg() + """Properties to apply to articulation.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_data.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_data.py new file mode 100644 index 0000000000..e609c0e0eb --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_data.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + + +@dataclass +class ArticulatedObjectData: + """Data container for an articulated object.""" + + ## + # Frame states. + ## + + root_state_w: torch.Tensor = None + """ + Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is ``(count, 13)``. + """ + + sites_state_w: torch.Tensor = None + """ + Sites frames state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is ``(count, num_sites, 13)``. + """ + + sites_state_b: torch.Tensor = None + """ + Sites frames state ``[pos, quat, lin_vel, ang_vel]`` in base frame. + Shape: (count, num_sites, 13). + """ + + ## + # DOF states <- From simulation. + ## + + dof_pos: torch.Tensor = None + """DOF positions of all joints. Shape is ``(count, num_dof)``.""" + + dof_vel: torch.Tensor = None + """DOF velocities of all joints. Shape is ``(count, num_dof)``.""" + + dof_acc: torch.Tensor = None + """DOF acceleration of all joints. Shape is ``(count, num_dof)``.""" + + """ + Properties + """ + + @property + def root_pos_w(self) -> torch.Tensor: + """Root position in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_state_w[:, :3] + + @property + def root_quat_w(self) -> torch.Tensor: + """ + Root orientation in quaternion (w, x, y, z) in simulation world frame. + Shape is ``(count, 4)``. + """ + return self.root_state_w[:, 3:7] + + @property + def root_lin_vel_w(self) -> torch.Tensor: + """Root linear velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_state_w[:, 7:10] + + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Root angular velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_state_w[:, 10:13] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/__init__.py new file mode 100644 index 0000000000..a9320b8c21 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodule for handling rigid objects. +""" + +from .rigid_object import RigidObject +from .rigid_object_cfg import RigidObjectCfg +from .rigid_object_data import RigidObjectData + +__all__ = ["RigidObjectCfg", "RigidObjectData", "RigidObject"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py new file mode 100644 index 0000000000..7f6f046710 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py @@ -0,0 +1,263 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from typing import Optional, Sequence + +import carb +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.materials import PhysicsMaterial +from omni.isaac.core.prims import GeometryPrim, RigidPrim, RigidPrimView + +import omni.isaac.orbit.utils.kit as kit_utils + +from .rigid_object_cfg import RigidObjectCfg +from .rigid_object_data import RigidObjectData + + +class RigidObject: + """Class for handling rigid objects. + + Rigid objects are spawned from USD files and are encapsulated by a single root prim. + The root prim is used to apply physics material to the rigid body. + + This class wraps around :class:`RigidPrimView` class from Isaac Sim to support the following: + + * Configuring using a single dataclass (struct). + * Applying physics material to the rigid body. + * Handling different rigid body views. + * Storing data related to the rigid object. + + """ + + cfg: RigidObjectCfg + """Configuration class for the rigid object.""" + objects: RigidPrimView + """Rigid prim view for the rigid object.""" + + def __init__(self, cfg: RigidObjectCfg): + """Initialize the rigid object. + + Args: + cfg (RigidObjectCfg): An instance of the configuration class. + """ + # store inputs + self.cfg = cfg + # container for data access + self._data = RigidObjectData() + # buffer variables (filled during spawn and initialize) + self._spawn_prim_path: str = None + + """ + Properties + """ + + @property + def count(self) -> int: + """Number of prims encapsulated.""" + return self.objects.count + + @property + def device(self) -> str: + """Memory device for computation.""" + return self.objects._device + + @property + def data(self) -> RigidObjectData: + """Data related to articulation.""" + return self._data + + """ + Operations. + """ + + def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): + """Spawn a rigid object into the stage (loaded from its USD file). + + Note: + If inputs `translation` or `orientation` are not :obj:`None`, then they override the initial root + state specified through the configuration class at spawning. + + Args: + prim_path (str): The prim path for spawning object at. + translation (Sequence[float], optional): The local position of prim from its parent. Defaults to None. + orientation (Sequence[float], optional): The local rotation (as quaternion `(w, x, y, z)` + of the prim from its parent. Defaults to None. + """ + # use default arguments + if translation is None: + translation = self.cfg.init_state.pos + if orientation is None: + orientation = self.cfg.init_state.rot + + # -- save prim path for later + self._spawn_prim_path = prim_path + # -- spawn asset if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + # add prim as reference to stage + prim_utils.create_prim( + self._spawn_prim_path, + usd_path=self.cfg.meta_info.usd_path, + translation=translation, + orientation=orientation, + scale=self.cfg.meta_info.scale, + ) + else: + carb.log_warn(f"A prim already exists at prim path: '{prim_path}'. Skipping...") + + # TODO: What if prim already exists in the stage and spawn isn't called? + # apply rigid body properties + RigidPrim(prim_path=prim_path) + # -- set rigid body properties + kit_utils.set_nested_rigid_body_properties( + prim_path, + max_linear_velocity=self.cfg.rigid_props.max_linear_velocity, + max_angular_velocity=self.cfg.rigid_props.max_angular_velocity, + max_depenetration_velocity=self.cfg.rigid_props.max_depenetration_velocity, + disable_gravity=self.cfg.rigid_props.disable_gravity, + ) + # create physics material + material = PhysicsMaterial( + prim_path + self.cfg.material_props.material_path, + static_friction=self.cfg.material_props.static_friction, + dynamic_friction=self.cfg.material_props.dynamic_friction, + restitution=self.cfg.material_props.restitution, + ) + # TODO: Iterate over the rigid prim and set this material to all collision shapes. + if self.cfg.meta_info.geom_prim_rel_path is not None: + object_geom = GeometryPrim(prim_path=prim_path + self.cfg.meta_info.geom_prim_rel_path) + else: + object_geom = GeometryPrim(prim_path=prim_path) + # apply physics material on object + object_geom.apply_physics_material(material) + + def initialize(self, prim_paths_expr: Optional[str] = None): + """Initializes the PhysX handles and internal buffers. + + Note: + PhysX handles are only enabled once the simulator starts playing. Hence, this function needs to be + called whenever the simulator "plays" from a "stop" state. + + Args: + prim_paths_expr (Optional[str], optional): The prim path expression for the prims. Defaults to None. + + Raises: + RuntimeError: When input `prim_paths_expr` is :obj:`None`, the method defaults to using the last + prim path set when calling the :meth:`spawn()` function. In case, the object was not spawned + and no valid `prim_paths_expr` is provided, the function throws an error. + """ + # default prim path if not cloned + if prim_paths_expr is None: + if self._is_spawned is not None: + self._prim_paths_expr = self._spawn_prim_path + else: + raise RuntimeError( + "Initialize the object failed! Please provide a valid argument for `prim_paths_expr`." + ) + else: + self._prim_paths_expr = prim_paths_expr + # create handles + # -- object views + self.objects = RigidPrimView(prim_paths_expr, reset_xform_properties=False) + self.objects.initialize() + # set the default state + self.objects.post_reset() + # set properties over all instances + # -- meta-information + self._process_info_cfg() + # create buffers + self._create_buffers() + + def reset_buffers(self, env_ids: Optional[Sequence[int]] = None): + """Resets all internal buffers. + + Args: + env_ids (Optional[Sequence[int]], optional): The indices of the object to reset. + Defaults to None (all instances). + """ + pass + + def update_buffers(self, dt: float = None): + """Update the internal buffers. + + The time step ``dt`` is used to compute numerical derivatives of quantities such as joint + accelerations which are not provided by the simulator. + + Args: + dt (float, optional): The amount of time passed from last `update_buffers` call. Defaults to None. + """ + # frame states + position_w, quat_w = self.objects.get_world_poses(indices=self._ALL_INDICES, clone=False) + self._data.root_state_w[:, 0:3] = position_w + self._data.root_state_w[:, 3:7] = quat_w + self._data.root_state_w[:, 7:] = self.objects.get_velocities(indices=self._ALL_INDICES, clone=False) + + """ + Operations - State. + """ + + def set_root_state(self, root_states: torch.Tensor, env_ids: Optional[Sequence[int]] = None): + """Sets the root state (pose and velocity) of the actor over selected environment indices. + + Args: + root_states (torch.Tensor): Input root state for the actor, shape: (len(env_ids), 13). + env_ids (Optional[Sequence[int]]): Environment indices. + If :obj:`None`, then all indices are used. + """ + # resolve all indices + if env_ids is None: + env_ids = self._ALL_INDICES + # set into simulation + self.objects.set_world_poses(root_states[:, 0:3], root_states[:, 3:7], indices=env_ids) + self.objects.set_velocities(root_states[:, 7:], indices=env_ids) + + # TODO: Move these to reset_buffers call. + # 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] = root_states.clone() + + def get_default_root_state(self, env_ids: Optional[Sequence[int]] = None, clone=True) -> torch.Tensor: + """Returns the default/initial root state of actor. + + Args: + env_ids (Optional[Sequence[int]], optional): Environment indices. + Defaults to None (all environment indices). + clone (bool, optional): Whether to return a copy or not. Defaults to True. + + Returns: + torch.Tensor: The default/initial root state of the actor, shape: (len(env_ids), 13). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + # return copy + if clone: + return torch.clone(self._default_root_states[env_ids]) + else: + return self._default_root_states[env_ids] + + """ + Internal helper. + """ + + def _process_info_cfg(self) -> None: + """Post processing of configuration parameters.""" + # default state + # -- root state + default_root_state = ( + self.cfg.init_state.pos + + self.cfg.init_state.rot + + self.cfg.init_state.lin_vel + + self.cfg.init_state.ang_vel + ) + self._default_root_states = torch.tensor(default_root_state, device=self.device).repeat(self.count, 1) + + def _create_buffers(self): + """Create buffers for storing data.""" + # constants + self._ALL_INDICES = torch.arange(self.count, dtype=torch.long, device=self.device) + + # -- frame states + self._data.root_state_w = torch.zeros(self.count, 13, dtype=torch.float, device=self.device) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py new file mode 100644 index 0000000000..2c235ea65d --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Optional, Tuple + +from omni.isaac.orbit.utils import configclass + + +@configclass +class RigidObjectCfg: + """Configuration parameters for a robot.""" + + @configclass + class MetaInfoCfg: + """Meta-information about the manipulator.""" + + usd_path: str = MISSING + """USD file to spawn asset from.""" + geom_prim_rel_path: str = MISSING + """Relative path to the collision geom from the default prim in the USD file. + + This is used to apply physics material to the rigid body. + """ + scale: Tuple[float, float, float] = (1.0, 1.0, 1.0) + """Scale to spawn the object with. Defaults to (1.0, 1.0, 1.0).""" + + @configclass + class RigidBodyPropertiesCfg: + """Properties to apply to the rigid body.""" + + max_linear_velocity: Optional[float] = 1000.0 + """Maximum linear velocity for rigid bodies. Defaults to 1000.0.""" + max_angular_velocity: Optional[float] = 1000.0 + """Maximum angular velocity for rigid bodies. Defaults to 1000.0.""" + max_depenetration_velocity: Optional[float] = 10.0 + """Maximum depenetration velocity permitted to be introduced by the solver. Defaults to 10.0.""" + disable_gravity: Optional[bool] = False + """Disable gravity for the actor. Defaults to False.""" + + @configclass + class PhysicsMaterialPropertiesCfg: + """Properties to apply to the rigid body.""" + + static_friction: Optional[float] = 0.5 + """Static friction for the rigid body. Defaults to 0.5.""" + dynamic_friction: Optional[float] = 0.5 + """Dynamic friction for the rigid body. Defaults to 0.5.""" + restitution: Optional[float] = 0.0 + """Restitution for the rigid body. Defaults to 0.0.""" + material_path: Optional[str] = "/physics_material" + """Relative path to spawn the material for the rigid body. Defaults to "/physics_material".""" + + @configclass + class InitialStateCfg: + """Initial state of the rigid body.""" + + # root position + pos: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + rot: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation ``(w, x, y, z)`` of the root in simulation world frame. + Defaults to (1.0, 0.0, 0.0, 0.0). + """ + lin_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Linear velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + ang_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Angular velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + + ## + # Initialize configurations. + ## + + meta_info: MetaInfoCfg = MetaInfoCfg() + """Meta-information about the rigid body.""" + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the rigid body.""" + rigid_props: RigidBodyPropertiesCfg = RigidBodyPropertiesCfg() + """Properties to apply to the rigid body.""" + material_props: PhysicsMaterialPropertiesCfg = PhysicsMaterialPropertiesCfg() + """Properties to apply to the physics material on the rigid body.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_data.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_data.py new file mode 100644 index 0000000000..1330b49f3a --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_data.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + + +@dataclass +class RigidObjectData: + """Data container for a robot.""" + + ## + # Frame states. + ## + + root_state_w: torch.Tensor = None + """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is ``(count, 13)``.""" + + """ + Properties + """ + + @property + def root_pos_w(self) -> torch.Tensor: + """Root position in simulation world frame. Shape is ``(count, 3).``""" + 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 ``(count, 4)``.""" + return self.root_state_w[:, 3:7] + + @property + def root_lin_vel_w(self) -> torch.Tensor: + """Root linear velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_state_w[:, 7:10] + + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Root angular velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_state_w[:, 10:13] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/__init__.py new file mode 100644 index 0000000000..9b8476b598 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/__init__.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodule containing all robot abstractions. +""" + +from .legged_robot import LeggedRobot, LeggedRobotCfg, LeggedRobotData +from .mobile_manipulator import ( + LeggedMobileManipulator, + LeggedMobileManipulatorCfg, + LeggedMobileManipulatorData, + MobileManipulator, + MobileManipulatorCfg, + MobileManipulatorData, +) +from .single_arm import SingleArmManipulator, SingleArmManipulatorCfg, SingleArmManipulatorData + +__all__ = [ + # single-arm manipulators + "SingleArmManipulatorCfg", + "SingleArmManipulatorData", + "SingleArmManipulator", + # legged robots + "LeggedRobotCfg", + "LeggedRobotData", + "LeggedRobot", + # mobile manipulators + "MobileManipulatorCfg", + "MobileManipulatorData", + "MobileManipulator", + # legged mobile manipulators + "LeggedMobileManipulator", + "LeggedMobileManipulatorCfg", + "LeggedMobileManipulatorData", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/__init__.py new file mode 100644 index 0000000000..11640753eb --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Default configurations for various supported robots. + +This module contains the default configurations for various supported robots. +These configurations can be used to easily configure a robot instance. + +Note: + These robot configurations are only meant to be used as a starting point for + defining your own configurations. You should always review and modify these + configurations to suit your needs. +""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/anymal.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/anymal.py new file mode 100644 index 0000000000..dcf9373033 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/anymal.py @@ -0,0 +1,149 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration fo the ANYbotics robots. + +The following configuration parameters are available: +* :obj:`ANYMAL_B_CFG`: The ANYmal-B robot with ANYdrives 3.0 +* :obj:`ANYMAL_C_CFG`: The ANYmal-C robot with ANYdrives 3.0 + +Reference: +* https://github.com/ANYbotics/anymal_b_simple_description +* https://github.com/ANYbotics/anymal_c_simple_description +""" + + +import math +from scipy.spatial.transform import Rotation + +from omni.isaac.orbit.actuators.config.anydrive import ANYMAL_C_DEFAULT_GROUP_CFG +from omni.isaac.orbit.utils.assets import ISAAC_ORBIT_NUCLEUS_DIR + +from ..legged_robot import LeggedRobotCfg + +__all__ = ["ANYMAL_B_CFG", "ANYMAL_C_CFG"] + +## +# Helper functions +## + + +def quat_from_euler_rpy(roll, pitch, yaw, degrees=False): + """Converts Euler XYZ to Quaternion (w, x, y, z).""" + quat = Rotation.from_euler("xyz", (roll, pitch, yaw), degrees=degrees).as_quat() + return tuple(quat[[3, 0, 1, 2]].tolist()) + + +def euler_rpy_apply(rpy, xyz, degrees=False): + """Applies rotation from Euler XYZ on position vector.""" + rot = Rotation.from_euler("xyz", rpy, degrees=degrees) + return tuple(rot.apply(xyz).tolist()) + + +## +# Configuration +## + +_ANYMAL_B_INSTANCEABLE_USD = f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/ANYbotics/ANYmalB/anymal_b_instanceable.usd" +_ANYMAL_C_INSTANCEABLE_USD = f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/ANYbotics/ANYmalC/anymal_c_minimal_instanceable.usd" + + +ANYMAL_B_CFG = LeggedRobotCfg( + meta_info=LeggedRobotCfg.MetaInfoCfg(usd_path=_ANYMAL_B_INSTANCEABLE_USD, soft_dof_pos_limit_factor=0.95), + feet_info={ + "LF_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="LF_SHANK", + pos_offset=(0.1, -0.02, -0.3215), + ), + "RF_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="RF_SHANK", + pos_offset=(0.1, 0.02, -0.3215), + ), + "LH_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="LH_SHANK", + pos_offset=(-0.1, -0.02, -0.3215), + ), + "RH_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="RH_SHANK", + pos_offset=(-0.1, 0.02, -0.3215), + ), + }, + init_state=LeggedRobotCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.6), + dof_pos={ + ".*HAA": 0.0, # all HAA + ".*F_HFE": 0.4, # both front HFE + ".*H_HFE": -0.4, # both hind HFE + ".*F_KFE": -0.8, # both front KFE + ".*H_KFE": 0.8, # both hind KFE + }, + dof_vel={".*": 0.0}, + ), + rigid_props=LeggedRobotCfg.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=LeggedRobotCfg.ArticulationRootPropertiesCfg( + enable_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=1 + ), + actuator_groups={"base_legs": ANYMAL_C_DEFAULT_GROUP_CFG}, +) +"""Configuration of ANYmal-B robot using actuator-net.""" + +ANYMAL_C_CFG = LeggedRobotCfg( + meta_info=LeggedRobotCfg.MetaInfoCfg(usd_path=_ANYMAL_C_INSTANCEABLE_USD, soft_dof_pos_limit_factor=0.95), + feet_info={ + "LF_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="LF_SHANK", + pos_offset=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), + rot_offset=quat_from_euler_rpy(0, 0, -math.pi / 2), + ), + "RF_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="RF_SHANK", + pos_offset=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), + rot_offset=quat_from_euler_rpy(0, 0, math.pi / 2), + ), + "LH_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="LH_SHANK", + pos_offset=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(-0.08795, 0.01305, -0.33797)), + rot_offset=quat_from_euler_rpy(0, 0, -math.pi / 2), + ), + "RH_FOOT": LeggedRobotCfg.FootFrameCfg( + body_name="RH_SHANK", + pos_offset=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(-0.08795, -0.01305, -0.33797)), + rot_offset=quat_from_euler_rpy(0, 0, math.pi / 2), + ), + }, + init_state=LeggedRobotCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.6), + dof_pos={ + ".*HAA": 0.0, # all HAA + ".*F_HFE": 0.4, # both front HFE + ".*H_HFE": -0.4, # both hind HFE + ".*F_KFE": -0.8, # both front KFE + ".*H_KFE": 0.8, # both hind KFE + }, + dof_vel={".*": 0.0}, + ), + rigid_props=LeggedRobotCfg.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=LeggedRobotCfg.ArticulationRootPropertiesCfg( + enable_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=1 + ), + actuator_groups={"base_legs": ANYMAL_C_DEFAULT_GROUP_CFG}, +) +"""Configuration of ANYmal-C robot using actuator-net.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py new file mode 100644 index 0000000000..e954178077 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py @@ -0,0 +1,79 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Franka Emika robots. + +The following configurations are available: + +* :obj:`FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG`: Franka Emika Panda robot with Panda hand + +Reference: https://github.com/frankaemika/franka_ros +""" + + +from omni.isaac.orbit.actuators.config.franka import PANDA_HAND_MIMIC_GROUP_CFG +from omni.isaac.orbit.actuators.group import ActuatorGroupCfg +from omni.isaac.orbit.actuators.group.actuator_group_cfg import ActuatorControlCfg +from omni.isaac.orbit.actuators.model import ImplicitActuatorCfg +from omni.isaac.orbit.utils.assets import ISAAC_ORBIT_NUCLEUS_DIR + +from ..single_arm import SingleArmManipulatorCfg + +_FRANKA_PANDA_ARM_INSTANCEABLE_USD = f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + + +FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG = SingleArmManipulatorCfg( + meta_info=SingleArmManipulatorCfg.MetaInfoCfg( + usd_path=_FRANKA_PANDA_ARM_INSTANCEABLE_USD, + arm_num_dof=7, + tool_num_dof=2, + tool_sites_names=["panda_leftfinger", "panda_rightfinger"], + ), + init_state=SingleArmManipulatorCfg.InitialStateCfg( + dof_pos={ + "panda_joint1": 0.0, + "panda_joint2": -0.569, + "panda_joint3": 0.0, + "panda_joint4": -2.810, + "panda_joint5": 0.0, + "panda_joint6": 3.037, + "panda_joint7": 0.741, + "panda_finger_joint*": 0.035, + }, + dof_vel={".*": 0.0}, + ), + ee_info=SingleArmManipulatorCfg.EndEffectorFrameCfg( + body_name="panda_hand", pos_offset=(0.0, 0.0, 0.1034), rot_offset=(1.0, 0.0, 0.0, 0.0) + ), + actuator_groups={ + "panda_shoulder": ActuatorGroupCfg( + dof_names=["panda_joint[1-4]"], + model_cfg=ImplicitActuatorCfg(velocity_limit=100.0, torque_limit=87.0), + control_cfg=ActuatorControlCfg( + command_types=["p_abs"], + stiffness={".*": 800.0}, + damping={".*": 40.0}, + dof_pos_offset={ + "panda_joint1": 0.0, + "panda_joint2": -0.569, + "panda_joint3": 0.0, + "panda_joint4": -2.810, + }, + ), + ), + "panda_forearm": ActuatorGroupCfg( + dof_names=["panda_joint[5-7]"], + model_cfg=ImplicitActuatorCfg(velocity_limit=100.0, torque_limit=12.0), + control_cfg=ActuatorControlCfg( + command_types=["p_abs"], + stiffness={".*": 800.0}, + damping={".*": 40.0}, + dof_pos_offset={"panda_joint5": 0.0, "panda_joint6": 3.037, "panda_joint7": 0.741}, + ), + ), + "panda_hand": PANDA_HAND_MIMIC_GROUP_CFG, + }, +) +"""Configuration of Franka arm with Franka Hand using implicit actuator models.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/ridgeback_franka.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/ridgeback_franka.py new file mode 100644 index 0000000000..84dcff5276 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/ridgeback_franka.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Ridgeback-Manipulation robots. + +The following configurations are available: + +* :obj:`RIDGEBACK_FRANKA_PANDA_CFG`: Clearpath Ridgeback base with Franka Emika arm + +Reference: https://github.com/ridgeback/ridgeback_manipulation +""" + + +from omni.isaac.orbit.actuators.config.franka import PANDA_HAND_MIMIC_GROUP_CFG +from omni.isaac.orbit.actuators.group import ActuatorControlCfg, ActuatorGroupCfg +from omni.isaac.orbit.actuators.model import ImplicitActuatorCfg +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + +from ..mobile_manipulator import MobileManipulatorCfg + +_RIDGEBACK_FRANKA_PANDA_ARM_USD = f"{ISAAC_NUCLEUS_DIR}/Robots/Clearpath/RidgebackFranka/ridgeback_franka.usd" + + +RIDGEBACK_FRANKA_PANDA_CFG = MobileManipulatorCfg( + meta_info=MobileManipulatorCfg.MetaInfoCfg( + usd_path=_RIDGEBACK_FRANKA_PANDA_ARM_USD, + base_num_dof=3, + arm_num_dof=7, + tool_num_dof=2, + tool_sites_names=["panda_leftfinger", "panda_rightfinger"], + ), + init_state=MobileManipulatorCfg.InitialStateCfg( + dof_pos={ + # base + "dummy_base_prismatic_y_joint": 0.0, + "dummy_base_prismatic_x_joint": 0.0, + "dummy_base_revolute_z_joint": 0.0, + # franka arm + "panda_joint1": 0.0, + "panda_joint2": -0.569, + "panda_joint3": 0.0, + "panda_joint4": -2.810, + "panda_joint5": 0.0, + "panda_joint6": 3.037, + "panda_joint7": 0.741, + # tool + "panda_finger_joint*": 0.035, + }, + dof_vel={".*": 0.0}, + ), + ee_info=MobileManipulatorCfg.EndEffectorFrameCfg( + body_name="panda_hand", pos_offset=(0.0, 0.0, 0.1034), rot_offset=(1.0, 0.0, 0.0, 0.0) + ), + actuator_groups={ + "base": ActuatorGroupCfg( + dof_names=["dummy_base_.*"], + model_cfg=ImplicitActuatorCfg(velocity_limit=100.0, torque_limit=1000.0), + control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 0.0}, damping={".*": 1e5}), + ), + "panda_shoulder": ActuatorGroupCfg( + dof_names=["panda_joint[1-4]"], + model_cfg=ImplicitActuatorCfg(velocity_limit=100.0, torque_limit=87.0), + control_cfg=ActuatorControlCfg( + command_types=["p_abs"], + stiffness={".*": 800.0}, + damping={".*": 40.0}, + dof_pos_offset={ + "panda_joint1": 0.0, + "panda_joint2": -0.569, + "panda_joint3": 0.0, + "panda_joint4": -2.810, + }, + ), + ), + "panda_forearm": ActuatorGroupCfg( + dof_names=["panda_joint[5-7]"], + model_cfg=ImplicitActuatorCfg(velocity_limit=100.0, torque_limit=12.0), + control_cfg=ActuatorControlCfg( + command_types=["p_abs"], + stiffness={".*": 800.0}, + damping={".*": 40.0}, + dof_pos_offset={"panda_joint5": 0.0, "panda_joint6": 3.037, "panda_joint7": 0.741}, + ), + ), + "panda_hand": PANDA_HAND_MIMIC_GROUP_CFG, + }, +) +"""Configuration of Franka arm with Franka Hand on a Clearpath Ridgeback base using implicit actuator models. + +The following control configuration is used: + +* Base: velocity control with damping +* Arm: position control with damping (contains default position offsets) +* Hand: mimic control + +""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/unitree.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/unitree.py new file mode 100644 index 0000000000..765ea792df --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/unitree.py @@ -0,0 +1,84 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Unitree robots. + +The following configurations are available: + +* :obj:`UNITREE_A1_CFG`: Unitree A1 robot with simple PD controller for the legs + +Reference: https://github.com/unitreerobotics/unitree_ros +""" + + +from omni.isaac.orbit.actuators.group import ActuatorControlCfg, ActuatorGroupCfg +from omni.isaac.orbit.actuators.model.actuator_cfg import DCMotorCfg +from omni.isaac.orbit.utils.assets import ISAAC_ORBIT_NUCLEUS_DIR + +from ..legged_robot import LeggedRobotCfg + +## +# Configuration +## + +_UNITREE_A1_INSTANCEABLE_USD = f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/Unitree/A1/a1_instanceable.usd" + + +UNITREE_A1_CFG = LeggedRobotCfg( + meta_info=LeggedRobotCfg.MetaInfoCfg(usd_path=_UNITREE_A1_INSTANCEABLE_USD, soft_dof_pos_limit_factor=0.9), + feet_info={ + "FR_foot": LeggedRobotCfg.FootFrameCfg(body_name="FR_calf", pos_offset=(0.0, 0.0, -0.2)), + "FL_foot": LeggedRobotCfg.FootFrameCfg(body_name="FL_calf", pos_offset=(0.0, 0.0, -0.2)), + "RR_foot": LeggedRobotCfg.FootFrameCfg(body_name="RR_calf", pos_offset=(0.0, 0.0, -0.2)), + "RL_foot": LeggedRobotCfg.FootFrameCfg(body_name="RL_calf", pos_offset=(0.0, 0.0, -0.2)), + }, + init_state=LeggedRobotCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.42), + dof_pos={ + ".*L_hip_joint": 0.1, + ".*R_hip_joint": -0.1, + "F[L,R]_thigh_joint": 0.8, + "R[L,R]_thigh_joint": 1.0, + ".*_calf_joint": -1.8, + }, + dof_vel={".*": 0.0}, + ), + rigid_props=LeggedRobotCfg.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=LeggedRobotCfg.ArticulationRootPropertiesCfg( + enable_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=1 + ), + actuator_groups={ + "base_legs": ActuatorGroupCfg( + dof_names=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + model_cfg=DCMotorCfg( + motor_torque_limit=33.5, gear_ratio=1.0, peak_motor_torque=33.5, motor_velocity_limit=21.0 + ), + control_cfg=ActuatorControlCfg( + command_types=["p_abs"], + stiffness={".*": 25.0}, + damping={".*": 0.5}, + dof_pos_offset={ + ".*L_hip_joint": 0.1, + ".*R_hip_joint": -0.1, + "F[L,R]_thigh_joint": 0.8, + "R[L,R]_thigh_joint": 1.0, + ".*_calf_joint": -1.8, + }, + ), + ) + }, +) +"""Configuration of Unitree A1 using DC motor. + +Note: Specifications taken from: https://www.trossenrobotics.com/a1-quadruped#specifications +""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/universal_robots.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/universal_robots.py new file mode 100644 index 0000000000..f5c00a3986 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/universal_robots.py @@ -0,0 +1,57 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Universal Robots. + +The following configuration parameters are available: + +* :obj:`UR10_CFG`: The UR10 arm without a gripper. + +Reference: https://github.com/ros-industrial/universal_robot +""" + + +from omni.isaac.orbit.actuators.group import ActuatorGroupCfg +from omni.isaac.orbit.actuators.group.actuator_group_cfg import ActuatorControlCfg +from omni.isaac.orbit.actuators.model import ImplicitActuatorCfg +from omni.isaac.orbit.utils.assets import ISAAC_ORBIT_NUCLEUS_DIR + +from ..single_arm import SingleArmManipulatorCfg + +_UR10_ARM_INSTANCEBALE_USD = f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/UniversalRobots/UR10/ur10_instanceable.usd" + + +UR10_CFG = SingleArmManipulatorCfg( + meta_info=SingleArmManipulatorCfg.MetaInfoCfg( + usd_path=_UR10_ARM_INSTANCEBALE_USD, + arm_num_dof=6, + tool_num_dof=0, + tool_sites_names=None, + ), + init_state=SingleArmManipulatorCfg.InitialStateCfg( + dof_pos={ + "shoulder_pan_joint": 0.0, + "shoulder_lift_joint": -1.712, + "elbow_joint": 1.712, + "wrist_1_joint": 0.0, + "wrist_2_joint": 0.0, + "wrist_3_joint": 0.0, + }, + dof_vel={".*": 0.0}, + ), + ee_info=SingleArmManipulatorCfg.EndEffectorFrameCfg(body_name="wrist_3_link"), + actuator_groups={ + "arm": ActuatorGroupCfg( + dof_names=[".*"], + model_cfg=ImplicitActuatorCfg(velocity_limit=100.0, torque_limit=87.0), + control_cfg=ActuatorControlCfg( + command_types=["p_abs"], + stiffness={".*": None}, + damping={".*": None}, + ), + ), + }, +) +"""Configuration of UR-10 arm using implicit actuator models.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/__init__.py new file mode 100644 index 0000000000..faca3f8d7f --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for handling legged robots.""" + +from .legged_robot import LeggedRobot +from .legged_robot_cfg import LeggedRobotCfg +from .legged_robot_data import LeggedRobotData + +__all__ = ["LeggedRobot", "LeggedRobotCfg", "LeggedRobotData"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot.py new file mode 100644 index 0000000000..42bd367c32 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot.py @@ -0,0 +1,195 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from typing import Dict, Optional, Sequence + +from omni.isaac.core.materials import PhysicsMaterial +from omni.isaac.core.prims import RigidPrimView +from omni.isaac.core.prims.geometry_prim_view import GeometryPrimView +from pxr import PhysxSchema + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.utils.math import combine_frame_transforms, quat_rotate_inverse, subtract_frame_transforms + +from ..robot_base import RobotBase +from .legged_robot_cfg import LeggedRobotCfg +from .legged_robot_data import LeggedRobotData + + +class LeggedRobot(RobotBase): + """Class for handling a floating-base legged robot.""" + + cfg: LeggedRobotCfg + """Configuration for the legged robot.""" + feet_bodies: Dict[str, RigidPrimView] + """Rigid body view for feet of the robot. + + Dictionary with keys as the foot names and values as the corresponding rigid body view + in the robot. + """ + + def __init__(self, cfg: LeggedRobotCfg): + # initialize parent + super().__init__(cfg) + # container for data access + self._data = LeggedRobotData() + + """ + Properties + """ + + @property + def data(self) -> LeggedRobotData: + """Data related to articulation.""" + return self._data + + @property + def feet_names(self) -> Sequence[str]: + """Names of the feet.""" + return list(self.cfg.feet_info.keys()) + + """ + Operations. + """ + + def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): + # spawn the robot and set its location + super().spawn(prim_path, translation, orientation) + # alter physics collision properties + kit_utils.set_nested_collision_properties(prim_path, contact_offset=0.02, rest_offset=0.0) + # add physics material to the feet bodies! + # TODO: Make this something configurable? + foot_material = PhysicsMaterial( + prim_path=self.cfg.physics_material.prim_path, + static_friction=self.cfg.physics_material.static_friction, + dynamic_friction=self.cfg.physics_material.dynamic_friction, + restitution=self.cfg.physics_material.restitution, + ) + # enable patch-friction: yields better results! + physx_material_api = PhysxSchema.PhysxMaterialAPI.Apply(foot_material.prim) + physx_material_api.CreateImprovePatchFrictionAttr().Set(True) + # add to bodies + body_names = [foot_cfg.body_name for foot_cfg in self.cfg.feet_info.values()] + # bind materials + geom_prim = GeometryPrimView(f"{prim_path}/{body_names}/collisions", reset_xform_properties=False) + geom_prim.apply_physics_materials(foot_material, weaker_than_descendants=False) + + def initialize(self, prim_paths_expr: Optional[str] = None): + # initialize parent handles + super().initialize(prim_paths_expr) + # create handles + # -- feet + self.feet_bodies: Dict[str, RigidPrimView] = dict() + for foot_name, body_name in self._feet_body_name.items(): + # create rigid body view to track + feet_body = RigidPrimView( + prim_paths_expr=f"{self._prim_paths_expr}/{body_name}", name=foot_name, reset_xform_properties=False + ) + feet_body.initialize() + # add to list + self.feet_bodies[foot_name] = feet_body + + def reset_buffers(self, env_ids: Optional[Sequence[int]] = None): + # reset parent buffers + super().reset_buffers(env_ids) + # reset timers + self._ongoing_feet_air_time[:] = 0.0 + self._data.feet_air_time[:] = 0.0 + + def update_buffers(self, dt: float): + # update parent buffers + super().update_buffers(dt) + # frame states + # -- root frame in base + self._data.root_vel_b[:, 0:3] = quat_rotate_inverse(self._data.root_quat_w, self._data.root_lin_vel_w) + self._data.root_vel_b[:, 3:6] = quat_rotate_inverse(self._data.root_quat_w, self._data.root_ang_vel_w) + self._data.projected_gravity_b[:] = quat_rotate_inverse(self._data.root_quat_w, self._GRAVITY_VEC_W) + # -- feet + # TODO: This can be sped up by combining all feet into one regex expression. + for index, (foot_name, body) in enumerate(self.feet_bodies.items()): + # extract foot offset information + foot_pos_offset = self._feet_pos_offset[foot_name] + foot_rot_offset = self._feet_rot_offset[foot_name] + # world frame + # -- foot frame in world: world -> shank frame -> foot frame + shank_position_w, shank_quat_w = body.get_world_poses(indices=self._ALL_INDICES, clone=False) + position_w, quat_w = combine_frame_transforms( + shank_position_w, shank_quat_w, foot_pos_offset, foot_rot_offset + ) + self._data.feet_state_w[:, index, 0:3] = position_w + self._data.feet_state_w[:, index, 3:7] = quat_w + # TODO: Transformation velocities from hand to end-effector? + self._data.feet_state_w[:, index, 7:] = body.get_velocities(indices=self._ALL_INDICES, clone=False) + # base frame + position_b, quat_b = subtract_frame_transforms( + self._data.root_state_w[:, 0:3], + self._data.root_state_w[:, 3:7], + self._data.feet_state_w[:, index, 0:3], + self._data.feet_state_w[:, index, 3:7], + ) + self._data.feet_state_b[:, index, 0:3] = position_b + self._data.feet_state_b[:, index, 3:7] = quat_b + self._data.feet_state_b[:, index, 7:10] = quat_rotate_inverse( + self._data.root_quat_w, self._data.feet_state_w[:, index, 7:10] + ) + self._data.feet_state_b[:, index, 10:13] = quat_rotate_inverse( + self._data.root_quat_w, self._data.feet_state_w[:, index, 10:13] + ) + # TODO: contact forces -- Waiting for contact sensors in IsaacSim. + # For now, use heuristics for flat terrain to say feet are in contact. + # air times + # -- update ongoing timer for feet air + self._ongoing_feet_air_time += dt + # -- check contact state of feet + is_feet_contact = self._data.feet_state_w[:, :, 2] < 0.03 + is_feet_first_contact = (self._ongoing_feet_air_time > 0) * is_feet_contact + # -- update buffers + self._data.feet_air_time = self._ongoing_feet_air_time * is_feet_first_contact + # -- reset timers for feet that are in contact for the first time + self._ongoing_feet_air_time *= ~is_feet_contact + + """ + Internal helpers - Override. + """ + + def _process_info_cfg(self) -> None: + """Post processing of configuration parameters.""" + # process parent config + super()._process_info_cfg() + # feet offsets + self._feet_body_name: Dict[str, str] = dict.fromkeys(self.cfg.feet_info) + self._feet_pos_offset: Dict[str, torch.Tensor] = dict.fromkeys(self.cfg.feet_info) + self._feet_rot_offset: Dict[str, torch.Tensor] = dict.fromkeys(self.cfg.feet_info) + for foot_name, foot_cfg in self.cfg.feet_info.items(): + # -- body name + self._feet_body_name[foot_name] = foot_cfg.body_name + # -- position + foot_pos_offset = torch.tensor(foot_cfg.pos_offset, device=self.device) + self._feet_pos_offset[foot_name] = foot_pos_offset.repeat(self.count, 1) + # -- orientation + foot_rot_offset = torch.tensor(foot_cfg.rot_offset, device=self.device) + self._feet_rot_offset[foot_name] = foot_rot_offset.repeat(self.count, 1) + + def _create_buffers(self): + """Create buffers for storing data.""" + # process parent buffers + super()._create_buffers() + # history + self._ongoing_feet_air_time = torch.zeros(self.count, len(self._feet_body_name), device=self.device) + # constants + # TODO: get gravity direction from stage. + self._GRAVITY_VEC_W = torch.tensor((0.0, 0.0, -1.0), device=self.device).repeat(self.count, 1) + self._FORWARD_VEC_B = torch.tensor((1.0, 0.0, 0.0), device=self.device).repeat(self.count, 1) + + # frame states + # -- base + self._data.root_vel_b = torch.zeros(self.count, 6, dtype=torch.float, device=self.device) + self._data.projected_gravity_b = torch.zeros(self.count, 3, dtype=torch.float, device=self.device) + # -- feet + self._data.feet_state_w = torch.zeros(self.count, len(self._feet_body_name), 13, device=self.device) + self._data.feet_state_b = torch.zeros_like(self._data.feet_state_w) + # -- air time + self._data.feet_air_time = torch.zeros(self.count, len(self._feet_body_name), device=self.device) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot_cfg.py new file mode 100644 index 0000000000..1e6c7e3fa9 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot_cfg.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Dict, Tuple + +from omni.isaac.orbit.utils import configclass + +from ..robot_base_cfg import RobotBaseCfg + + +@configclass +class LeggedRobotCfg(RobotBaseCfg): + """Properties for a legged robot (floating-base).""" + + @configclass + class FootFrameCfg: + """Information about the end-effector/foot frame location.""" + + body_name: str = MISSING + """Name of the body to track.""" + pos_offset: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Additional position offset from the body frame. (default: (0, 0, 0).""" + rot_offset: Tuple[float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Additional rotation offset (w, x, y, z) from the body frame. (default: (1, 0, 0, 0).""" + + @configclass + class PhysicsMaterialCfg: + """Physics material applied to the feet of the robot.""" + + prim_path = "/World/Materials/footMaterial" + """Path to the physics material prim. Default: /World/Materials/footMaterial.""" + static_friction: float = 1.0 + """Static friction coefficient. Default: 1.0.""" + dynamic_friction: float = 1.0 + """Dynamic friction coefficient. Default: 1.0.""" + restitution: float = 0.0 + """Restitution coefficient. Default: 0.0.""" + + ## + # Initialize configurations. + ## + + feet_info: Dict[str, FootFrameCfg] = MISSING + """Information about the feet to track (added to :obj:`data`). + + The returned tensor for feet state is in the same order as that of the provided list. + """ + physics_material: PhysicsMaterialCfg = PhysicsMaterialCfg() + """Settings for the physics material to apply to feet.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot_data.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot_data.py new file mode 100644 index 0000000000..93f8e8cc59 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot_data.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + +from ..robot_base_data import RobotBaseData + + +@dataclass +class LeggedRobotData(RobotBaseData): + """Data container for a legged robot.""" + + ## + # Frame states. + ## + + root_vel_b: torch.Tensor = None + """Root velocity `[lin_vel, ang_vel]` in base frame. Shape is ``(count, 6)``.""" + + projected_gravity_b: torch.Tensor = None + """Projection of the gravity direction on base frame. Shape is ``(count, 3)``.""" + + feet_state_w: torch.Tensor = None + """Feet sites frames state `[pos, quat, lin_vel, ang_vel]` in simulation world frame. Shape is ``(count, num_feet, 13)``.""" + + feet_state_b: torch.Tensor = None + """Feet frames state `[pos, quat, lin_vel, ang_vel]` in base frame. Shape is ``(count, num_feet, 13)``.""" + + feet_air_time: torch.Tensor = None + """Time spent (in s) during swing phase of each leg since last contact. Shape is ``(count, num_feet)``.""" + + ## + # Proprioceptive sensors. + ## + + feet_contact_forces: torch.Tensor = None + """Feet contact wrenches `[force, torque]` in simulation world frame. Shape is ``(count, num_feet, 6)``.""" + + """ + Properties + """ + + @property + def root_lin_vel_b(self) -> torch.Tensor: + """Root linear velocity in base frame. Shape is ``(count, 3)``.""" + return self.root_vel_b[:, 0:3] + + @property + def root_ang_vel_b(self) -> torch.Tensor: + """Root angular velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_vel_b[:, 3:6] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/__init__.py new file mode 100644 index 0000000000..ec558455b2 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/__init__.py @@ -0,0 +1,21 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for handling mobile manipulators.""" + +from .mobile_manipulator import LeggedMobileManipulator, MobileManipulator +from .mobile_manipulator_cfg import LeggedMobileManipulatorCfg, MobileManipulatorCfg +from .mobile_manipulator_data import LeggedMobileManipulatorData, MobileManipulatorData + +__all__ = [ + # general mobile manipulator + "MobileManipulator", + "MobileManipulatorCfg", + "MobileManipulatorData", + # mobile manipulator with a legged base + "LeggedMobileManipulator", + "LeggedMobileManipulatorCfg", + "LeggedMobileManipulatorData", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator.py new file mode 100644 index 0000000000..65e54e7da2 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator.py @@ -0,0 +1,154 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from typing import Optional, Sequence + +from omni.isaac.orbit.utils.math import quat_rotate_inverse + +from ..legged_robot import LeggedRobot +from ..single_arm import SingleArmManipulator +from .mobile_manipulator_cfg import LeggedMobileManipulatorCfg, MobileManipulatorCfg +from .mobile_manipulator_data import LeggedMobileManipulatorData, MobileManipulatorData + +__all__ = ["MobileManipulator", "LeggedMobileManipulator"] + + +class MobileManipulator(SingleArmManipulator): + """Class for handling a mobile manipulator robot with a tool on it.""" + + cfg: MobileManipulatorCfg + """Configuration for the mobile manipulator.""" + + def __init__(self, cfg: MobileManipulatorCfg): + # initialize parent + super().__init__(cfg) + # container for data access + self._data = MobileManipulatorData() + + """ + Properties + """ + + @property + def base_num_dof(self) -> int: + """Number of DOFs in the robot base.""" + return self.cfg.meta_info.base_num_dof + + @property + def data(self) -> MobileManipulatorData: + """Data related to articulation.""" + return self._data + + """ + Operations. + """ + + def update_buffers(self, dt: float): + # update parent buffers + super().update_buffers(dt) + # frame states + # -- root frame in base + self._data.root_vel_b[:, 0:3] = quat_rotate_inverse(self._data.root_quat_w, self._data.root_lin_vel_w) + self._data.root_vel_b[:, 3:6] = quat_rotate_inverse(self._data.root_quat_w, self._data.root_ang_vel_w) + self._data.projected_gravity_b[:] = quat_rotate_inverse(self._data.root_quat_w, self._GRAVITY_VEC_W) + + """ + Internal helpers - Override. + """ + + def _create_buffers(self): + """Create buffers for storing data.""" + # process parent buffers + super()._create_buffers() + # constants + self._GRAVITY_VEC_W = torch.tensor((0.0, 0.0, -1.0), device=self.device).repeat(self.count, 1) + + # frame states + # -- base + self._data.root_vel_b = torch.zeros(self.count, 6, dtype=torch.float, device=self.device) + self._data.projected_gravity_b = torch.zeros(self.count, 3, dtype=torch.float, device=self.device) + # dof states (redefined here to include base) + # ---- base + self._data.base_dof_pos = self._data.dof_pos[:, : self.base_num_dof] + self._data.base_dof_vel = self._data.dof_vel[:, : self.base_num_dof] + self._data.base_dof_acc = self._data.dof_acc[:, : self.base_num_dof] + # ----- arm + self._data.arm_dof_pos = self._data.dof_pos[:, self.base_num_dof : self.base_num_dof + self.arm_num_dof] + self._data.arm_dof_vel = self._data.dof_vel[:, self.base_num_dof : self.base_num_dof + self.arm_num_dof] + self._data.arm_dof_acc = self._data.dof_acc[:, self.base_num_dof : self.base_num_dof + self.arm_num_dof] + # ---- tool + self._data.tool_dof_pos = self._data.dof_pos[:, self.base_num_dof + self.arm_num_dof :] + self._data.tool_dof_vel = self._data.dof_vel[:, self.base_num_dof + self.arm_num_dof :] + self._data.tool_dof_acc = self._data.dof_acc[:, self.base_num_dof + self.arm_num_dof :] + + def _update_optional_buffers(self): + """Update buffers from articulation that are optional.""" + # Note: we implement this function here to allow inherited classes decide whether these + # quantities need to be updated similarly or not. + # -- dynamic state (note: base and tools don't contribute towards these quantities) + # jacobian + if self.cfg.data_info.enable_jacobian: + jacobians = self.articulations.get_jacobians(indices=self._ALL_INDICES, clone=False) + self._data.ee_jacobian[:] = jacobians[:, self.ee_body_index, :, self.base_num_dof : self.arm_num_dof] + # mass matrix + if self.cfg.data_info.enable_mass_matrix: + mass_matrices = self.articulations.get_mass_matrices(indices=self._ALL_INDICES, clone=False) + self._data.mass_matrix[:] = mass_matrices[ + :, self.base_num_dof : self.arm_num_dof, self.base_num_dof : self.arm_num_dof + ] + # coriolis + if self.cfg.data_info.enable_coriolis: + forces = self.articulations.get_coriolis_and_centrifugal_forces(indices=self._ALL_INDICES, clone=False) + self._data.coriolis[:] = forces[:, self.base_num_dof : self.arm_num_dof] + # gravity + if self.cfg.data_info.enable_gravity: + gravity = self.articulations.get_generalized_gravity_forces(indices=self._ALL_INDICES, clone=False) + self._data.gravity[:] = gravity[:, self.base_num_dof : self.arm_num_dof] + + +class LeggedMobileManipulator(MobileManipulator, LeggedRobot): + """Class for handling a legged mobile manipulator with a tool on it.""" + + cfg: LeggedMobileManipulatorCfg + """Configuration for the legged mobile manipulator.""" + + def __init__(self, cfg: LeggedMobileManipulatorCfg): + # initialize parent + super().__init__(cfg) + # container for data access + self._data = LeggedMobileManipulatorData() + + """ + Properties + """ + + @property + def data(self) -> LeggedMobileManipulatorData: + """Data related to articulation.""" + return self._data + + """ + Operations. + """ + + def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): + # spawn using parent + LeggedRobot.spawn(self, prim_path, translation, orientation) + + def initialize(self, prim_paths_expr: Optional[str] = None): + # reset parent buffers + LeggedRobot.initialize(self, prim_paths_expr) + MobileManipulator.initialize(self, prim_paths_expr) + + def reset_buffers(self, env_ids: Optional[Sequence[int]] = None): + # reset parent buffers + LeggedRobot.reset_buffers(self, env_ids) + MobileManipulator.reset_buffers(self, env_ids) + + def update_buffers(self, dt: float): + # update parent buffers + LeggedRobot.update_buffers(self, dt) + MobileManipulator.update_buffers(self, dt) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator_cfg.py new file mode 100644 index 0000000000..54a0759420 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator_cfg.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from omni.isaac.orbit.utils import configclass + +from ..legged_robot import LeggedRobotCfg +from ..single_arm import SingleArmManipulatorCfg + +__all__ = ["MobileManipulatorCfg", "LeggedMobileManipulatorCfg"] + + +@configclass +class MobileManipulatorCfg(SingleArmManipulatorCfg): + """Properties for a mobile manipulator.""" + + @configclass + class MetaInfoCfg(SingleArmManipulatorCfg.MetaInfoCfg): + """Meta-information about the mobile manipulator.""" + + base_num_dof: int = MISSING + """Number of degrees of freedom of base""" + + ## + # Initialize configurations. + ## + + meta_info: MetaInfoCfg = MetaInfoCfg() + """Meta-information about the mobile manipulator.""" + + +@configclass +class LeggedMobileManipulatorCfg(MobileManipulatorCfg, LeggedRobotCfg): + """Properties for a mobile manipulator.""" + + pass diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator_data.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator_data.py new file mode 100644 index 0000000000..eb486f685d --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/mobile_manipulator/mobile_manipulator_data.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + +from ..legged_robot import LeggedRobotData +from ..single_arm import SingleArmManipulatorData + +__all__ = ["MobileManipulatorData", "LeggedMobileManipulatorData"] + + +@dataclass +class MobileManipulatorData(SingleArmManipulatorData): + """Data container for a mobile manipulator with an optional gripper/tool.""" + + ## + # Frame states. + ## + + root_vel_b: torch.Tensor = None + """Root velocity `[lin_vel, ang_vel]` in base frame. Shape is ``(count, 6)``.""" + + projected_gravity_b: torch.Tensor = None + """Projection of the gravity direction on base frame. Shape is ``(count, 3)``.""" + + ## + # DOF states. + ## + + base_dof_pos: torch.Tensor = None + """Base positions. Shape is ``(count, base_num_dof)``.""" + + base_dof_vel: torch.Tensor = None + """Base velocities. Shape is ``(count, base_num_dof)``.""" + + base_dof_acc: torch.Tensor = None + """Base acceleration. Shape is ``(count, base_num_dof)``.""" + + """ + Properties + """ + + @property + def root_lin_vel_b(self) -> torch.Tensor: + """Root linear velocity in base frame. Shape is ``(count, 3)``.""" + return self.root_vel_b[:, 0:3] + + @property + def root_ang_vel_b(self) -> torch.Tensor: + """Root angular velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_vel_b[:, 3:6] + + +@dataclass +class LeggedMobileManipulatorData(MobileManipulatorData, LeggedRobotData): + """Data container for a legged mobile manipulator with an optional gripper/tool.""" + + pass diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py new file mode 100644 index 0000000000..fc516ff855 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py @@ -0,0 +1,506 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import re +import torch +from typing import Dict, List, Optional, Sequence, Tuple, Union + +import carb +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.articulations import ArticulationView + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.actuators.group import * # noqa: F403, F401 +from omni.isaac.orbit.actuators.group import ActuatorGroup +from omni.isaac.orbit.utils.math import sample_uniform + +from .robot_base_cfg import RobotBaseCfg +from .robot_base_data import RobotBaseData + + +class RobotBase: + """Base class for robots. + + The robot class is strictly treated as the physical articulated system which typically + runs at a high frequency (most commonly 1000 Hz). + + This class wraps around :class:`ArticulationView` class from Isaac Sim to support the following: + * Configuring using a single dataclass (struct). + * Handling different rigid body views inside the robot. + * Handling different actuator groups and models for the robot. + """ + + cfg: RobotBaseCfg + """Configuration for the robot.""" + articulations: ArticulationView = None + """Articulation view of the robot.""" + actuator_groups: Dict[str, ActuatorGroup] + """Mapping between actuator group names and instance.""" + + def __init__(self, cfg: RobotBaseCfg): + """Initialize the robot class. + + Args: + cfg (RobotBaseCfg): The configuration instance. + """ + # store inputs + self.cfg = cfg + # container for data access + self._data = RobotBaseData() + + # Flag to check that instance is spawned. + self._is_spawned = False + # data for storing actuator group + self.actuator_groups = dict.fromkeys(self.cfg.actuator_groups.keys()) + + """ + Properties + """ + + @property + def count(self) -> int: + """Number of prims encapsulated.""" + return self.articulations.count + + @property + def device(self) -> str: + """Memory device for computation.""" + return self.articulations._device + + @property + def body_names(self) -> List[str]: + """Ordered names of links/bodies in articulation.""" + return self.articulations.body_names + + @property + def dof_names(self) -> List[str]: + """Ordered names of DOFs in articulation.""" + return self.articulations.dof_names + + @property + def num_dof(self) -> int: + """Total number of DOFs in articulation.""" + return self.articulations.num_dof + + @property + def num_actions(self) -> int: + """Dimension of the actions applied.""" + return sum(group.control_dim for group in self.actuator_groups.values()) + + @property + def data(self) -> RobotBaseData: + """Data related to articulation.""" + return self._data + + """ + Operations. + """ + + def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): + """Spawn a robot into the stage (loaded from its USD file). + + Note: + If inputs `translation` or `orientation` are not :obj:`None`, then they override the initial root + state specified through the configuration class at spawning. + + Args: + prim_path (str): The prim path for spawning robot at. + translation (Sequence[float], optional): The local position of prim from its parent. Defaults to None. + orientation (Sequence[float], optional): The local rotation (as quaternion `(w, x, y, z)` + of the prim from its parent. Defaults to None. + """ + # use default arguments + if translation is None: + translation = self.cfg.init_state.pos + if orientation is None: + orientation = self.cfg.init_state.rot + + # -- save prim path for later + self._spawn_prim_path = prim_path + # -- spawn asset if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + # add prim as reference to stage + prim_utils.create_prim( + self._spawn_prim_path, + usd_path=self.cfg.meta_info.usd_path, + translation=translation, + orientation=orientation, + ) + else: + carb.log_warn(f"A prim already exists at prim path: '{prim_path}'. Skipping...") + + # apply rigid body properties + kit_utils.set_nested_rigid_body_properties( + prim_path, + linear_damping=self.cfg.rigid_props.linear_damping, + angular_damping=self.cfg.rigid_props.angular_damping, + max_linear_velocity=self.cfg.rigid_props.max_linear_velocity, + max_angular_velocity=self.cfg.rigid_props.max_angular_velocity, + max_depenetration_velocity=self.cfg.rigid_props.max_depenetration_velocity, + disable_gravity=self.cfg.rigid_props.disable_gravity, + retain_accelerations=self.cfg.rigid_props.retain_accelerations, + ) + # articulation root settings + kit_utils.set_articulation_properties( + prim_path, + enable_self_collisions=self.cfg.articulation_props.enable_self_collisions, + solver_position_iteration_count=self.cfg.articulation_props.solver_position_iteration_count, + solver_velocity_iteration_count=self.cfg.articulation_props.solver_velocity_iteration_count, + ) + # set spawned to true + self._is_spawned = True + + def initialize(self, prim_paths_expr: Optional[str] = None): + """Initializes the PhysX handles and internal buffers. + + Note: + PhysX handles are only enabled once the simulator starts playing. Hence, this function needs to be + called whenever the simulator "plays" from a "stop" state. + + Args: + prim_paths_expr (Optional[str], optional): The prim path expression for robot prims. Defaults to None. + + Raises: + RuntimeError: When input `prim_paths_expr` is :obj:`None`, the method defaults to using the last + prim path set when calling the :meth:`spawn()` function. In case, the robot was not spawned + and no valid `prim_paths_expr` is provided, the function throws an error. + """ + # default prim path if not cloned + if prim_paths_expr is None: + if self._is_spawned is not None: + self._prim_paths_expr = self._spawn_prim_path + else: + raise RuntimeError( + "Initialize the robot failed! Please provide a valid argument for `prim_paths_expr`." + ) + else: + self._prim_paths_expr = prim_paths_expr + # create handles + # -- robot articulation + self.articulations = ArticulationView(self._prim_paths_expr, reset_xform_properties=False) + self.articulations.initialize() + # set the default state + self.articulations.post_reset() + # set properties over all instances + # -- meta-information + self._process_info_cfg() + # -- actuation properties + self._process_actuators_cfg() + # create buffers + self._create_buffers() + + def reset_buffers(self, env_ids: Optional[Sequence[int]] = None): + """Resets all internal buffers. + + Args: + env_ids (Optional[Sequence[int]], optional): The indices of the robot to reset. + Defaults to None (all instances). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + # reset history + self._previous_dof_vel[env_ids] = 0 + # TODO: Reset other cached variables. + # reset actuators + for group in self.actuator_groups.values(): + group.reset(env_ids) + + def apply_action(self, actions: torch.Tensor): + """Apply the input action for the robot into the simulator. + + The actions are first processed using actuator groups. Depending on the robot configuration, + the groups compute the joint level simulation commands and sets them into the PhysX buffers. + + Args: + actions (torch.Tensor): The input actions to apply. + """ + # slice actions per actuator group + group_actions_dims = [group.control_dim for group in self.actuator_groups.values()] + all_group_actions = torch.split(actions, group_actions_dims, dim=-1) + # silence the physics sim for warnings that make no sense :) + self.articulations._physics_sim_view.enable_warnings(False) + # note (18.08.2022): Saw a difference of up to 5 ms per step when using Isaac Sim + # interfaces compared to direct PhysX calls. Thus, this function is optimized. + # acquire tensors for whole robot + # note: we use internal buffers to deal with the resets() as the buffers aren't forwarded + # unit the next simulation step. + dof_pos = self._data.dof_pos + dof_vel = self._data.dof_vel + # process actions per group + for group, group_actions in zip(self.actuator_groups.values(), all_group_actions): + # compute group dof command + control_action = group.compute( + group_actions, dof_pos=dof_pos[:, group.dof_indices], dof_vel=dof_vel[:, group.dof_indices] + ) + # update targets + if control_action.joint_positions is not None: + self._data.dof_pos_targets[:, group.dof_indices] = control_action.joint_positions + if control_action.joint_velocities is not None: + self._data.dof_vel_targets[:, group.dof_indices] = control_action.joint_velocities + if control_action.joint_efforts is not None: + self._data.dof_effort_targets[:, group.dof_indices] = control_action.joint_efforts + # update state + # -- torques + self._data.computed_torques[:, group.dof_indices] = group.computed_torques + self._data.applied_torques[:, group.dof_indices] = group.applied_torques + # -- actuator data + self._data.gear_ratio[:, group.dof_indices] = group.gear_ratio + if group.velocity_limit is not None: + self._data.soft_dof_vel_limits[:, group.dof_indices] = group.velocity_limit + # apply actions into sim + if self.sim_dof_control_modes["position"]: + self.articulations._physics_view.set_dof_position_targets(self._data.dof_pos_targets, self._ALL_INDICES) + if self.sim_dof_control_modes["velocity"]: + self.articulations._physics_view.set_dof_velocity_targets(self._data.dof_vel_targets, self._ALL_INDICES) + if self.sim_dof_control_modes["effort"]: + self.articulations._physics_view.set_dof_actuation_forces(self._data.dof_effort_targets, self._ALL_INDICES) + # enable warnings for other unsafe operations ;) + self.articulations._physics_sim_view.enable_warnings(True) + + def update_buffers(self, dt: float): + """Update the internal buffers. + + Args: + dt (float): The amount of time passed from last `update_buffers` call. + + This is used to compute numerical derivatives of quantities such as joint accelerations + which are not provided by the simulator. + """ + # TODO: Make function independent of `dt` by using internal clocks that check the rate of `apply_action()` call. + # frame states + # -- root frame in world + position_w, quat_w = self.articulations.get_world_poses(indices=self._ALL_INDICES, clone=False) + self._data.root_state_w[:, 0:3] = position_w + self._data.root_state_w[:, 3:7] = quat_w + self._data.root_state_w[:, 7:] = self.articulations.get_velocities(indices=self._ALL_INDICES, clone=False) + # -- dof states + self._data.dof_pos[:] = self.articulations.get_joint_positions(indices=self._ALL_INDICES, clone=False) + self._data.dof_vel[:] = self.articulations.get_joint_velocities(indices=self._ALL_INDICES, clone=False) + self._data.dof_acc[:] = (self._data.dof_vel - self._previous_dof_vel) / dt + # update history buffers + self._previous_dof_vel[:] = self._data.dof_vel[:] + + """ + Operations - State. + """ + + def set_root_state(self, root_states: torch.Tensor, env_ids: Optional[Sequence[int]] = None): + """Sets the root state (pose and velocity) of the actor over selected environment indices. + + Args: + root_states (torch.Tensor): Input root state for the actor, shape: (len(env_ids), 13). + env_ids (Optional[Sequence[int]]): Environment indices. + If :obj:`None`, then all indices are used. + """ + # resolve all indices + if env_ids is None: + env_ids = self._ALL_INDICES + # set into simulation + self.articulations.set_world_poses(root_states[:, 0:3], root_states[:, 3:7], indices=env_ids) + self.articulations.set_velocities(root_states[:, 7:], indices=env_ids) + + # TODO: Move these to reset_buffers call. + # 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] = root_states.clone() + + def get_default_root_state(self, env_ids: Optional[Sequence[int]] = None, clone=True) -> torch.Tensor: + """Returns the default/initial root state of actor. + + Args: + env_ids (Optional[Sequence[int]], optional): Environment indices. + Defaults to None (all environment indices). + clone (bool, optional): Whether to return a copy or not. Defaults to True. + + Returns: + torch.Tensor: The default/initial root state of the actor, shape: (len(env_ids), 13). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + # return copy + if clone: + return torch.clone(self._default_root_states[env_ids]) + else: + return self._default_root_states[env_ids] + + def set_dof_state(self, dof_pos: torch.Tensor, dof_vel: torch.Tensor, env_ids: Optional[Sequence[int]] = None): + """Sets the DOF state (position and velocity) of the actor over selected environment indices. + + Args: + dof_pos (torch.Tensor): Input DOF position for the actor, shape: (len(env_ids), 1). + dof_vel (torch.Tensor): Input DOF velocity for the actor, shape: (len(env_ids), 1). + env_ids (torch.Tensor): Environment indices. + If :obj:`None`, then all indices are used. + """ + # resolve all indices + if env_ids is None: + env_ids = self._ALL_INDICES + # set into simulation + self.articulations.set_joint_positions(dof_pos, indices=env_ids) + self.articulations.set_joint_velocities(dof_vel, indices=env_ids) + + # TODO: Move these to reset_buffers call. + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.dof_pos[env_ids] = dof_pos.clone() + self._data.dof_vel[env_ids] = dof_vel.clone() + self._data.dof_acc[env_ids] = 0.0 + + def get_default_dof_state( + self, env_ids: Optional[Sequence[int]] = None, clone=True + ) -> Tuple[torch.Tensor, torch.Tensor]: + """Returns the default/initial DOF state (position and velocity) of actor. + + Args: + env_ids (Optional[Sequence[int]], optional): Environment indices. + Defaults to None (all environment indices). + clone (bool, optional): Whether to return a copy or not. Defaults to True. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: The default/initial DOF position and velocity of the actor. + Each tensor has shape: (len(env_ids), 1). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + # return copy + if clone: + return torch.clone(self._default_dof_pos[env_ids]), torch.clone(self._default_dof_vel[env_ids]) + else: + return self._default_dof_pos[env_ids], self._default_dof_vel[env_ids] + + def get_random_dof_state( + self, + env_ids: Optional[Sequence[int]] = None, + lower: Union[float, torch.Tensor] = 0.5, + upper: Union[float, torch.Tensor] = 1.5, + ) -> Tuple[torch.Tensor, torch.Tensor]: + """Returns randomly sampled DOF state (position and velocity) of actor. + + Currently, the following sampling is supported: + + - DOF positions: + + - uniform sampling between `(lower, upper)` times the default DOF position. + + - DOF velocities: + + - zero. + + Args: + env_ids (Optional[Sequence[int]], optional): Environment indices. + Defaults to None (all environment indices). + lower (Union[float, torch.Tensor], optional): Minimum value for uniform sampling. Defaults to 0.5. + upper (Union[float, torch.Tensor], optional): Maximum value for uniform sampling. Defaults to 1.5. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: The sampled DOF position and velocity of the actor. + Each tensor has shape: (len(env_ids), 1). + """ + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = ... + actor_count = self.count + else: + actor_count = len(env_ids) + # sample DOF position + dof_pos = self._default_dof_pos[env_ids] * sample_uniform( + lower, upper, (actor_count, self.num_dof), device=self.device + ) + dof_vel = self._default_dof_vel[env_ids] + # return sampled dof state + return dof_pos, dof_vel + + """ + Internal helper. + """ + + def _process_info_cfg(self) -> None: + """Post processing of configuration parameters.""" + # default state + # -- root state + default_root_state = ( + self.cfg.init_state.pos + + self.cfg.init_state.rot + + self.cfg.init_state.lin_vel + + self.cfg.init_state.ang_vel + ) + self._default_root_states = torch.tensor(default_root_state, device=self.device).repeat(self.count, 1) + # -- dof state + self._default_dof_pos = torch.zeros(self.count, self.num_dof, device=self.device) + self._default_dof_vel = torch.zeros(self.count, self.num_dof, device=self.device) + for index, dof_name in enumerate(self.articulations.dof_names): + # dof pos + for re_key, value in self.cfg.init_state.dof_pos.items(): + if re.match(re_key, dof_name): + self._default_dof_pos[:, index] = value + # dof vel + for re_key, value in self.cfg.init_state.dof_vel.items(): + if re.match(re_key, dof_name): + self._default_dof_vel[:, index] = value + + def _process_actuators_cfg(self): + """Process and apply articulation DOF properties.""" + # sim control mode and dof indices (optimization) + self.sim_dof_control_modes = {"position": list(), "velocity": list(), "effort": list()} + # iterate over all actuator configuration + for group_name, group_cfg in self.cfg.actuator_groups.items(): + # create actuator group + actuator_group_cls = eval(group_cfg.cls_name) + actuator_group: ActuatorGroup = actuator_group_cls(cfg=group_cfg, view=self.articulations) + # store actuator group + self.actuator_groups[group_name] = actuator_group + # store the control mode and dof indices (optimization) + self.sim_dof_control_modes[actuator_group.control_mode].extend(actuator_group.dof_indices) + + # perform some sanity checks to ensure actuators are prepared correctly + total_act_dof = sum(group.num_actuators for group in self.actuator_groups.values()) + if total_act_dof != self.num_dof: + carb.log_warn( + f"Not all actuators are configured through groups! Total number of actuated DOFs not equal to number of DOFs available: {total_act_dof} != {self.num_dof}." + ) + + def _create_buffers(self): + """Create buffers for storing data.""" + # history buffers + self._previous_dof_vel = torch.zeros(self.count, self.num_dof, dtype=torch.float, device=self.device) + # constants + self._ALL_INDICES = torch.arange(self.count, dtype=torch.long, device=self.device) + + # -- frame states + self._data.root_state_w = torch.zeros(self.count, 13, dtype=torch.float, device=self.device) + # -- dof states + self._data.dof_pos = torch.zeros(self.count, self.num_dof, dtype=torch.float, device=self.device) + self._data.dof_vel = torch.zeros_like(self._data.dof_pos) + self._data.dof_acc = torch.zeros_like(self._data.dof_pos) + # -- dof commands + self._data.dof_pos_targets = torch.zeros_like(self._data.dof_pos) + self._data.dof_vel_targets = torch.zeros_like(self._data.dof_pos) + self._data.dof_effort_targets = torch.zeros_like(self._data.dof_pos) + # -- dof commands (explicit) + self._data.computed_torques = torch.zeros_like(self._data.dof_pos) + self._data.applied_torques = torch.zeros_like(self._data.dof_pos) + # -- default actuator offset + self._data.actuator_pos_offset = torch.zeros_like(self._data.dof_pos) + self._data.actuator_vel_offset = torch.zeros_like(self._data.dof_pos) + # -- other data + self._data.soft_dof_pos_limits = torch.zeros(self.count, self.num_dof, 2, dtype=torch.float, device=self.device) + self._data.soft_dof_vel_limits = torch.zeros(self.count, self.num_dof, dtype=torch.float, device=self.device) + self._data.gear_ratio = torch.ones(self.count, self.num_dof, dtype=torch.float, device=self.device) + + # soft dof position limits (recommended not to be too close to limits). + dof_pos_limits = self.articulations.get_dof_limits() + dof_pos_mean = (dof_pos_limits[..., 0] + dof_pos_limits[..., 1]) / 2 + dof_pos_range = dof_pos_limits[..., 1] - dof_pos_limits[..., 0] + soft_limit_factor = self.cfg.meta_info.soft_dof_pos_limit_factor + # add to data + self._data.soft_dof_pos_limits[..., 0] = dof_pos_mean - 0.5 * dof_pos_range * soft_limit_factor + self._data.soft_dof_pos_limits[..., 1] = dof_pos_mean + 0.5 * dof_pos_range * soft_limit_factor + + # store the offset amounts from actuator groups + for group in self.actuator_groups.values(): + self._data.actuator_pos_offset[:, group.dof_indices] = group.dof_pos_offset diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py new file mode 100644 index 0000000000..bc7fd670df --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py @@ -0,0 +1,91 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Dict, Optional, Tuple + +from omni.isaac.orbit.actuators.group import ActuatorGroupCfg +from omni.isaac.orbit.utils import configclass + + +@configclass +class RobotBaseCfg: + """Configuration parameters for a robot.""" + + @configclass + class MetaInfoCfg: + """Meta-information about the manipulator.""" + + usd_path: str = MISSING + """USD file to spawn asset from.""" + soft_dof_pos_limit_factor: float = 1.0 + """Fraction specifying the range of DOF position limits (parsed from the asset) to use. + Defaults to 1.0.""" + + @configclass + class RigidBodyPropertiesCfg: + """Properties to apply to all rigid bodies in the articulation.""" + + linear_damping: Optional[float] = None + """Linear damping coefficient.""" + angular_damping: Optional[float] = None + """Angular damping coefficient.""" + max_linear_velocity: Optional[float] = 1000.0 + """Maximum linear velocity for rigid bodies. Defaults to 1000.0.""" + max_angular_velocity: Optional[float] = 1000.0 + """Maximum angular velocity for rigid bodies. Defaults to 1000.0.""" + max_depenetration_velocity: Optional[float] = 10.0 + """Maximum depenetration velocity permitted to be introduced by the solver. Defaults to 10.0.""" + disable_gravity: Optional[bool] = False + """Disable gravity for the actor. Defaults to False.""" + retain_accelerations: Optional[bool] = None + """Carries over forces/accelerations over sub-steps.""" + + @configclass + class ArticulationRootPropertiesCfg: + """Properties to apply to articulation.""" + + enable_self_collisions: Optional[bool] = None + """Whether to enable or disable self-collisions.""" + solver_position_iteration_count: Optional[int] = None + """Solver position iteration counts for the body.""" + solver_velocity_iteration_count: Optional[int] = None + """Solver position iteration counts for the body.""" + + @configclass + class InitialStateCfg: + """Initial state of the robot.""" + + # root state + pos: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + rot: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation ``(w, x, y, z)`` of the root in simulation world frame. + Defaults to (1.0, 0.0, 0.0, 0.0). + """ + lin_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Linear velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + ang_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Angular velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + # dof state + dof_pos: Dict[str, float] = MISSING + """DOF positions of all joints.""" + dof_vel: Dict[str, float] = MISSING + """DOF velocities of all joints.""" + + ## + # Initialize configurations. + ## + + meta_info: MetaInfoCfg = MetaInfoCfg() + """Meta-information about the robot.""" + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the robot.""" + rigid_props: RigidBodyPropertiesCfg = RigidBodyPropertiesCfg() + """Properties to apply to all rigid bodies in the articulation.""" + articulation_props: ArticulationRootPropertiesCfg = ArticulationRootPropertiesCfg() + """Properties to apply to articulation.""" + actuator_groups: Dict[str, ActuatorGroupCfg] = MISSING + """Actuator groups for the robot.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_data.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_data.py new file mode 100644 index 0000000000..b0e52174d9 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_data.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + + +@dataclass +class RobotBaseData: + """Data container for a robot.""" + + ## + # Frame states. + ## + + root_state_w: torch.Tensor = None + """Root state `[pos, quat, lin_vel, ang_vel]` in simulation world frame. Shape is ``(count, 13)``.""" + + ## + # DOF states <- From simulation. + ## + + dof_pos: torch.Tensor = None + """DOF positions of all joints. Shape is ``(count, num_dof)``.""" + + dof_vel: torch.Tensor = None + """DOF velocities of all joints. Shape is ``(count, num_dof)``.""" + + dof_acc: torch.Tensor = None + """DOF acceleration of all joints. Shape is ``(count, num_dof)``.""" + + ## + # DOF commands -- Set into simulation. + ## + + dof_pos_targets: torch.Tensor = None + """DOF position targets provided to simulation. Shape is ``(count, num_dof)``. + + Note: The position targets are zero for explicit actuator models. + """ + + dof_vel_targets: torch.Tensor = None + """DOF velocity targets provided to simulation. Shape is ``(count, num_dof)``. + + Note: The velocity targets are zero for explicit actuator models. + """ + + dof_effort_targets: torch.Tensor = None + """DOF effort targets provided to simulation. Shape is ``(count, num_dof)``. + + Note: The torques are zero for implicit actuator models without feed-forward torque. + """ + + ## + # DOF commands -- Explicit actuators. + ## + + computed_torques: torch.Tensor = None + """DOF torques computed from the actuator model (before clipping). + Shape is ``(count, num_dof)``. + + Note: The torques are zero for implicit actuator models. + """ + + applied_torques: torch.Tensor = None + """DOF torques applied from the actuator model (after clipping). + Shape is ``(count, num_dof)``. + + Note: The torques are zero for implicit actuator models. + """ + + ## + # Default actuator offsets <- From the actuator groups. + ## + + actuator_pos_offset: torch.Tensor = None + """Joint positions offsets applied by actuators when using "p_abs". Shape is ``(count, num_dof)``.""" + + ## + # Other Data. + ## + + soft_dof_pos_limits: torch.Tensor = None + """DOF positions limits for all joints. Shape is ``(count, num_dof, 2)``.""" + + soft_dof_vel_limits: torch.Tensor = None + """DOF velocity limits for all joints. Shape is ``(count, num_dof)``.""" + + gear_ratio: torch.Tensor = None + """Gear ratio for relating motor torques to applied DOF torques. Shape is ``(count, num_dof)``.""" + + """ + Properties + """ + + @property + def root_pos_w(self) -> torch.Tensor: + """Root position in simulation world frame. Shape is ``(count, 3)``.""" + 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 ``(count, 4)``.""" + return self.root_state_w[:, 3:7] + + @property + def root_lin_vel_w(self) -> torch.Tensor: + """Root linear velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_state_w[:, 7:10] + + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Root angular velocity in simulation world frame. Shape is ``(count, 3)``.""" + return self.root_state_w[:, 10:13] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/__init__.py new file mode 100644 index 0000000000..7c4929bc63 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for handling fixed-arm manipulators.""" + +from .single_arm import SingleArmManipulator +from .single_arm_cfg import SingleArmManipulatorCfg +from .single_arm_data import SingleArmManipulatorData + +__all__ = ["SingleArmManipulator", "SingleArmManipulatorCfg", "SingleArmManipulatorData"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm.py new file mode 100644 index 0000000000..36bee27901 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm.py @@ -0,0 +1,222 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import re +import torch +from typing import Dict, Optional + +from omni.isaac.core.prims import RigidPrimView + +from omni.isaac.orbit.utils.math import combine_frame_transforms, quat_rotate_inverse, subtract_frame_transforms + +from ..robot_base import RobotBase +from .single_arm_cfg import SingleArmManipulatorCfg +from .single_arm_data import SingleArmManipulatorData + + +class SingleArmManipulator(RobotBase): + """Class for handling a fixed-base robot arm with a tool on it.""" + + cfg: SingleArmManipulatorCfg + """Configuration for the robot.""" + ee_parent_body: RigidPrimView + """Rigid body view for the end-effector parent body.""" + tool_site_bodies: Dict[str, RigidPrimView] + """Rigid body views for the tool sites. + + Dictionary with keys as the site names and values as the corresponding rigid body view + in the articulated object. + """ + + def __init__(self, cfg: SingleArmManipulatorCfg): + # initialize parent + super().__init__(cfg) + # container for data access + self._data = SingleArmManipulatorData() + + """ + Properties + """ + + @property + def arm_num_dof(self) -> int: + """Number of DOFs in the robot arm.""" + return self.cfg.meta_info.arm_num_dof + + @property + def tool_num_dof(self) -> int: + """Number of DOFs in the robot tool/gripper.""" + return self.cfg.meta_info.tool_num_dof + + @property + def data(self) -> SingleArmManipulatorData: + """Data related to articulation.""" + return self._data + + """ + Operations. + """ + + def initialize(self, prim_paths_expr: Optional[str] = None): + # initialize parent handles + super().initialize(prim_paths_expr) + # create handles + # -- ee frame + self.ee_parent_body = RigidPrimView( + prim_paths_expr=f"{self._prim_paths_expr}/{self.cfg.ee_info.body_name}", reset_xform_properties=False + ) + self.ee_parent_body.initialize() + # -- gripper + if self.tool_sites_indices is not None: + self.tool_site_bodies: Dict[str, RigidPrimView] = dict() + for name in self.tool_sites_indices: + # create rigid body view to track + site_body = RigidPrimView( + prim_paths_expr=f"{self._prim_paths_expr}/{name}", reset_xform_properties=False + ) + site_body.initialize() + # add to list + self.tool_site_bodies[name] = site_body + else: + self.tool_site_bodies = None + + def update_buffers(self, dt: float): + # update parent buffers + super().update_buffers(dt) + # frame states + # -- ee frame in world: world -> hand frame -> ee frame + hand_position_w, hand_quat_w = self.ee_parent_body.get_world_poses(indices=self._ALL_INDICES, clone=False) + position_w, quat_w = combine_frame_transforms( + hand_position_w, hand_quat_w, self._ee_pos_offset, self._ee_rot_offset + ) + self._data.ee_state_w[:, 0:3] = position_w + self._data.ee_state_w[:, 3:7] = quat_w + # TODO: Transformation velocities from hand to end-effector? + self._data.ee_state_w[:, 7:] = self.ee_parent_body.get_velocities(indices=self._ALL_INDICES, clone=False) + # ee frame in body + position_b, quat_b = subtract_frame_transforms( + self._data.root_state_w[:, 0:3], + self._data.root_state_w[:, 3:7], + self._data.ee_state_w[:, 0:3], + self._data.ee_state_w[:, 3:7], + ) + self._data.ee_state_b[:, 0:3] = position_b + self._data.ee_state_b[:, 3:7] = quat_b + self._data.ee_state_b[:, 7:10] = quat_rotate_inverse(self._data.root_quat_w, self._data.ee_state_w[:, 7:10]) + self._data.ee_state_b[:, 10:13] = quat_rotate_inverse(self._data.root_quat_w, self._data.ee_state_w[:, 10:13]) + # -- tool sites + # TODO: This can be sped up by combining all tool sites into one regex expression. + if self.tool_site_bodies is not None: + for index, body in enumerate(self.tool_site_bodies.values()): + # world frame + position_w, quat_w = body.get_world_poses(indices=self._ALL_INDICES, clone=False) + self._data.tool_sites_state_w[:, index, 0:3] = position_w + self._data.tool_sites_state_w[:, index, 3:7] = quat_w + self._data.tool_sites_state_w[:, index, 7:] = body.get_velocities( + indices=self._ALL_INDICES, clone=False + ) + # base frame + position_b, quat_b = subtract_frame_transforms( + self._data.root_state_w[:, 0:3], + self._data.root_state_w[:, 3:7], + self._data.tool_sites_state_w[:, index, 0:3], + self._data.tool_sites_state_w[:, index, 3:7], + ) + self._data.tool_sites_state_b[:, index, 0:3] = position_b + self._data.tool_sites_state_b[:, index, 3:7] = quat_b + self._data.tool_sites_state_b[:, index, 7:10] = quat_rotate_inverse( + self._data.root_quat_w, self._data.tool_sites_state_w[:, index, 7:10] + ) + self._data.tool_sites_state_b[:, index, 10:13] = quat_rotate_inverse( + self._data.root_quat_w, self._data.tool_sites_state_w[:, index, 10:13] + ) + # update optional data + self._update_optional_buffers() + + """ + Internal helpers - Override. + """ + + def _process_info_cfg(self) -> None: + """Post processing of configuration parameters.""" + # process parent config + super()._process_info_cfg() + # resolve regex expressions for indices + # -- end-effector body + self.ee_body_index = None + for body_index, body_name in enumerate(self.body_names): + if re.fullmatch(self.cfg.ee_info.body_name, body_name): + self.ee_body_index = body_index + # -- tool sites + if self.cfg.meta_info.tool_sites_names: + tool_sites_names = list() + tool_sites_indices = list() + for body_index, body_name in enumerate(self.body_names): + for re_key in self.cfg.meta_info.tool_sites_names: + if re.fullmatch(re_key, body_name): + tool_sites_names.append(body_name) + tool_sites_indices.append(body_index) + self.tool_sites_indices: Dict[str, int] = dict(zip(tool_sites_names, tool_sites_indices)) + else: + self.tool_sites_indices = None + # end-effector offsets + # -- position + ee_pos_offset = torch.tensor(self.cfg.ee_info.pos_offset, device=self.device) + self._ee_pos_offset = ee_pos_offset.repeat(self.count, 1) + # -- orientation + ee_rot_offset = torch.tensor(self.cfg.ee_info.rot_offset, device=self.device) + self._ee_rot_offset = ee_rot_offset.repeat(self.count, 1) + + def _create_buffers(self): + """Create buffers for storing data.""" + # process parent buffers + super()._create_buffers() + + # -- frame states + self._data.ee_state_w = torch.zeros_like(self._data.root_state_w) + self._data.ee_state_b = torch.zeros_like(self._data.root_state_w) + if self.tool_sites_indices is not None: + self._data.tool_sites_state_w = torch.zeros( + self.count, len(self.tool_sites_indices), 13, device=self.device + ) + self._data.tool_sites_state_b = torch.zeros_like(self._data.tool_sites_state_w) + # -- dof states + self._data.arm_dof_pos = self._data.dof_pos[:, : self.arm_num_dof] + self._data.arm_dof_vel = self._data.dof_vel[:, : self.arm_num_dof] + self._data.arm_dof_acc = self._data.dof_acc[:, : self.arm_num_dof] + self._data.tool_dof_pos = self._data.dof_pos[:, self.arm_num_dof :] + self._data.tool_dof_vel = self._data.dof_vel[:, self.arm_num_dof :] + self._data.tool_dof_acc = self._data.dof_acc[:, self.arm_num_dof :] + # -- dynamic states + if self.cfg.data_info.enable_jacobian: + self._data.ee_jacobian = torch.zeros(self.count, 6, self.arm_num_dof, device=self.device) + if self.cfg.data_info.enable_mass_matrix: + self._data.mass_matrix = torch.zeros(self.count, self.arm_num_dof, self.arm_num_dof, device=self.device) + if self.cfg.data_info.enable_coriolis: + self._data.coriolis = torch.zeros(self.count, self.arm_num_dof, device=self.device) + if self.cfg.data_info.enable_gravity: + self._data.gravity = torch.zeros(self.count, self.arm_num_dof, device=self.device) + + def _update_optional_buffers(self): + """Update buffers from articulation that are optional.""" + # Note: we implement this function here to allow inherited classes decide whether these + # quantities need to be updated similarly or not. + # -- dynamic state (note: tools don't contribute towards these quantities) + # jacobian + if self.cfg.data_info.enable_jacobian: + jacobians = self.articulations.get_jacobians(indices=self._ALL_INDICES, clone=False) + self._data.ee_jacobian[:] = jacobians[:, self.ee_body_index, :, : self.arm_num_dof] + # mass matrix + if self.cfg.data_info.enable_mass_matrix: + mass_matrices = self.articulations.get_mass_matrices(indices=self._ALL_INDICES, clone=False) + self._data.mass_matrix[:] = mass_matrices[:, : self.arm_num_dof, : self.arm_num_dof] + # coriolis + if self.cfg.data_info.enable_coriolis: + forces = self.articulations.get_coriolis_and_centrifugal_forces(indices=self._ALL_INDICES, clone=False) + self._data.coriolis[:] = forces[:, : self.arm_num_dof] + # gravity + if self.cfg.data_info.enable_gravity: + gravity = self.articulations.get_generalized_gravity_forces(indices=self._ALL_INDICES, clone=False) + self._data.gravity[:] = gravity[:, : self.arm_num_dof] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_cfg.py new file mode 100644 index 0000000000..58ab933210 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_cfg.py @@ -0,0 +1,68 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Optional, Sequence, Tuple + +from omni.isaac.orbit.utils import configclass + +from ..robot_base_cfg import RobotBaseCfg + + +@configclass +class SingleArmManipulatorCfg(RobotBaseCfg): + """Properties for a single arm manipulator.""" + + @configclass + class MetaInfoCfg(RobotBaseCfg.MetaInfoCfg): + """Meta-information about the manipulator.""" + + arm_num_dof: int = MISSING + """Number of degrees of freedom of arm.""" + tool_num_dof: int = MISSING + """Number of degrees of freedom of tool.""" + tool_sites_names: Optional[Sequence[str]] = None + """Name of the tool sites to track (added to :obj:`data`). Defaults to :obj:`None`. + + If :obj:`None`, then no tool sites are tracked. The returned tensor for tool sites state + is in the same order as that of the provided list. + """ + + @configclass + class EndEffectorFrameCfg: + """Information about the end-effector frame location.""" + + body_name: str = MISSING + """Name of the body corresponding to the end-effector.""" + pos_offset: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Additional position offset from the body frame. Defaults to (0, 0, 0).""" + rot_offset: Tuple[float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Additional rotation offset ``(w, x, y, z)`` from the body frame. Defaults to (1, 0, 0, 0).""" + + class DataInfoCfg: + """Information about what all data to read from simulator. + + Note: Setting all of them to true leads to an overhead of 10-15%. + """ + + enable_jacobian: bool = False + """Fill in jacobian for the end-effector into data buffers. Defaults to False.""" + enable_mass_matrix: bool = False + """Fill in mass matrix into data buffers. Defaults to False.""" + enable_coriolis: bool = False + """Fill in coriolis and centrifugal forces into data buffers. Defaults to False.""" + enable_gravity: bool = False + """Fill in generalized gravity forces into data buffers. Defaults to False.""" + + ## + # Initialize configurations. + ## + + meta_info: MetaInfoCfg = MetaInfoCfg() + """Meta-information about the manipulator.""" + ee_info: EndEffectorFrameCfg = EndEffectorFrameCfg() + """Information about the end-effector frame location.""" + data_info: DataInfoCfg = DataInfoCfg() + """Information about what all data to read from simulator.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_data.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_data.py new file mode 100644 index 0000000000..7290dd4dfc --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_data.py @@ -0,0 +1,68 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + +from ..robot_base_data import RobotBaseData + + +@dataclass +class SingleArmManipulatorData(RobotBaseData): + """Data container for a robot arm with an optional gripper/tool.""" + + ## + # Frame states. + ## + + ee_state_w: torch.Tensor = None + """End-effector frame state `[pos, quat, lin_vel, ang_vel]` in simulation world frame. Shape: (count, 13).""" + + ee_state_b: torch.Tensor = None + """End-effector frame state `[pos, quat, lin_vel, ang_vel]` in base frame. Shape: (count, 13).""" + + tool_sites_state_w: torch.Tensor = None + """Tool sites frames state `[pos, quat, lin_vel, ang_vel]` in simulation world frame. Shape: (count, num_sites, 13).""" + + tool_sites_state_b: torch.Tensor = None + """Tool sites frames state `[pos, quat, lin_vel, ang_vel]` in base frame. Shape: (count, num_sites, 13).""" + + ## + # Dynamics state. + ## + + ee_jacobian: torch.Tensor = None + """Geometric Jacobian of the parent body of end-effector frame in simulation frame. Shape: (count, 6, arm_num_dof).""" + + mass_matrix: torch.Tensor = None + """Mass matrix of the parent body of end-effector frame. Shape: (count, arm_num_dof, arm_num_dof).""" + + coriolis: torch.Tensor = None + """Coriolis and centrifugal force on parent body of end-effector frame. Shape: (count, arm_num_dof).""" + + gravity: torch.Tensor = None + """Generalized gravitational force on parent body of end-effector frame. Shape: (count, arm_num_dof).""" + + ## + # DOF states. + ## + + arm_dof_pos: torch.Tensor = None + """Arm joint positions. Shape: (count, arm_num_dof).""" + + arm_dof_vel: torch.Tensor = None + """Arm joint velocities. Shape: (count, arm_num_dof).""" + + arm_dof_acc: torch.Tensor = None + """Arm joint acceleration. Shape: (count, arm_num_dof).""" + + tool_dof_pos: torch.Tensor = None + """Tool joint positions. Shape: (count, tool_num_dof).""" + + tool_dof_vel: torch.Tensor = None + """Tool joint velocities. Shape: (count, tool_num_dof).""" + + tool_dof_acc: torch.Tensor = None + """Tool joint acceleration. Shape: (count, arm_num_dof).""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/__init__.py new file mode 100644 index 0000000000..de5a6ee413 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/__init__.py new file mode 100644 index 0000000000..7f5db507da --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Camera. +""" + +from .camera import Camera, CameraData +from .camera_cfg import FisheyeCameraCfg, PinholeCameraCfg + +__all__ = ["Camera", "CameraData", "PinholeCameraCfg", "FisheyeCameraCfg"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/camera.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/camera.py new file mode 100644 index 0000000000..6f2dcba378 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/camera.py @@ -0,0 +1,548 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Camera class in Omniverse workflows.""" + + +import builtins +import math +import numpy as np +import scipy.spatial.transform as tf +from dataclasses import dataclass +from typing import Any, Dict, Sequence, Tuple, Union + +import carb +import omni.isaac.core.utils.prims as prim_utils +import omni.replicator.core as rep +import omni.usd +from omni.isaac.core.prims import XFormPrimView +from omni.isaac.core.simulation_context import SimulationContext +from pxr import UsdGeom + +# omni-isaac-orbit +from omni.isaac.orbit.utils import class_to_dict, to_camel_case +from omni.isaac.orbit.utils.math import convert_quat + +from ..sensor_base import SensorBase +from .camera_cfg import FisheyeCameraCfg, PinholeCameraCfg + + +@dataclass +class CameraData: + """Data container for the camera sensor.""" + + position: np.ndarray = None + """Position of the sensor origin in world frame, following ROS convention.""" + orientation: np.ndarray = None + """Quaternion orientation `(w, x, y, z)` of the sensor origin in world frame, following ROS convention..""" + intrinsic_matrix: np.ndarray = None + """The intrinsic matrix for the camera.""" + image_shape: Tuple[int, int] = None + """A tuple containing (height, width) of the camera sensor.""" + output: Dict[str, Any] = None + """The retrieved sensor data with sensor types as key. + + The format of the data is available at [1]. + + References: + [1] https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_replicator/annotators_details.html#annotator-output + """ + + +class Camera(SensorBase): + """The camera sensor for acquiring visual data. + + Summarizing from the replicator extension [1], the following sensor types are supported: + + - "rgb": A rendered color image. + - "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. + - "motion_vectors": An image containing the motion vector data at each pixel. + - "instance_segmentation": The instance segmentation data. + - "semantic_segmentation": The semantic segmentation data. + - "bounding_box_2d_tight": The tight 2D bounding box data (only contains non-occluded regions). + - "bounding_box_2d_loose": The loose 2D bounding box data (contains occluded regions). + - "bounding_box_3d": The 3D view space bounding box data. + - "occlusion": The occlusion information (such as instance id, semantic id and occluded ratio). + + Typically, the sensor comprises of two prims: + + 1. Camera rig: A dummy Xform prim to which the camera is attached to. + 2. Camera prim: An instance of the USDGeom Camera. [3] + + However, for the sake of generality, we allow omission of the camera rig prim. This is mostly the case when + the camera is static. In such cases, any request to set the camera pose is directly set on the camera prim, + instead of setting the pose of the camera rig Xform prim. + + Reference: + [1] https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_replicator/annotators_details.html#annotator-output + [2] https://graphics.pixar.com/usd/docs/api/class_usd_geom_camera.html + + """ + + def __init__(self, cfg: Union[PinholeCameraCfg, FisheyeCameraCfg], device: str = "cpu"): + """Initializes the scanner object. + + Warning: + Replicator currently ignores the device and returns the data only on cpu. + This behavior will be fixed in the future, when replicator improves. + + Args: + cfg (Union[PinholeCameraCfg, FisheyeCameraCfg]): The configuration parameters. + device (str): The device on which to receive data. + """ + # store inputs + self.cfg = cfg + self.device = device + # initialize base class + super().__init__(self.cfg.sensor_tick) + # change the default rendering settings + # TODO: Should this be done here or maybe inside the app config file? + rep.settings.set_render_rtx_realtime(antialiasing="FXAA") + + # Acquire simulation context + self._sim_context = SimulationContext.instance() + # Xform prim for the camera rig + self._sensor_rig_prim: XFormPrimView = None + # UsdGeom Camera prim for the sensor + self._sensor_prim: UsdGeom.Camera = None + # Create empty variables for storing output data + self._data = CameraData() + self._data.output = dict.fromkeys(self.cfg.data_types, None) + # Flag to check that sensor is spawned. + self._is_spawned = False + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + # message for class + msg = ( + f"Camera @ '{self.prim_path}': \n" + f"\tdata types : {list(self._data.output.keys())} \n" + f"\ttick rate (s): {self.sensor_tick}\n" + f"\ttimestamp (s): {self.timestamp}\n" + f"\tframe : {self.frame}\n" + f"\tshape : {self.image_shape}\n" + f"\tposition : {self._data.position} \n" + f"\torientation : {self._data.orientation} \n" + ) + return msg + + """ + Properties + """ + + @property + def prim_path(self) -> str: + """The path to the camera prim.""" + return prim_utils.get_prim_path(self._sensor_prim) + + @property + def render_product_path(self) -> str: + """The path of the render product for the camera. + + This can be used via replicator interfaces to attach to writes or external annotator registry. + """ + return self._render_product_path + + @property + def data(self) -> CameraData: + """Data related to Camera sensor.""" + return self._data + + @property + def image_shape(self) -> Tuple[int, int]: + """A tuple containing (height, width) of the camera sensor.""" + return (self.cfg.height, self.cfg.width) + + """ + Configuration + """ + + def set_visibility(self, visible: bool): + """Set visibility of the instance in the scene. + + Args: + visible (bool): Whether to make instance visible or invisible. + + Raises: + RuntimeError: If the camera prim is not set. Need to call `initialize(...)` first. + """ + # check camera prim + if self._sensor_prim is None: + raise RuntimeError("Camera prim is None. Please call 'initialize(...)' first.") + # get imageable object + imageable = UsdGeom.Imageable(self._sensor_prim) + # set visibility + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + def set_intrinsic_matrix(self, matrix: np.ndarray, focal_length: float = 1.0): + """Set parameters of the USD camera from its intrinsic matrix. + + Note: + Due to limitations of Omniverse camera, we need to assume that the camera is a spherical lens, + i.e. has square pixels, and the optical center is centered at the camera eye. If this assumption + is not true in the input intrinsic matrix, then the camera will not set up correctly. + + Args: + intrinsic_matrix (np.ndarray): The intrinsic matrix for the camera. + focal_length (float, optional): Focal length to use when computing aperture values. Defaults to 1.0. + """ + # convert to numpy for sanity + intrinsic_matrix = np.asarray(matrix).astype(np.float) + # 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, + } + # set parameters for camera + for param_name, param_value in params.items(): + # convert to camel case (CC) + param_name = to_camel_case(param_name, to="CC") + # get attribute from the class + param_attr = getattr(self._sensor_prim, f"Get{param_name}Attr") + # set value + # note: We have to do it this way because the camera might be on a different layer (default cameras are on session layer), + # and this is the simplest way to set the property on the right layer. + omni.usd.utils.set_prop_val(param_attr(), param_value) + + """ + Operations - Set pose. + """ + + def set_world_pose_ros(self, pos: Sequence[float] = None, quat: Sequence[float] = None): + """Set the pose of the camera w.r.t. world frame using ROS convention. + + This methods uses the ROS convention to resolve the input pose. In this convention, + we assume that the camera front-axis is +Z-axis and up-axis is -Y-axis. + + Args: + pos (Sequence[float], optional): The cartesian coordinates (in meters). Defaults to None. + quat (Sequence[float], optional): The quaternion orientation in (w, x, y, z). Defaults to None. + + Raises: + RuntimeError: If the camera prim is not set. Need to call `initialize(...)` first. + """ + # add note that this function is not working correctly + # FIXME: Fix this function. Getting the camera pose and setting back over here doesn't work. + carb.log_warn("The function `set_world_pose_ros` is currently not implemented correctly.") + + # check camera prim exists + if self._sensor_prim is None: + raise RuntimeError("Camera prim is None. Please call 'initialize(...)' first.") + # convert from meters to stage units + if pos is not None: + pos = np.asarray(pos) + # convert rotation matrix from ROS convention to OpenGL + if quat is not None: + rotm = tf.Rotation.from_quat(convert_quat(quat, "xyzw")).as_matrix() + rotm[:, 2] = -rotm[:, 2] + rotm[:, 1] = -rotm[:, 1] + rotm = rotm.transpose() + quat_gl = tf.Rotation.from_matrix(rotm).as_quat() + # convert to isaac-sim convention + quat_gl = convert_quat(quat_gl, "wxyz") + else: + quat_gl = None + # set the pose + if self._sensor_rig_prim is None: + # Note: Technically, we should prefer not to do this. + cam_prim = XFormPrimView(self.prim_path, reset_xform_properties=False) + cam_prim.set_world_poses(pos, quat_gl) + else: + self._sensor_rig_prim.set_world_poses(pos, quat_gl) + + def set_world_pose_from_ypr( + self, target_position: Sequence[float], distance: float, yaw: float, pitch: float, roll: float, up_axis: str + ): + """Computes the view matrix from the inputs and sets the camera prim pose. + + The implementation follows from the `computeViewMatrixFromYawPitchRoll()` function in Bullet3. + + Args: + target_position (Sequence[float]): Target focus point in cartesian world coordinates. + distance (float): Distance from eye to focus point. + yaw (float): Yaw angle in degrees (up, down). + pitch (float): Pitch angle in degrees around up vector. + roll (float): Roll angle in degrees around forward vector. + up_axis (str): Either 'y/Y' or 'z/Z' axis up. + + Raises: + RuntimeError: If the camera prim is not set. Need to call `initialize(...)` first. + ValueError: When the `up_axis` is not "y/Y" or "z/Z". + """ + # sanity conversion + camera_target_position = np.asarray(target_position) + up_axis = up_axis.upper() + # check camera prim exists + if self._sensor_prim is None: + raise RuntimeError("Camera prim is None. Please call 'initialize(...)' first.") + # check inputs + if up_axis not in ["Y", "Z"]: + raise ValueError(f"Invalid specified up axis '{up_axis}'. Valid options: ['Y', 'Z'].") + # compute camera's eye pose + if up_axis == "Y": + eye_position = np.array([0.0, 0.0, -distance]) + eye_rotation = tf.Rotation.from_euler("ZYX", [roll, yaw, -pitch], degrees=True).as_matrix() + up_vector = np.array([0.0, 1.0, 0.0]) + else: + eye_position = np.array([0.0, -distance, 0.0]) + eye_rotation = tf.Rotation.from_euler("ZYX", [yaw, roll, pitch], degrees=True).as_matrix() + up_vector = np.array([0.0, 0.0, 1.0]) + # transform eye to get camera position + cam_up_vector = np.dot(eye_rotation, up_vector) + cam_position = np.dot(eye_rotation, eye_position) + camera_target_position + # axis direction for camera's view matrix + f = (camera_target_position - cam_position) / np.linalg.norm(camera_target_position - cam_position) + u = cam_up_vector / np.linalg.norm(cam_up_vector) + s = np.cross(f, u) + # create camera's view matrix: camera_T_world + cam_view_matrix = np.eye(4) + cam_view_matrix[:3, 0] = s + cam_view_matrix[:3, 1] = u + cam_view_matrix[:3, 2] = -f + cam_view_matrix[3, 0] = -np.dot(s, cam_position) + cam_view_matrix[3, 1] = -np.dot(u, cam_position) + cam_view_matrix[3, 2] = np.dot(f, cam_position) + # compute camera transform: world_T_camera + cam_tf = np.linalg.inv(cam_view_matrix) + cam_quat = tf.Rotation.from_matrix(cam_tf[:3, :3].T).as_quat() + cam_pos = cam_tf[3, :3] + # set camera pose + self.set_camera_pose(cam_pos, cam_quat) + + def set_world_pose_from_view(self, eye: Sequence[float], target: Sequence[float] = [0, 0, 0], vel: float = 0.0): + """Set the pose of the camera from the eye position and look-at target position. + + Warn: + This method directly sets the camera prim pose and not the pose of the camera rig. + It is advised not to use it when the camera is part of a sensor rig. + + Args: + eye (Sequence[float]): The position of the camera's eye. + target (Sequence[float], optional): The target location to look at. Defaults to [0, 0, 0]. + vel (float, optional): The velocity of the camera.. Defaults to 0.0. + """ + with self._rep_camera: + rep.modify.pose(position=eye, look_at=target) + # FIXME: add note that this function is not working correctly + carb.log_warn("The function `set_world_pose_from_view` is currently not implemented correctly.") + + """ + Operations + """ + + def spawn(self, parent_prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): + """Spawns the sensor into the stage. + + Args: + parent_prim_path (str): The path of the parent prim to attach sensor to. + translation (Sequence[float], optional): The local position offset w.r.t. parent prim. Defaults to None. + orientation (Sequence[float], optional): The local rotation offset in `(w, x, y, z)` w.r.t. parent prim. Defaults to None. + """ + # Check projection type of the camera. + if isinstance(self.cfg, FisheyeCameraCfg): + projection_type: str = "fisheye_polynomial" + else: + projection_type = "pinhole" + + # Create camera using replicator. This creates under it two prims: + # 1) the rig: at the path f"{prim_path}/Camera_Xform" + # 2) the USD camera: at the path f"{prim_path}/Camera_Xform/Camera" + self._rep_camera = rep.create.camera( + parent=parent_prim_path, + projection_type=projection_type, + **class_to_dict(self.cfg.usd_params), + ) + # Acquire the sensor prims + # 1) the rig + cam_rig_prim_path = rep.utils.get_node_targets(self._rep_camera.node, "inputs:prims")[0] + self._sensor_rig_prim = XFormPrimView(cam_rig_prim_path, reset_xform_properties=False) + # 2) the USD camera + cam_prim = prim_utils.get_prim_children(prim_utils.get_prim_at_path(cam_rig_prim_path))[0] + self._sensor_prim = UsdGeom.Camera(cam_prim) + # Set the transformation of the camera + # Note: As mentioned in Isaac Sim documentation, it is better to never transform to camera directly. + # Hence, we only transform the camera rig. + self._sensor_rig_prim.set_local_poses(translation, orientation) + # Set spawning to true + self._is_spawned = True + + def initialize(self, cam_prim_path: str = None, has_rig: bool = False): + """Initializes the sensor handles and internal buffers. + + Args: + cam_prim_path (str, optional): The prim path to existing camera. Defaults to None. + has_rig (bool, optional): Whether the passed camera prim path is attached to a rig. Defaults to False. + + Raises: + RuntimeError: When input `cam_prim_path` is :obj:`None`, the method defaults to using the last + camera prim path set when calling the :meth:`spawn()` function. In case, the camera was not spawned + and no valid `cam_prim_path` is provided, the function throws an error. + """ + # Check that sensor has been spawned + if cam_prim_path is None: + if not self._is_spawned: + raise RuntimeError("Initialize the camera failed! Please provide a valid argument for `prim_path`.") + else: + # Force to set active camera to input prim path/ + cam_prim = prim_utils.get_prim_at_path(cam_prim_path) + self._sensor_prim = UsdGeom.Camera(cam_prim) + # Check rig + if has_rig: + self._sensor_rig_prim = XFormPrimView(cam_prim_path.rsplit("/", 1)[0], reset_xform_properties=False) + else: + self._sensor_rig_prim = None + + # Enable synthetic data sensors + self._render_product_path = rep.create.render_product( + self.prim_path, resolution=(self.cfg.width, self.cfg.height) + ) + # Attach the sensor data types to render node + self._rep_registry: Dict[str, rep.annotators.Annotator] = dict.fromkeys(self.cfg.data_types, None) + # -- iterate over each data type + for name in self.cfg.data_types: + # init params -- Checked from rep.scripts.writes_default.basic_writer.py + # note: we are verbose here to make it easier to understand the code. + # if colorize is true, the data is mapped to colors and a uint8 4 channel image is returned. + # if colorize is false, the data is returned as a uint32 image with ids as values. + if name in ["bounding_box_2d_tight", "bounding_box_2d_loose", "bounding_box_3d"]: + init_params = {"semanticTypes": self.cfg.semantic_types} + elif name in ["semantic_segmentation", "instance_segmentation"]: + init_params = {"semanticTypes": self.cfg.semantic_types, "colorize": False} + elif name in ["instance_id_segmentation"]: + init_params = {"colorize": False} + else: + init_params = None + # create annotator node + rep_annotator = rep.AnnotatorRegistry.get_annotator(name, init_params) + rep_annotator.attach([self._render_product_path]) + # add to registry + self._rep_registry[name] = rep_annotator + # Reset internal buffers + self.reset() + # When running in standalone mode, need to render a few times to fill all the buffers + # FIXME: Check with simulation team to get rid of this. What if someone has render or other callbacks? + if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: + for _ in range(4): + self._sim_context.render() + + def reset(self): + # reset the timestamp + super().reset() + # reset the buffer + self._data.position = None + self._data.orientation = None + self._data.intrinsic_matrix = self._compute_intrinsic_matrix() + self._data.image_shape = self.image_shape + self._data.output = dict.fromkeys(self._data.output, None) + + def update(self, dt: float): + """Updates the buffers at sensor frequency. + + Args: + dt (float): The simulation time-step. + """ + super().update(dt) + + def buffer(self): + # When running in standalone mode, need to render a few times to fill all the buffers + # FIXME: Check with simulation team to get rid of this. What if someone has render or other callbacks? + if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: + for _ in range(4): + self._sim_context.render() + # -- intrinsic matrix + self._data.intrinsic_matrix = self._compute_intrinsic_matrix() + # -- pose + self._data.position, self._data.orientation = self._compute_ros_pose() + # -- read the data from annotator registry + for name in self._rep_registry: + self._data.output[name] = self._rep_registry[name].get_data(device=self.device) + # -- update the trigger call data (needed by replicator BasicWriter method) + self._data.output["trigger_outputs"] = {"on_time": self.frame} + + """ + Private Helpers + """ + + def _compute_intrinsic_matrix(self) -> np.ndarray: + """Compute camera's matrix of intrinsic parameters. + + Also called calibration matrix. This matrix works for linear depth images. We assume square pixels. + + Note: + The calibration matrix projects points in the 3D scene onto an imaginary screen of the camera. + The coordinates of points on the image plane are in the homogeneous representation. + + Returns: + np.ndarray: A 3 x 3 numpy array containing the intrinsic parameters for the camera. + + Raises: + RuntimeError: If the camera prim is not set. Need to call `initialize(...)` first. + """ + # check camera prim exists + if self._sensor_prim is None: + raise RuntimeError("Camera prim is None. Please call 'initialize(...)' first.") + # get camera parameters + focal_length = self._sensor_prim.GetFocalLengthAttr().Get() + horiz_aperture = self._sensor_prim.GetHorizontalApertureAttr().Get() + # get viewport parameters + height, width = self.image_shape + # calculate the field of view + fov = 2 * math.atan(horiz_aperture / (2 * focal_length)) + # calculate the focal length in pixels + focal_px = width * 0.5 / math.tan(fov / 2) + # create intrinsic matrix for depth linear + a = focal_px + b = width * 0.5 + c = focal_px + d = height * 0.5 + cam_intrinsic_matrix = np.array([[a, 0, b], [0, c, d], [0, 0, 1]], dtype=float) + + return cam_intrinsic_matrix + + def _compute_ros_pose(self) -> Tuple[np.ndarray, np.ndarray]: + """Computes the pose of the camera in the world frame with ROS convention. + + This methods uses the ROS convention to resolve the input pose. In this convention, + we assume that the camera front-axis is +Z-axis and up-axis is -Y-axis. + + Returns: + Tuple[np.ndarray, np.ndarray]: A tuple of the position (in meters) and quaternion (w, x, y, z). + """ + # get camera's location in world space + prim_tf = UsdGeom.Xformable(self._sensor_prim).ComputeLocalToWorldTransform(0.0) + # GfVec datatypes are row vectors that post-multiply matrices to effect transformations, + # which implies, for example, that it is the fourth row of a GfMatrix4d that specifies + # the translation of the transformation. Thus, we take transpose here to make it post multiply. + prim_tf = np.transpose(prim_tf) + # extract the position (convert it to SI units-- assumed that stage units is 1.0) + pos = prim_tf[0:3, 3] + # extract rotation + # Note: OpenGL camera transform is such that camera faces along -z axis and +y is up. + # In robotics, we need to rotate it such that the camera is along +z axis and -y is up. + cam_rotm = prim_tf[0:3, 0:3] + # make +z forward + cam_rotm[:, 2] = -cam_rotm[:, 2] + # make +y down + cam_rotm[:, 1] = -cam_rotm[:, 1] + # convert rotation to quaternion + quat = tf.Rotation.from_matrix(cam_rotm).as_quat() + + return pos, convert_quat(quat, "wxyz") diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/camera_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/camera_cfg.py new file mode 100644 index 0000000000..b50b414d58 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/camera_cfg.py @@ -0,0 +1,148 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the camera sensor.""" + + +from dataclasses import MISSING +from typing import List, Tuple + +# omni-isaac-orbit +from omni.isaac.orbit.utils import configclass + + +@configclass +class PinholeCameraCfg: + """Configuration for a pinhole camera sensor. + + The following sensor types are supported: + + - "rgb": A rendered color image. + - "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. + - "motion_vectors": An image containing the motion vector data at each pixel. + - "instance_segmentation": The instance segmentation data. + - "semantic_segmentation": The semantic segmentation data. + - "bounding_box_2d_tight": The tight 2D bounding box data (only contains non-occluded regions). + - "bounding_box_2d_loose": The loose 2D bounding box data (contains occluded regions). + - "bounding_box_3d": The 3D view space bounding box data. + - "occlusion": The occlusion information (such as instance id, semantic id and occluded ratio). + """ + + sensor_tick: float = 0.0 + """Simulation seconds between sensor buffers. Defaults to 0.0.""" + data_types: List[str] = ["rgb"] + """List of sensor names/types to enable for the camera. Defaults to ["rgb"].""" + width: int = MISSING + """Width of the image in pixels.""" + height: int = MISSING + """Height of the image in pixels.""" + semantic_types: List[str] = ["class"] + """List of allowed semantic types the types. Defaults to ["class"]. + + For example, if `semantic_types` is [“class”], only the bounding boxes for prims with semantics of + type “class” will be retrieved. + + More information available at: + https://docs.omniverse.nvidia.com/app_code/prod_extensions/ext_replicator/semantic_schema_editor.html + """ + + @configclass + class UsdCameraCfg: + """USD related configuration for the sensor. + + Note: + The parameter is kept default from USD if it is set to :obj:`None`. This includes the default + parameters (in case the sensor is created) or the ones set by the user (in case the sensor is + loaded from existing USD stage). + + Reference: + * https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_replicator/annotators_details.html + * https://graphics.pixar.com/usd/docs/api/class_usd_geom_camera.html + """ + + clipping_range: Tuple[float, float] = None + """Near and far clipping distances (in stage units).""" + focal_length: float = None + """Perspective focal length (in mm). Longer lens lengths narrower FOV, shorter lens lengths wider FOV.""" + focus_distance: float = None + """Distance from the camera to the focus plane (in stage units). + + The distance at which perfect sharpness is achieved. + """ + f_stop: float = None + """Lens aperture. Defaults to 0.0, which turns off focusing. + + Controls Distance Blurring. Lower Numbers decrease focus range, larger numbers increase it. + """ + horizontal_aperture: float = None + """Horizontal aperture (in mm). Emulates sensor/film width on a camera.""" + horizontal_aperture_offset: float = None + """Offsets Resolution/Film gate horizontally.""" + vertical_aperture_offset: float = None + """Offsets Resolution/Film gate vertically.""" + + usd_params: UsdCameraCfg = UsdCameraCfg() + """Parameters for setting USD camera settings.""" + + +@configclass +class FisheyeCameraCfg(PinholeCameraCfg): + """Configuration for a fisheye camera sensor. + + The following sensor types are supported: + + - "rgb": A rendered color image. + - "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. + - "motion_vectors": An image containing the motion vector data at each pixel. + - "instance_segmentation": The instance segmentation data. + - "semantic_segmentation": The semantic segmentation data. + - "bounding_box_2d_tight": The tight 2D bounding box data (only contains non-occluded regions). + - "bounding_box_2d_loose": The loose 2D bounding box data (contains occluded regions). + - "bounding_box_3d": The 3D view space bounding box data. + - "occlusion": The occlusion information (such as instance id, semantic id and occluded ratio). + + """ + + @configclass + class UsdCameraCfg(PinholeCameraCfg.UsdCameraCfg): + """USD related configuration for the sensor. + + Note: + The parameter is kept default from USD if it is set to :obj:`None`. This includes the default + parameters (in case the sensor is created) or the ones set by the user (in case the sensor is + loaded from existing USD stage). + + Reference: + * https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/manual_replicator_composer_parameter_list.html#camera-parameters + * https://graphics.pixar.com/usd/docs/api/class_usd_geom_camera.html + """ + + fisheye_nominal_width: float = None + """Nominal width of fisheye lens model.""" + fisheye_nominal_height: float = None + """Nominal height of fisheye lens model.""" + fisheye_optical_centre_x: float = None + """Horizontal optical centre position of fisheye lens model.""" + fisheye_optical_centre_y: float = None + """Vertical optical centre position of fisheye lens model.""" + fisheye_max_fov: float = None + """Maximum field of view of fisheye lens model.""" + fisheye_polynomial_a: float = None + """First component of fisheye polynomial.""" + fisheye_polynomial_b: float = None + """Second component of fisheye polynomial.""" + fisheye_polynomial_c: float = None + """Third component of fisheye polynomial.""" + fisheye_polynomial_d: float = None + """Fourth component of fisheye polynomial.""" + fisheye_polynomial_e: float = None + """Fifth component of fisheye polynomial.""" + + usd_params: UsdCameraCfg = UsdCameraCfg() + """Parameters for setting USD camera settings.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/utils.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/utils.py new file mode 100644 index 0000000000..83c298a37c --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/camera/utils.py @@ -0,0 +1,217 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Helper functions to project between pointcloud and depth images.""" + + +import numpy as np +import scipy.spatial.transform as tf +import torch +from typing import Sequence, Tuple, Union + +from omni.isaac.orbit.utils.math import convert_quat + + +def transform_pointcloud( + points: Union[np.ndarray, torch.Tensor], position: Sequence[float] = None, orientation: Sequence[float] = None +) -> Union[np.ndarray, torch.Tensor]: + """Transform input points in a given frame to a target frame. + + Args: + points (Union[np.ndarray, torch.Tensor]): An array of shape (N, 3) comprising of 3D points in source frame. + position (Sequence[float], optional): The position of source frame in target frame. Defaults to None. + orientation (Sequence[float], optional): The orientation `(w, x, y, z)` of source frame in target frame. + Defaults to None. + + Returns: + Union[np.ndarray, torch.Tensor]: An array of shape (N, 3) comprising of 3D points in target frame. + """ + # device for carrying out conversion + device = "cpu" + is_torch = True + # convert to torch + if not isinstance(points, torch.Tensor): + is_torch = False + points = torch.from_numpy(points).to(device) + # rotate points + if orientation is not None: + # convert to numpy (sanity) + orientation = np.asarray(orientation) + # convert using scipy to simplify life + rot = tf.Rotation.from_quat(convert_quat(orientation, "xyzw")) + rot_matrix = torch.from_numpy(rot.as_matrix()).to(device) + # apply rotation + points = torch.matmul(rot_matrix, points.T).T + # translate points + if position is not None: + # convert to torch to simplify life + position = torch.from_numpy(position).to(device) + # apply translation + points += position + # return results + if is_torch: + return points + else: + return points.detach().cpu().numpy() + + +def create_pointcloud_from_depth( + intrinsic_matrix: np.ndarray, + depth: np.ndarray, + keep_invalid: bool = False, + position: Sequence[float] = None, + orientation: Sequence[float] = None, +) -> np.ndarray: + """Creates pointcloud from input depth image and camera intrinsic matrix. + + If the inputs `camera_position` and `camera_orientation` are provided, the pointcloud is transformed + from the camera frame to the target frame. + + We use PyTorch here for matrix multiplication since it is compiled with Intel MKL while numpy + by default uses OpenBLAS. With PyTorch (CPU), we could process a depth image of size (480, 640) + in 0.0106 secs, while with numpy it took 0.0292 secs. + + Args: + intrinsic_matrix (np.ndarray): A (3, 3) numpy array providing camera's calibration matrix. + depth (np.ndarray): An array of shape (H, W) with values encoding the depth measurement. + keep_invalid (bool, optional): Whether to keep invalid points in the cloud or not. Invalid points + correspond to pixels with depth values 0.0 or NaN. Defaults to False. + position (Sequence[float], optional): The position of the camera in a target frame. Defaults to None. + orientation (Sequence[float], optional): The orientation `(w, x, y, z)` of the camera in a target frame. + Defaults to None. + + Raises: + ValueError: When intrinsic matrix is not of shape (3, 3). + ValueError: When depth image is not of shape (H, W) or (H, W, 1). + + Returns: + np.ndarray: An array of shape (N, 3) comprising of 3D coordinates of points. + + """ + # device for carrying out conversion + device = "cpu" + # convert to numpy matrix + intrinsic_matrix = np.asarray(intrinsic_matrix) + depth = np.asarray(depth).copy() + # squeeze out excess dimension + if len(depth.shape) == 3: + depth = depth.squeeze(axis=2) + # check shape of inputs + if intrinsic_matrix.shape != (3, 3): + raise ValueError(f"Input intrinsic matrix of invalid shape: {intrinsic_matrix.shape} != (3, 3).") + if len(depth.shape) != 2: + raise ValueError(f"Input depth image not two-dimensional. Received shape: {depth.shape}.") + # convert inputs to numpy arrays + intrinsic_matrix = torch.from_numpy(intrinsic_matrix).to(device) + depth = torch.from_numpy(depth).to(device) + # get image height and width + im_height, im_width = depth.shape + # convert image points into list of shape (3, H x W) + img_indices = np.indices((im_width, im_height)).reshape(2, -1) + pixels = np.pad(img_indices, [(0, 1), (0, 0)], mode="constant", constant_values=1.0) + pixels = torch.tensor(pixels, dtype=torch.double, device=device) + # convert into 3D points + points = torch.matmul(torch.inverse(intrinsic_matrix), pixels) + points = points / points[-1, :] + points_xyz = points * depth.T.reshape(-1) + # convert it to (H x W , 3) + points_xyz = torch.transpose(points_xyz, dim0=0, dim1=1) + # convert 3D points to world frame + if position is not None or orientation is not None: + points_xyz = transform_pointcloud(points_xyz, position, orientation) + # convert to numpy + points_xyz = points_xyz.detach().cpu().numpy() + # remove points that have invalid depth + if not keep_invalid: + invalid_points_idx = np.where(np.logical_or(np.isnan(points_xyz), np.isinf(points_xyz))) + points_xyz = np.delete(points_xyz, invalid_points_idx, axis=0) + + return points_xyz + + +def create_pointcloud_from_rgbd( + intrinsic_matrix: np.ndarray, + depth: np.ndarray, + rgb: Union[np.ndarray, Tuple[float, float, float]] = None, + normalize_rgb: bool = False, + position: Sequence[float] = None, + orientation: Sequence[float] = None, + num_channels: int = 3, +) -> Tuple[np.ndarray, np.ndarray]: + """Creates pointcloud from input depth image and camera transformation matrix. + + The `rgb` attribute is used to resolve the corresponding point's color: + + - If a numpy array of shape (H, W, 3) then the corresponding channels encode RGB values. + - If a tuple then the point cloud has a single color specified by the values (r, g, b). + - If None, then default color is white, i.e. (0, 0, 0). + + If the inputs `camera_position` and `camera_orientation` are provided, the pointcloud is transformed + from the camera frame to the target frame. + + Args: + intrinsic_matrix (np.ndarray): A (3, 3) numpy array providing camera's calibration matrix. + depth (np.ndarray): An array of shape (H, W) with values encoding the depth measurement. + rgb (Union[np.ndarray, Tuple[float, float, float]], optional): Color for generated point cloud. + Defaults to None. + normalize_rgb (bool, optional): Whether to normalize input rgb. Defaults to False. + position (Sequence[float], optional): The position of the camera in a target frame. Defaults to None. + orientation (Sequence[float], optional): The orientation `(w, x, y, z)` of the camera in a target frame. + Defaults to None. + num_channels (int, optional): Number of channels in RGB pointcloud. Defaults to 3. + + Raises: + ValueError: When rgb image is a numpy array but not of shape (H, W, 3) or (H, W, 4). + + Returns: + Tuple[np.ndarray, np.ndarray]: A tuple of (N, 3) numpy arrays containing 3D coordinates of + points and their RGB color respectively. + """ + # check valid inputs + if rgb is not None and not isinstance(rgb, tuple): + if len(rgb.shape) == 3: + if rgb.shape[2] not in [3, 4]: + raise ValueError(f"Input rgb image of invalid shape: {rgb.shape} != (H, W, 3) or (H, W, 4).") + else: + raise ValueError(f"Input rgb image not three-dimensional. Received shape: {rgb.shape}.") + if num_channels not in [3, 4]: + raise ValueError(f"Invalid number of channels: {num_channels} != 3 or 4.") + # retrieve XYZ pointcloud + points_xyz = create_pointcloud_from_depth( + intrinsic_matrix, depth, position=position, orientation=orientation, keep_invalid=True + ) + + # get image height and width + im_height, im_width = depth.shape[:2] + # total number of points + num_points = im_height * im_width + # extract color value + if rgb is not None: + if isinstance(rgb, np.ndarray): + # copy numpy array to preserve + rgb = np.asarray(rgb, dtype="float").copy() + rgb = rgb[:, :, :3] + # convert the matrix to (W, H, 3) since depth processing + # is done in the order (u, v) where u: 0 - W-1 and v: 0 - H-1 + points_rgb = np.reshape(rgb.transpose(1, 0, 2), (-1, 3)) + elif isinstance(rgb, Tuple): + points_rgb = np.asarray((rgb,) * num_points, dtype=np.uint8) + else: + points_rgb = np.asarray(((0, 0, 0),) * num_points, dtype=np.uint8) + else: + points_rgb = np.asarray(((0, 0, 0),) * num_points, dtype=np.uint8) + # normalize color values + if normalize_rgb: + points_rgb = np.asarray(points_rgb, dtype="float") / 255 + + # remove invalid points + invalid_points_idx = np.where(np.logical_or(np.isnan(points_xyz), np.isinf(points_xyz))) + points_xyz = np.delete(points_xyz, invalid_points_idx, axis=0) + points_rgb = np.delete(points_rgb, invalid_points_idx, axis=0) + # add additional channels if required + if num_channels == 4: + points_rgb = np.pad(points_rgb, [(0, 0), (0, 1)], mode="constant", constant_values=1.0) + + return points_xyz, points_rgb diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/__init__.py new file mode 100644 index 0000000000..5a0170466b --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Height-scanner. +""" + +from .height_scanner import HeightScanner, HeightScannerData +from .height_scanner_cfg import HeightScannerCfg + +__all__ = ["HeightScanner", "HeightScannerData", "HeightScannerCfg"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner.py new file mode 100644 index 0000000000..69786fad96 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner.py @@ -0,0 +1,289 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +@author Mayank Mittal +@email mittalma@ethz.ch +@author David Hoeller +@email dhoeller@nvidia.com + +@brief Height-map scanner in Omniverse workflows. +""" + +import numpy as np +import scipy.spatial.transform as tf +from dataclasses import dataclass +from typing import List, Union + +import omni +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +from omni.isaac.core.prims import XFormPrimView + +# omni-isaac-orbit +from omni.isaac.orbit.utils.math import convert_quat + +from ..sensor_base import SensorBase +from .height_scanner_cfg import HeightScannerCfg +from .height_scanner_marker import HeightScannerMarker + + +@dataclass +class HeightScannerData: + """Data container for the height-scanner sensor.""" + + position: np.ndarray = None + """Position of the sensor origin in world frame.""" + orientation: np.ndarray = None + """Orientation of the sensor origin in quaternion (w, x, y, z) in world frame.""" + hit_points: np.ndarray = None + """The end point locations of ray-casted rays.""" + hit_distance: np.ndarray = None + """The ray-cast travel distance from query point.""" + hit_status: np.ndarray = None + """Whether the ray hit an object or not.""" + + +class HeightScanner(SensorBase): + """A two-dimensional height-map scan sensor. + + A local map is often required to plan a robot's motion over a limited time horizon. For mobile systems, + often we care about the terrain for locomotion. The height-map, also called elevation map, simplifies the + terrain as a two-dimensional surface. Each grid-cell represents the height of the terrain. + + Unlike algorithms which fuse depth measurements to create an elevation map [1], in this method we directly use + the PhysX API for ray-casting and query the height of the terrain from a set of query scan points. These points + represent the location of the grid cells. + + The height-scanner uses PhysX for ray-casting to collision bodies. To prevent the casting to certain prims + in the scene (such as the robot on which height-scanner is present), one needs to provide the names of the + prims to not check collision with as a part of the dictionary config. + + The scanner offset :math:`(x_o, y_o, z_o)` is the offset of the sensor from the frame it is attached to. During the + `update(...)` or `buffer(...)`, the pose of the mounted frame needs to be provided. + + If visualization is enabled, rays that have a hit are displayed in red, while a miss is displayed in blue. + During a miss, the point's distance is set to the maximum ray-casting distance. + + References: + [1] Fankhauser, P., Bloesch, M., & Hutter, M. (2018). Probabilistic terrain mapping for mobile robots + with uncertain localization. IEEE Robotics and Automation Letters, 3(4), 3019-3026. + + TODO: + Move this class to use generic range sensor from Isaac Sim. + Reference: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/ext_omni_isaac_range_sensor.html#isaac-sim-generic-range-sensor-example + """ + + def __init__(self, cfg: HeightScannerCfg): + """Initializes the scanner object. + + Args: + cfg (HeightScannerCfg): The configuration parameters. + """ + # store inputs + self.cfg = cfg + # initialize base class + super().__init__(self.cfg.sensor_tick) + + # Points to query ray-casting from + self._scan_points = np.asarray(self.cfg.points) + # If points are 2D, add dimension along z (z=0 relative to the sensor frame) + if self._scan_points.shape[1] == 2: + self._scan_points = np.pad(self._scan_points, [(0, 0), (0, 1)], constant_values=0) + + # Flag to check that sensor is spawned. + self._is_spawned = False + # Whether to visualize the scanner points. Defaults to False. + self._visualize = False + # Xform prim for the sensor rig + self._sensor_prim: XFormPrimView = None + # Create empty variables for storing output data + self._data = HeightScannerData() + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + # message for class + msg = ( + f"Height Scanner @ '{self.prim_path}': \n" + f"\ttick rate (s) : {self.sensor_tick}\n" + f"\ttimestamp (s) : {self.timestamp}\n" + f"\tframe : {self.frame}\n" + f"\tposition : {self.data.position}\n" + f"\torientation : {self.data.orientation}\n" + f"\t# of hits : {np.sum(self.data.hit_status)} / {self._scan_points[0]}\n" + ) + return msg + + """ + Properties + """ + + @property + def prim_path(self) -> str: + """Returns: The path to the height-map sensor.""" + return self._sensor_prim.prim_paths[0] + + @property + def data(self) -> HeightScannerData: + """Returns: Data related to height scanner.""" + return self._data + + """ + Helpers + """ + + def set_visibility(self, visible: bool): + """Enables drawing of the scan points in the viewport. + + Arguments: + visible {bool} -- Whether to draw scan points or not. (default: {True}) + """ + # copy argument + self._visualize = visible + # set visibility + self._height_scanner_vis.set_visibility(visible) + + def set_filter_prims(self, names: List[str]): + """Set the names of prims to ignore ray-casting collisions with. + + If None is passed into argument, then no filtering is performed. + + Args: + names (List[str]): A list of prim names to ignore raycast collisions with. + """ + # default + if names is None: + self.cfg.filter_prims = list() + else: + # set into the class + self.cfg.filter_prims = names + + """ + Operations + """ + + def spawn(self, parent_prim_path: str): + # Check if sensor is already spawned + if self._is_spawned: + raise RuntimeError(f"The height scanner sensor instance has already been spawned at: {self.prim_path}.") + # Create sensor prim path + prim_path = stage_utils.get_next_free_path(f"{parent_prim_path}/HeightScan_Xform") + # Create the sensor prim + prim_utils.create_prim(prim_path, "XForm") + self._sensor_prim = XFormPrimView(prim_path, translations=np.expand_dims(self.cfg.offset, axis=0)) + # Create visualization marker + # TODO: Move this inside the height-scan prim to make it cleaner? + vis_prim_path = stage_utils.get_next_free_path("/World/Visuals/HeightScan") + self._height_scanner_vis = HeightScannerMarker(vis_prim_path, count=self._scan_points.shape[0], radius=0.02) + # Set spawning to true + self._is_spawned = True + + def initialize(self): + # Check that sensor has been spawned + if not self._is_spawned: + raise RuntimeError("Height scanner sensor must be spawned first. Please call `spawn(...)`.") + # Initialize Xform class + self._sensor_prim.initialize() + # Acquire physx ray-casting interface + self._physx_query_interface = omni.physx.get_physx_scene_query_interface() + # Since height scanner is fictitious sensor, we have no schema config to set in this case. + # Initialize buffers + self.reset() + + def reset(self): + # reset the timestamp + super().reset() + # reset the buffer + self._data.hit_points = np.empty(shape=(self._scan_points.shape)) + self._data.hit_distance = np.empty(shape=(self._scan_points.shape[0])) + self._data.hit_status = np.zeros(shape=(self._scan_points.shape[0])) + self._data.position = None + self._data.orientation = None + + def update(self, dt: float, pos: Union[np.ndarray, List[float]], quat: Union[np.ndarray, List[float]]): + """Updates the buffers at sensor frequency. + + Args: + dt (float): The simulation time-step. + pos (Union[np.ndarray, List[float]]): Position of the frame to which the sensor is attached. + quat (Union[np.ndarray, List[float]]): Quaternion (w, x, y, z) of the frame to which the sensor is attached. + """ + super().update(dt, pos, quat) + + def buffer(self, pos: Union[np.ndarray, List[float]], quat: Union[np.ndarray, List[float]]): + """Fills the buffers of the sensor data. + + This function does not perform any time-based checks and directly fills the data into the data container. + + Warning: + Although this method is public, `update(dt)` should be the preferred way of filling buffers. + + Args: + pos (Union[np.ndarray, List[float]]): Position of the frame to which the sensor is attached. + quat (Union[np.ndarray, List[float]]): Quaternion (w, x, y, z) of the frame to which the sensor is attached. + """ + # convert to numpy for sanity + pos = np.asarray(pos) + quat = np.asarray(quat) + # account for the offset of the sensor + self._data.position, self._data.orientation = (pos + self.cfg.offset, quat) + + # construct 3D rotation matrix for grid points + # TODO: Check if this is the most generic case. It ignores the base pitch and roll. + tf_rot = tf.Rotation.from_quat(convert_quat(self.data.orientation, "xyzw")) + rpy = tf_rot.as_euler("xyz", degrees=False) + rpy[:2] = 0 + rotation = tf.Rotation.from_euler("xyz", rpy).as_matrix() + # transform the scan points to world frame + world_scan_points = np.matmul(rotation, self._scan_points.T).T + self.data.position + + # iterate over all the points and query ray-caster + for i in range(world_scan_points.shape[0]): + # set current query info to empty + self._query_info = None + # perform ray-cast to get distance of a point along (0, 0, -1) + self._physx_query_interface.raycast_all( + tuple(world_scan_points[i]), + self.cfg.direction, + self.cfg.max_distance, + self._hit_report_callback, + ) + # check if hit happened based on query info and add to data + if self._query_info is not None: + self._data.hit_status[i] = 1 + self._data.hit_distance[i] = self._query_info.distance + else: + self._data.hit_status[i] = 0 + self._data.hit_distance[i] = self.cfg.max_distance + # add ray-end point (in world frame) to data + self._data.hit_points[i] = world_scan_points[i] + np.array(self.cfg.direction) * self._data.hit_distance[i] + + # visualize the prim + if self._visualize: + self._height_scanner_vis.set_status(status=self._data.hit_status) + self._height_scanner_vis.set_world_poses(positions=self._data.hit_points) + + """ + Private helpers + """ + + def _hit_report_callback(self, hit) -> bool: + """A PhysX callback to filter out hit-reports that are on the collision bodies. + + Returns: + bool: If True, continue casting the ray. Otherwise, stop and report the result. + """ + # unset the query info + self._query_info = None + # get ray's current contact rigid body + current_hit_body = hit.rigid_body + # check if the collision body is in the filtering list + if current_hit_body in self.cfg.filter_prims: + # continue casting the ray + return True + else: + # set the hit information + self._query_info = hit + return False diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner_cfg.py new file mode 100644 index 0000000000..6bce01d728 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner_cfg.py @@ -0,0 +1,31 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the height scanner sensor.""" + + +from dataclasses import MISSING +from typing import List, Tuple + +# omni-isaac-orbit +from omni.isaac.orbit.utils import configclass + + +@configclass +class HeightScannerCfg: + """Configuration for the height-scanner sensor.""" + + sensor_tick: float = 0.0 + """Simulation seconds between sensor buffers. Defaults to 0.0.""" + points: list = MISSING + """The 2D scan points to query ray-casting from. Results are reported in this order.""" + offset: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """The offset from the frame the sensor is attached to. Defaults to (0.0, 0.0, 0.0).""" + direction: Tuple[float, float, float] = (0.0, 0.0, -1.0) + """Unit direction for the scanner ray-casting. Defaults to (0.0, 0.0, -1.0).""" + max_distance: float = 100.0 + """Maximum distance from the sensor to ray cast to. Defaults to 100.0.""" + filter_prims: List[str] = list() + """A list of prim names to ignore ray-cast collisions with. Defaults to empty list.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner_marker.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner_marker.py new file mode 100644 index 0000000000..113f389402 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner_marker.py @@ -0,0 +1,148 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import torch +from typing import List, Optional, Sequence, Union + +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +from pxr import Gf, UsdGeom + + +class HeightScannerMarker: + """Helper class to handle visual sphere markers to show ray-casting of height scanner. + + The class uses :class:`UsdGeom.PointInstancer` for efficient handling of multiple markers in the stage. + It creates two spherical markers of different colors. Based on the indices provided the referenced + marker is activated. + + Proto Indices (status): + - 0 -> ray miss (blue sphere). + - 1 -> successful ray hit (red sphere). + - 2 -> invisible ray (disabled visualization) + + TODO: Make this generic marker for all and put inside the `omni.isaac.orbit.marker` directory. + """ + + def __init__(self, prim_path: str, count: int, radius: float = 1.0) -> None: + # check inputs + stage = stage_utils.get_current_stage() + # -- prim path + if prim_utils.is_prim_path_valid(prim_path): + prim = prim_utils.get_prim_at_path(prim_path) + if not prim.IsA(UsdGeom.PointInstancer): + raise ValueError(f"The prim at path {prim_path} cannot be parsed as a `PointInstancer` object") + self._instancer_manager = UsdGeom.PointInstancer(prim) + else: + self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + # store inputs + self.prim_path = prim_path + self.count = count + self._radius = radius + # create manager for handling instancing of frame markers + self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + # create a child prim for the marker + # -- target missed + prim = prim_utils.create_prim(f"{prim_path}/point_miss", "Sphere", attributes={"radius": self._radius}) + geom = UsdGeom.Sphere(prim) + geom.GetDisplayColorAttr().Set([(0.0, 0.0, 1.0)]) + # -- target achieved + prim = prim_utils.create_prim(f"{prim_path}/point_hit", "Sphere", attributes={"radius": self._radius}) + geom = UsdGeom.Sphere(prim) + geom.GetDisplayColorAttr().Set([(1.0, 0.0, 0.0)]) + # -- target invisible + prim = prim_utils.create_prim(f"{prim_path}/point_invisible", "Sphere", attributes={"radius": self._radius}) + geom = UsdGeom.Sphere(prim) + geom.GetDisplayColorAttr().Set([(0.0, 0.0, 1.0)]) + prim_utils.set_prim_visibility(prim, visible=False) + # add child reference to point instancer + relation_manager = self._instancer_manager.GetPrototypesRel() + relation_manager.AddTarget(f"{prim_path}/point_miss") # target index: 0 + relation_manager.AddTarget(f"{prim_path}/point_hit") # target index: 1 + relation_manager.AddTarget(f"{prim_path}/point_invisible") # target index: 2 + + # buffers for storing data in pixar Gf format + # TODO: Make them very far away from the scene? + self._proto_indices = [2] * self.count + self._gf_positions = [Gf.Vec3f(0.0, 0.0, -10.0) for _ in range(self.count)] + self._gf_orientations = [Gf.Quath() for _ in range(self.count)] + + # specify that all initial prims are related to same geometry + self._instancer_manager.GetProtoIndicesAttr().Set(self._proto_indices) + # set initial positions of the targets + self._instancer_manager.GetPositionsAttr().Set(self._gf_positions) + self._instancer_manager.GetOrientationsAttr().Set(self._gf_orientations) + + def set_visibility(self, visible: bool): + """Sets the visibility of the markers. + + The method does this through the USD API. + + Args: + visible (bool): flag to set the visibility. + """ + imageable = UsdGeom.Imageable(self._instancer_manager) + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + def set_world_poses( + self, + positions: Optional[Union[np.ndarray, torch.Tensor]] = None, + orientations: Optional[Union[np.ndarray, torch.Tensor]] = None, + indices: Optional[Sequence[int]] = None, + ): + """Update marker poses in the simulation world frame. + + Args: + positions (Optional[Union[np.ndarray, torch.Tensor]], optional): + Positions in the world frame. Shape: (M, 3). Defaults to :obj:`None`, which means left unchanged. + orientations (Optional[Union[np.ndarray, torch.Tensor]], optional): + Quaternion orientations (w, x, y, z) in the world frame of the prims. Shape: (M, 4). + Defaults to :obj:`None`, which means left unchanged. + indices (Optional[Sequence[int]], optional): Indices to specify which alter poses. + Shape: (M,), where M <= total number of markers. Defaults to :obj:`None` (i.e: all markers). + """ + # resolve inputs + if positions is not None: + positions = positions.tolist() + if orientations is not None: + orientations = orientations.tolist() + if indices is None: + indices = range(self.count) + # change marker locations + for i, marker_index in enumerate(indices): + if positions is not None: + self._gf_positions[marker_index][:] = positions[i] + if orientations is not None: + self._gf_orientations[marker_index].SetReal(orientations[i][0]) + self._gf_orientations[marker_index].SetImaginary(orientations[i][1:]) + # apply to instance manager + self._instancer_manager.GetPositionsAttr().Set(self._gf_positions) + self._instancer_manager.GetOrientationsAttr().Set(self._gf_orientations) + + def set_status(self, status: Union[List[int], np.ndarray, torch.Tensor], indices: Optional[Sequence[int]] = None): + """Updates the marker activated by the instance manager. + + Args: + status (Union[List[int], np.ndarray, torch.Tensor]): Decides which prototype marker to visualize. Shape: (M) + indices (Optional[Sequence[int]], optional): Indices to specify which alter poses. + Shape: (M,), where M <= total number of markers. Defaults to :obj:`None` (i.e: all markers). + """ + # default values + if indices is None: + indices = range(self.count) + # resolve input + if status is not list: + proto_indices = status.tolist() + else: + proto_indices = status + # change marker locations + for i, marker_index in enumerate(indices): + self._proto_indices[marker_index] = int(proto_indices[i]) + # apply to instance manager + self._instancer_manager.GetProtoIndicesAttr().Set(self._proto_indices) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/utils.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/utils.py new file mode 100644 index 0000000000..46fa5bd4a3 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/utils.py @@ -0,0 +1,97 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities to create and visualize 2D height-maps.""" + + +import matplotlib.pyplot as plt +import numpy as np +from matplotlib.axes import Axes +from matplotlib.image import AxesImage +from typing import Tuple + + +def create_points_from_grid(size: Tuple[float, float], resolution: float) -> np.ndarray: + """Creates a list of points from 2D meshgrid. + + The terrain scan is approximated with a grid map of the input resolution. + + By default, we consider the origin as the center of the local map and the scan size (X, Y) is the map size. + Given these settings, the elevation map spans from: `(- X / 2, - Y / 2)` to `(+ X / 2, + Y / 2)`. + + Example: + For a grid of size (0.2, 0.2) with resolution of 0.1, the created points will first x-axis fixed, while the + y-axis changes, i.e.: + ``` + [ + [-0.1, -0.1], [-0.1, 0.0], [-0.1, 0.1], + [0.0, -0.1], [0.0, 0.], [0.0, 0.1], + [0.1, -0.1], [0.1, 0.0], [0.1, 0.1], + ] + ``` + + Args: + size (Tuple[float, float]): The 2D scan region along x and y directions (in meters). + resolution (float): The resolution of the scanner (in meters/cell). + + Returns: + np.ndarray: A set of points of shape (N, 2) or (N, 3), where first x is fixed while y changes. + """ + # Compute the scan grid + # Note: np.arange does not include end-point when dealing with floats. That is why we add resolution. + x = np.arange(-size[0] / 2, size[0] / 2 + resolution, resolution) + y = np.arange(-size[1] / 2, size[1] / 2 + resolution, resolution) + grid = np.meshgrid(x, y, sparse=False, indexing="ij") + # Concatenate the scan grid into points array (N, 2): first x is fixed while y changes + points = np.vstack(list(map(np.ravel, grid))).T + + return points + + +def plot_height_grid( + hit_distance: np.ndarray, size: Tuple[float, float], resolution: float, ax: Axes = None +) -> AxesImage: + """Plots the sensor height-map distances using matplotlib. + + Note: + This method currently only supports if the grid is evenly spaced, i.e. the scan points are created using + `create_points_from_grid(...)` method. + + Args: + hit_distance (dict): The ray hit distance measured from the sensor. + size (Tuple[float, float]): The 2D scan region along x and y directions (in meters). + resolution (float): The resolution of the scanner (in meters/cell). + ax (Axes, optional): The current matplotlib axes to plot in.. Defaults to None. + + Returns: + AxesImage: Image axes of the created plot. + """ + # Check that request of keys has same length as available axes. + if ax is None: + # Create axes if not provided + # Setup a figure + _, ax = plt.subplots() + # turn axes off + ax.clear() + # resolve shape of the heightmap + x = np.arange(-size[0] / 2, size[0] / 2 + resolution, resolution) + y = np.arange(-size[1] / 2, size[1] / 2 + resolution, resolution) + shape = (len(x), len(y)) + # convert the map shape + heightmap = hit_distance.reshape(shape) + # plot the scanned distance + caxes = ax.imshow(heightmap, cmap="turbo", interpolation="none", vmin=0) + # set the label + ax.set_xlabel("y (m)") + ax.set_ylabel("x (m)") + # set the ticks + ax.set_xticks(np.arange(shape[1]), minor=False) + ax.set_yticks(np.arange(shape[0]), minor=False) + ax.set_xticklabels([round(value, 2) for value in y]) + ax.set_yticklabels([round(value, 2) for value in x]) + # add grid + ax.grid(color="w", linestyle="--", linewidth=1) + # return the color axes + return caxes diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/sensor_base.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/sensor_base.py new file mode 100644 index 0000000000..cabd4017fb --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/sensor_base.py @@ -0,0 +1,137 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base class for sensors in Omniverse workflows. + +This class defines an interface similar to how the RobotBase class works. +""" + + +from abc import abstractmethod +from typing import Any + + +class SensorBase: + """ + The base class for implementation of a sensor. + + Note: + These sensors are not vectorized yet. + + Attributes: + frame (int) - Frame number when the measurement took place. + timestamp (float) - Simulation time of the measurement (in seconds). + sensor_tick (float) - Simulation seconds between sensor buffers (ticks). + """ + + def __init__(self, sensor_tick: float = 0.0): + """Initialize the sensor class. + + Args: + sensor_tick (float, optional): Simulation seconds between sensor buffers. Defaults to 0.0. + """ + # Copy arguments to class + self._sensor_tick: float = sensor_tick + # Current timestamp of animation (in seconds) + self._timestamp: float = 0.0 + # Timestamp from last update + self._timestamp_last_update: float = 0.0 + # Frame number when the measurement is taken + self._frame: int = 0 + + """ + Properties + """ + + @property + def frame(self) -> int: + """Frame number when the measurement took place.""" + return self._frame + + @property + def timestamp(self) -> float: + """Simulation time of the measurement (in seconds).""" + return self._timestamp + + @property + def sensor_tick(self) -> float: + """Simulation seconds between sensor buffers (ticks).""" + return self._sensor_tick + + @property + def data(self) -> Any: + """The data from the simulated sensor.""" + return None + + """ + Helpers + """ + + def set_visibility(self, visible: bool): + """Set visibility of the instance in the scene. + + Note: + Sensors are mostly XForms which do not have any mesh associated to them. Thus, + overriding this method is optional. + + Args: + visible (bool) -- Whether to make instance visible or invisible. + """ + pass + + """ + Operations + """ + + @abstractmethod + def spawn(self, parent_prim_path: str): + """Spawns the sensor into the stage. + + Args: + parent_prim_path (str): The path of the parent prim to attach sensor to. + """ + raise NotImplementedError + + @abstractmethod + def initialize(self): + """Initializes the sensor handles and internal buffers.""" + raise NotImplementedError + + def reset(self): + """Resets the sensor internals.""" + # Set current time + self._timestamp = 0.0 + # Set zero captures + self._frame = 0 + + def update(self, dt: float, *args, **kwargs): + """Updates the buffers at sensor frequency. + + Args: + dt (float): The simulation time-step. + args (tuple): Other positional arguments passed to function :meth:`buffer()`. + kwargs (dict): Other keyword arguments passed to function :meth:`buffer()`. + """ + # Get current time + self._timestamp += dt + # Buffer the sensor data. + if (self._timestamp - self._timestamp_last_update) >= self._sensor_tick: + # Buffer the data + self.buffer(*args, **kwargs) + # Update the frame count + self._frame += 1 + # Update capture time + self._timestamp_last_update = self._timestamp + + @abstractmethod + def buffer(self, *args, **kwargs): + """Fills the buffers of the sensor data. + + This function does not perform any time-based checks and directly fills the data into the data container. + + Warning: + Although this method is public, `update(dt)` should be the preferred way of filling buffers. + """ + raise NotImplementedError diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/__init__.py new file mode 100644 index 0000000000..fa05938fb2 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/__init__.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Sub-module containing utilities for the Orbit framework. + +* `configclass`: Provides wrapper around `dataclass` for working with configurations. +* `dict`: Provides helper functions for converting dictionaries and type-cases. +* `kit`: Provides helper functions for Omniverse kit (USD operations). +* `math`: Provides helper functions for math operations. +* `string`: Provides helper functions for string operations. +* `timer`: Provides a timer class (uses contextlib) for benchmarking. +""" + +from .configclass import configclass +from .dict import class_to_dict, print_dict, update_class_from_dict, update_dict +from .string import to_camel_case, to_snake_case +from .timer import Timer + +__all__ = [ + # config wrapper + "configclass", + # dictionary utilities + "class_to_dict", + "update_class_from_dict", + "print_dict", + "update_dict", + # string utilities + "to_camel_case", + "to_snake_case", + # timer + "Timer", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/assets.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/assets.py new file mode 100644 index 0000000000..e19032880a --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/assets.py @@ -0,0 +1,77 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Defines the host-server where assets and resources are stored. + +By default, we use the Isaac Sim Nucleus Server for hosting assets and resources. This makes +distribution of the assets easier and makes the repository smaller in size code-wise. + +For more information on Omniverse Nucleus: +https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus +""" + +import io +import os + +import carb +import omni.client +import omni.isaac.core.utils.nucleus as nucleus_utils + +# 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" + ) + carb.log_error(msg) + raise RuntimeError(msg) + + +ISAAC_NUCLEUS_DIR = f"{nucleus_utils.get_assets_root_path()}/Isaac" +"""Path to the `Isaac` directory on the NVIDIA Nucleus Server.""" + +ISAAC_ORBIT_NUCLEUS_DIR = f"{nucleus_utils.get_assets_root_path()}/Isaac/Samples/Orbit" +"""Path to the `Isaac/Samples/Orbit` directory on the NVIDIA Nucleus Server.""" + + +def check_file_path(path: str) -> int: + """Checks if a file exists on the Nucleus Server or locally. + + Args: + path (str): The path to the file. + + Returns: + int: The status of the file. Possible values are: :obj:`0` if the file does not exist + :obj:`1` if the file exists locally, or :obj:`2` if the file exists on the Nucleus Server. + """ + if os.path.isfile(path): + return 1 + elif omni.client.stat(path)[0] == omni.client.Result.OK: + return 2 + else: + return 0 + + +def read_file(path: str) -> io.BytesIO: + """Reads a file from the Nucleus Server or locally. + + Args: + path (str): The path to the file. + + Raises: + FileNotFoundError: When the file not found locally or on Nucleus Server. + + Returns: + io.BytesIO: The content of the file. + """ + # check file status + file_status = check_file_path(path) + if file_status == 1: + return io.BytesIO(open(path, "rb").read()) + elif file_status == 2: + file_content = omni.client.read_file(path)[2] + return io.BytesIO(memoryview(file_content).tobytes()) + else: + raise FileNotFoundError(f"Unable to find the file: {path}") diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/configclass.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/configclass.py new file mode 100644 index 0000000000..faef30aca5 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/configclass.py @@ -0,0 +1,222 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapper around the Python 3.7 onwards `dataclasses` module.""" + + +from copy import deepcopy +from dataclasses import Field, dataclass, field +from typing import Any, Callable, ClassVar, Dict + +from .dict import class_to_dict, update_class_from_dict + +# List of all methods provided by sub-module. +__all__ = ["configclass"] + + +""" +Wrapper around dataclass. +""" + + +def __dataclass_transform__(): + """Add annotations decorator for PyLance.""" + return lambda a: a + + +@__dataclass_transform__() +def configclass(cls, **kwargs): + """Wrapper around `dataclass` functionality to add extra checks and utilities. + + As of Python3.8, the standard dataclasses have two main issues which makes them non-generic for configuration use-cases. + These include: + + 1. Requiring a type annotation for all its members. + 2. Requiring explicit usage of :meth:`field(default_factory=...)` to reinitialize mutable variables. + + This function wraps around :class:`dataclass` utility to deal with the above two issues. + + Usage: + .. code-block:: python + + from dataclasses import MISSING + + from omni.isaac.orbit.utils.configclass import configclass + + + @configclass + class ViewerCfg: + eye: list = [7.5, 7.5, 7.5] # field missing on purpose + lookat: list = field(default_factory=[0.0, 0.0, 0.0]) + + + @configclass + class EnvCfg: + num_envs: int = MISSING + episode_length: int = 2000 + viewer: ViewerCfg = ViewerCfg() + + # create configuration instance + env_cfg = EnvCfg(num_envs=24) + # print information + print(env_cfg.to_dict()) + + Reference: + https://docs.python.org/3/library/dataclasses.html#dataclasses.Field + """ + # add type annotations + _add_annotation_types(cls) + # add field factory + _process_mutable_types(cls) + # copy mutable members + setattr(cls, "__post_init__", _custom_post_init) + # add helper functions for dictionary conversion + setattr(cls, "to_dict", _class_to_dict) + setattr(cls, "from_dict", _update_class_from_dict) + # wrap around dataclass + cls = dataclass(cls, **kwargs) + # return wrapped class + return cls + + +""" +Dictionary <-> Class operations. + +These are redefined here to add new docstrings. +""" + + +def _class_to_dict(obj: object) -> Dict[str, Any]: + """Convert an object into dictionary recursively. + + Returns: + Dict[str, Any]: Converted dictionary mapping. + """ + return class_to_dict(obj) + + +def _update_class_from_dict(obj, data: Dict[str, Any]) -> None: + """Reads a dictionary and sets object variables recursively. + + This function performs in-place update of the class member attributes. + + Args: + data (Dict[str, Any]): Input (nested) dictionary to update from. + + Raises: + TypeError: When input is not a dictionary. + ValueError: When dictionary has a value that does not match default config type. + KeyError: When dictionary has a key that does not exist in the default config type. + """ + return update_class_from_dict(obj, data, _ns="") + + +""" +Private helper functions. +""" + + +def _add_annotation_types(cls): + """Add annotations to all elements in the dataclass. + + By definition in Python, a field is defined as a class variable that has a type annotation. + + In case type annotations are not provided, dataclass ignores those members when :func:`__dict__()` is called. + This function adds these annotations to the class variable to prevent any issues in case the user forgets to + specify the type annotation. + + This makes the following a feasible operation: + + @dataclass + class State: + pos = (0.0, 0.0, 0.0) + ^^ + If the function is NOT used, the following type-error is returned: + TypeError: 'pos' is a field but has no type annotation + """ + # Note: Do not change this line. `cls.__dict__.get("__annotations__", {})` is different from `cls.__annotations__` because of inheritance. + cls.__annotations__ = cls.__dict__.get("__annotations__", {}) + # cls.__annotations__ = dict() + for key in dir(cls): + # skip dunder members + if key.startswith("__"): + continue + # skip class functions + if key in ["from_dict", "to_dict"]: + continue + # add type annotations for members that are not functions + var = getattr(cls, key) + if not isinstance(var, type): + if key not in cls.__annotations__: + cls.__annotations__[key] = type(var) + + +def _process_mutable_types(cls): + """Initialize all mutable elements through :obj:`dataclasses.Field` to avoid unnecessary complaints. + + By default, dataclass requires usage of :obj:`field(default_factory=...)` to reinitialize mutable objects every time a new + class instance is created. If a member has a mutable type and it is created without specifying the `field(default_factory=...)`, + then Python throws an error requiring the usage of `default_factory`. + + Additionally, Python only explicitly checks for field specification when the type is a list, set or dict. This misses the + use-case where the type is class itself. Thus, the code silently carries a bug with it which can lead to undesirable effects. + + This function deals with this issue + + This makes the following a feasible operation: + + @dataclass + class State: + pos: list = [0.0, 0.0, 0.0] + ^^ + If the function is NOT used, the following value-error is returned: + ValueError: mutable default for field pos is not allowed: use default_factory + """ + + def _return_f(f: Any) -> Callable[[], Any]: + """Returns default function for creating mutable/immutable variables.""" + + def _wrap(): + if isinstance(f, Field): + return f.default_factory + else: + return f + + return _wrap + + for key in dir(cls): + # skip dunder members + if key.startswith("__"): + continue + # skip class functions + if key in ["from_dict", "to_dict"]: + continue + # do not create field for class variables + if key in cls.__annotations__: + origin = getattr(cls.__annotations__[key], "__origin__", None) + if origin is ClassVar: + continue + # define explicit field for data members + f = getattr(cls, key) + if not isinstance(f, type): + f = field(default_factory=_return_f(f)) + setattr(cls, key, f) + + +def _custom_post_init(obj): + """Deepcopy all elements to avoid shared memory issues for mutable objects in dataclasses initialization. + + This function is called explicitly instead of as a part of :func:`_process_mutable_types()` to prevent mapping + proxy type i.e. a read only proxy for mapping objects. The error is thrown when using hierarchical data-classes + for configuration. + """ + for key in dir(obj): + # skip dunder members + if key.startswith("__"): + continue + # duplicate data members + var = getattr(obj, key) + if not callable(var): + setattr(obj, key, deepcopy(var)) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/dict.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/dict.py new file mode 100644 index 0000000000..50b13673ea --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/dict.py @@ -0,0 +1,189 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for working with dictionaries.""" + + +import collections.abc +import importlib +from typing import Any, Callable, Dict, Iterable, Mapping + +__all__ = ["class_to_dict", "update_class_from_dict", "update_dict", "print_dict"] + +""" +Dictionary <-> Class operations. +""" + + +def class_to_dict(obj: object) -> Dict[str, Any]: + """Convert an object into dictionary recursively. + + Note: + Ignores all names starting with "__" (i.e. built-in methods). + + Args: + obj (object): An instance of a class to convert. + + Raises: + ValueError: When input argument is not an object. + + Returns: + Dict[str, Any]: Converted dictionary mapping. + """ + # check that input data is class instance + if not hasattr(obj, "__class__"): + raise ValueError(f"Expected a class instance. Received: {type(obj)}.") + # convert object to dictionary + if isinstance(obj, dict): + obj_dict = obj + else: + obj_dict = obj.__dict__ + # convert to dictionary + data = dict() + for key, value in obj_dict.items(): + # disregard builtin attributes + if key.startswith("__"): + continue + # check if attribute is callable -- function + if callable(value): + data[key] = f"{value.__module__}:{value.__name__}" + # check if attribute is a dictionary + elif hasattr(value, "__dict__") or isinstance(value, dict): + data[key] = class_to_dict(value) + else: + data[key] = value + return data + + +def update_class_from_dict(obj, data: Dict[str, Any], _ns: str = "") -> None: + """Reads a dictionary and sets object variables recursively. + + This function performs in-place update of the class member attributes. + + Args: + obj (object): An instance of a class to update. + data (Dict[str, Any]): Input dictionary to update from. + _ns (str): Namespace of the current object. This is useful for nested configuration + classes or dictionaries. Defaults to "". + + Raises: + TypeError: When input is not a dictionary. + ValueError: When dictionary has a value that does not match default config type. + KeyError: When dictionary has a key that does not exist in the default config type. + """ + for key, value in data.items(): + key_ns = _ns + "/" + key + if hasattr(obj, key): + obj_mem = getattr(obj, key) + if isinstance(obj_mem, Mapping): + # Note: We don't handle two-level nested dictionaries. Just use configclass if this is needed. + # iterate over the dictionary to look for callable values + for k, v in obj_mem.items(): + if callable(v): + value[k] = _string_to_callable(value[k]) + setattr(obj, key, value) + elif isinstance(value, Mapping): + # recursively call if it is a dictionary + update_class_from_dict(obj_mem, value, _ns=key_ns) + elif isinstance(value, Iterable) and not isinstance(value, str): + # check length of value to be safe + if len(obj_mem) != len(value) and obj_mem is not None: + raise ValueError( + f"[Config]: Incorrect length under namespace: {key_ns}. Expected: {len(obj_mem)}, Received: {len(value)}." + ) + else: + setattr(obj, key, value) + elif callable(obj_mem): + # update function name + value = _string_to_callable(value) + setattr(obj, key, value) + elif isinstance(value, type(obj_mem)): + # check that they are type-safe + setattr(obj, key, value) + else: + raise ValueError( + f"[Config]: Incorrect type under namespace: {key_ns}. Expected: {type(obj_mem)}, Received: {type(value)}." + ) + else: + raise KeyError(f"[Config]: Key not found under namespace: {key_ns}.") + + +""" +Dictionary operations. +""" + + +def update_dict(orig_dict: dict, new_dict: collections.abc.Mapping) -> dict: + """Updates existing dictionary with values from a new dictionary. + + This function mimics the dict.update() function. However, it works for + nested dictionaries as well. + + Reference: + https://stackoverflow.com/questions/3232943/update-value-of-a-nested-dictionary-of-varying-depth + + Args: + orig_dict (dict): The original dictionary to insert items to. + new_dict (collections.abc.Mapping): The new dictionary to insert items from. + + Returns: + dict: The updated dictionary. + """ + for keyname, value in new_dict.items(): + if isinstance(value, collections.abc.Mapping): + orig_dict[keyname] = update_dict(orig_dict.get(keyname, {}), value) + else: + orig_dict[keyname] = value + return orig_dict + + +def print_dict(val, nesting: int = -4, start: bool = True): + """Outputs a nested dictionary.""" + if type(val) == dict: + if not start: + print("") + nesting += 4 + for k in val: + print(nesting * " ", end="") + print(k, end=": ") + print_dict(val[k], nesting, start=False) + else: + print(val) + + +""" +Private helper functions. +""" + + +def _string_to_callable(name: str) -> Callable: + """Resolves the module and function names to return the function. + + Args: + name (str): The function name. The format should be 'module:attribute_name'. + + Raises: + ValueError: When the resolved attribute is not a function. + ValueError: _description_ + + Returns: + Callable: The function loaded from the module. + """ + try: + mod_name, attr_name = name.split(":") + mod = importlib.import_module(mod_name) + callable_object = getattr(mod, attr_name) + # check if attribute is callable + if callable(callable_object): + return callable_object + else: + raise ValueError(f"The imported object is not callable: '{name}'") + except AttributeError as e: + msg = ( + "While updating the config from a dictionary, we could not interpret the entry" + "as a callable object. The format of input should be 'module:attribute_name'\n" + f"While processing input '{name}', received the error:\n {e}." + ) + raise ValueError(msg) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/__init__.py new file mode 100644 index 0000000000..0037b5aa83 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodules for files IO operations. +""" + +from .pkl import dump_pickle, load_pickle +from .yaml import dump_yaml, load_yaml + +__all__ = ["load_pickle", "dump_pickle", "load_yaml", "dump_yaml"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/pkl.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/pkl.py new file mode 100644 index 0000000000..38b7735cb7 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/pkl.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import pickle +from typing import Any + + +def load_pickle(filename: str) -> Any: + """Loads an input PKL file safely. + + Args: + filename (str): The path to pickled file. + + Raises: + FileNotFoundError: When the specified file does not exist. + + Returns: + Any: 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 (str): The path to save the file at. + data (Any): 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/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/yaml.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/yaml.py new file mode 100644 index 0000000000..402c2cec66 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/io/yaml.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import yaml +from typing import Dict, Union + +from omni.isaac.orbit.utils import class_to_dict + + +def load_yaml(filename: str) -> Dict: + """Loads an input PKL file safely. + + Args: + filename (str): The path to pickled file. + + Raises: + FileNotFoundError: When the specified file does not exist. + + Returns: + Dict: The data read from the input file. + """ + if not os.path.exists(filename): + raise FileNotFoundError(f"File not found: {filename}") + with open(filename) as f: + data = yaml.full_load(f) + return data + + +def dump_yaml(filename: str, data: Union[Dict, object]): + """Saves data into a YAML file safely. + + Note: + The function creates any missing directory along the file's path. + + Args: + filename (str): The path to save the file at. + data (Union[Dict, object]): The data to save either a dictionary or class object. + """ + # check ending + if not filename.endswith("yaml"): + filename += ".yaml" + # create directory + if not os.path.exists(os.path.dirname(filename)): + os.makedirs(os.path.dirname(filename), exist_ok=True) + # convert data into dictionary + if not isinstance(data, dict): + data = class_to_dict(data) + # save data + with open(filename, "w") as f: + yaml.dump(data, f, default_flow_style=None) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/kit.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/kit.py new file mode 100644 index 0000000000..a3fc5e9693 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/kit.py @@ -0,0 +1,553 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from typing import Optional, Sequence + +import carb +import omni.isaac.core.utils.nucleus as nucleus_utils +import omni.isaac.core.utils.prims as prim_utils +import omni.kit +from omni.isaac.core.materials import PhysicsMaterial +from omni.isaac.core.prims import GeometryPrim +from pxr import Gf, PhysxSchema, UsdPhysics + + +def create_ground_plane( + prim_path: str, + z_position: float = 0.0, + static_friction: float = 1.0, + dynamic_friction: float = 1.0, + restitution: float = 0.0, + color: Optional[Sequence[float]] = (0.065, 0.0725, 0.080), + **kwargs, +): + """Spawns a ground plane into the scene. + + This method spawns the default ground plane (grid plane) from Isaac Sim into the scene. + It applies a physics material to the ground plane and sets the color of the ground plane. + + Args: + prim_path (str): The prim path to spawn the ground plane at. + z_position (float, optional): The z-location of the plane. Defaults to 0. + static_friction (float, optional): The static friction coefficient. Defaults to 1.0. + dynamic_friction (float, optional): The dynamic friction coefficient. Defaults to 1.0. + restitution (float, optional): The coefficient of restitution. Defaults to 0.0. + color (Optional[Sequence[float]], optional): The color of the ground plane. + Defaults to (0.065, 0.0725, 0.080). + + Keyword Args: + usd_path (str): The USD path to the ground plane. Defaults to the asset path + `Isaac/Environments/Grid/default_environment.usd` on the Isaac Sim Nucleus server. + improve_patch_friction (bool): Whether to enable patch friction. Defaults to False. + combine_mode (str): Determines the way physics materials will be combined during collisions. + Available options are `average`, `min`, `multiply`, `multiply`, and `max`. Defaults to `average`. + light_intensity (Optional[float]): The power intensity of the light source. Defaults to 1e7. + light_radius (Optional[float]): The radius of the light source. Defaults to 50.0. + """ + # Retrieve path to the plane + if "usd_path" in kwargs: + usd_path = kwargs["usd_path"] + else: + # get path to the nucleus server + assets_root_path = nucleus_utils.get_assets_root_path() + if assets_root_path is None: + carb.log_error("Unable to access the Isaac Sim assets folder on Nucleus server.") + return + # prepend path to the grid plane + usd_path = f"{assets_root_path}/Isaac/Environments/Grid/default_environment.usd" + # Spawn Ground-plane + prim_utils.create_prim(prim_path, usd_path=usd_path, translation=(0.0, 0.0, z_position)) + # Create physics material + material = PhysicsMaterial(f"{prim_path}/groundMaterial", static_friction, dynamic_friction, restitution) + # Apply PhysX Rigid Material schema + physx_material_api = PhysxSchema.PhysxMaterialAPI.Apply(material.prim) + # Set patch friction property + improve_patch_friction = kwargs.get("improve_patch_friction", False) + physx_material_api.CreateImprovePatchFrictionAttr().Set(improve_patch_friction) + # Set combination mode for coefficients + combine_mode = kwargs.get("combine_mode", "average") + physx_material_api.CreateFrictionCombineModeAttr().Set(combine_mode) + physx_material_api.CreateRestitutionCombineModeAttr().Set(combine_mode) + # 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" + ) + ) + GeometryPrim(collision_prim_path, collision=True).apply_physics_material(material) + # Change the color of the plane + # Warning: This is specific to the default grid plane asset. + if color is not None: + omni.kit.commands.execute( + "ChangeProperty", + prop_path=f"{prim_path}/Looks/theGrid.inputs:diffuse_tint", + value=Gf.Vec3f(*color), + prev=None, + ) + # Change the settings of the sphere light + # By default, the one from Isaac Sim is too dim for large number of environments. + # Warning: This is specific to the default grid plane asset. + light_intensity = kwargs.get("light_intensity", 1e7) + light_radius = kwargs.get("light_radius", 100.0) + # -- light intensity + if light_intensity is not None: + omni.kit.commands.execute( + "ChangeProperty", + prop_path=f"{prim_path}/SphereLight.intensity", + value=light_intensity, + prev=None, + ) + # -- light radius + if light_radius is not None: + # set radius of the light + omni.kit.commands.execute( + "ChangeProperty", + prop_path=f"{prim_path}/SphereLight.radius", + value=light_radius, + prev=None, + ) + # as a rule of thumb, we set the sphere center 1.5 times + omni.kit.commands.execute( + "ChangeProperty", + prop_path=f"{prim_path}/SphereLight.xformOp:translate", + value=(0.0, 0.0, 1.5 * light_radius), + prev=None, + ) + # -- ambient light + ambient_light = kwargs.get("ambient_light", True) + if ambient_light: + prim_utils.create_prim( + f"{prim_path}/AmbientLight", + "DistantLight", + attributes={"intensity": 600.0}, + ) + + +def move_nested_prims(source_ns: str, target_ns: str): + """Moves all prims from source namespace to target namespace. + + This function also moves all the references inside the source prim path + to the target prim path. + + Args: + source_ns (str): The source prim path. + target_ns (str): The target prim path. + """ + # check if target namespace exists + prim_utils.define_prim(target_ns) + # move all children prim from source namespace + prim = prim_utils.get_prim_at_path(source_ns) + for children in prim.GetChildren(): + orig_prim_path = prim_utils.get_prim_path(children) + new_prim_path = orig_prim_path.replace(source_ns, target_ns) + prim_utils.move_prim(orig_prim_path, new_prim_path) + + +def set_drive_dof_properties( + prim_path: str, + dof_name: str, + stiffness: Optional[float] = None, + damping: Optional[float] = None, + max_velocity: Optional[float] = None, + max_force: Optional[float] = None, +) -> None: + """Set the DOF properties of a drive on an articulation. + + Args: + prim_path (str): The prim path to the articulation root. + dof_name (str): The name of the DOF/joint. + stiffness (Optional[float]): The stiffness of the drive. + damping (Optional[float]): The damping of the drive. + max_velocity (Optional[float]): The max velocity of the drive. + max_force (Optional[float]): The max effort of the drive. + + Raises: + ValueError: When no joint of given name found under the provided prim path. + """ + # find matching prim path for the dof name + dof_prim = prim_utils.get_first_matching_child_prim(prim_path, lambda x: dof_name in x) + if not dof_prim.IsValid(): + raise ValueError(f"No joint named '{dof_name}' found in articulation '{prim_path}'.") + # obtain the dof drive type + if dof_prim.IsA(UsdPhysics.RevoluteJoint): + drive_type = "angular" + elif dof_prim.IsA(UsdPhysics.PrismaticJoint): + drive_type = "linear" + else: + # get joint USD prim + dof_prim_path = prim_utils.get_prim_path(dof_prim) + raise ValueError(f"The joint at path '{dof_prim_path}' is not linear or angular.") + + # convert to USD Physics drive + if dof_prim.HasAPI(UsdPhysics.DriveAPI): + drive_api = UsdPhysics.DriveAPI(dof_prim, drive_type) + else: + drive_api = UsdPhysics.DriveAPI.Apply(dof_prim, drive_type) + # convert DOF type to be force + if not drive_api.GetTypeAttr(): + drive_api.CreateTypeAttr().Set("force") + else: + drive_api.GetTypeAttr().Set("force") + + # set stiffness of the drive + if stiffness is not None: + # convert from radians to degrees + # note: gains have units "per degrees" + if drive_type == "angular": + stiffness = stiffness * math.pi / 180 + # set property + if not drive_api.GetStiffnessAttr(): + drive_api.CreateStiffnessAttr(stiffness) + else: + drive_api.GetStiffnessAttr().Set(stiffness) + # set damping of the drive + if damping is not None: + # convert from radians to degrees + # note: gains have units "per degrees" + if drive_type == "angular": + damping = damping * math.pi / 180 + # set property + if not drive_api.GetDampingAttr(): + drive_api.CreateDampingAttr(damping) + else: + drive_api.GetDampingAttr().Set(damping) + # set maximum force + if max_force is not None: + if not drive_api.GetMaxForceAttr(): + drive_api.CreateMaxForceAttr(max_force) + else: + drive_api.GetMaxForceAttr().Set(max_force) + + # convert to physx schema + drive_schema = PhysxSchema.PhysxJointAPI(dof_prim) + # set maximum velocity + if max_velocity is not None: + # convert from radians to degrees + if drive_type == "angular": + max_velocity = math.degrees(max_velocity) + # set property + if not drive_schema.GetMaxJointVelocityAttr(): + drive_schema.CreateMaxJointVelocityAttr(max_velocity) + else: + drive_schema.GetMaxJointVelocityAttr().Set(max_velocity) + + +def set_articulation_properties( + prim_path: str, + articulation_enabled: Optional[bool] = None, + solver_position_iteration_count: Optional[int] = None, + solver_velocity_iteration_count: Optional[int] = None, + sleep_threshold: Optional[float] = None, + stabilization_threshold: Optional[float] = None, + enable_self_collisions: Optional[bool] = None, +) -> None: + """Set PhysX parameters for an articulation prim. + + Args: + prim_path (str): The prim path to the articulation root. + articulation_enabled (Optional[bool]): Whether the articulation should be enabled/disabled. + solver_position_iteration_count (Optional[int]): Solver position iteration counts for the body. + solver_velocity_iteration_count (Optional[int]): Solver velocity iteration counts for the body. + sleep_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an + actor may go to sleep. + stabilization_threshold (Optional[float]): The mass-normalized kinetic energy threshold below + which an articulation may participate in stabilization. + enable_self_collisions (Optional[bool]): Boolean defining whether self collisions should be + enabled or disabled. + + Raises: + ValueError: When no articulation schema found at specified articulation path. + """ + # get articulation USD prim + articulation_prim = prim_utils.get_prim_at_path(prim_path) + # check if prim has articulation applied on it + if not UsdPhysics.ArticulationRootAPI(articulation_prim): + raise ValueError(f"No articulation schema present for prim '{prim_path}'.") + # retrieve the articulation api + physx_articulation_api = PhysxSchema.PhysxArticulationAPI(articulation_prim) + if not physx_articulation_api: + physx_articulation_api = PhysxSchema.PhysxArticulationAPI.Apply(articulation_prim) + # set enable/disable rigid body API + if articulation_enabled is not None: + physx_articulation_api.GetArticulationEnabledAttr().Set(articulation_enabled) + # set solver position iteration + if solver_position_iteration_count is not None: + physx_articulation_api.GetSolverPositionIterationCountAttr().Set(solver_position_iteration_count) + # set solver velocity iteration + if solver_velocity_iteration_count is not None: + physx_articulation_api.GetSolverVelocityIterationCountAttr().Set(solver_velocity_iteration_count) + # set sleep threshold + if sleep_threshold is not None: + physx_articulation_api.GetSleepThresholdAttr().Set(sleep_threshold) + # set stabilization threshold + if stabilization_threshold is not None: + physx_articulation_api.GetStabilizationThresholdAttr().Set(stabilization_threshold) + # set self collisions + if enable_self_collisions is not None: + physx_articulation_api.GetEnabledSelfCollisionsAttr().Set(enable_self_collisions) + + +def set_rigid_body_properties( + prim_path: str, + rigid_body_enabled: Optional[bool] = None, + solver_position_iteration_count: Optional[int] = None, + solver_velocity_iteration_count: Optional[int] = None, + linear_damping: Optional[float] = None, + angular_damping: Optional[float] = None, + max_linear_velocity: Optional[float] = None, + max_angular_velocity: Optional[float] = None, + sleep_threshold: Optional[float] = None, + stabilization_threshold: Optional[float] = None, + max_depenetration_velocity: Optional[float] = None, + max_contact_impulse: Optional[float] = None, + enable_gyroscopic_forces: Optional[bool] = None, + disable_gravity: Optional[bool] = None, + retain_accelerations: Optional[bool] = None, +): + """Set PhysX parameters for a rigid body prim. + + Args: + prim_path (str): The prim path to the rigid body. + rigid_body_enabled (Optional[bool]): Whether to enable or disable rigid body API. + solver_position_iteration_count (Optional[int]): Solver position iteration counts for the body. + solver_velocity_iteration_count (Optional[int]): Solver velocity iteration counts for the body. + linear_damping (Optional[float]): Linear damping coefficient. + angular_damping (Optional[float]): Angular damping coefficient. + max_linear_velocity (Optional[float]): Max allowable linear velocity for rigid body. + max_angular_velocity (Optional[float]): Max allowable angular velocity for rigid body. + sleep_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an actor + may go to sleep. + stabilization_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which + an actor may participate in stabilization. + max_depenetration_velocity (Optional[float]): The maximum depenetration velocity permitted to + be introduced by the solver. + max_contact_impulse (Optional[float]): The limit on the impulse that may be applied at a contact. + enable_gyroscopic_forces (Optional[bool]): Enables computation of gyroscopic forces on the + rigid body. + disable_gravity (Optional[bool]): Disable gravity for the actor. + retain_accelerations (Optional[bool]): Carries over forces/accelerations over sub-steps. + + Raises: + ValueError: When no rigid-body schema found at specified prim path. + """ + # get rigid-body USD prim + rigid_body_prim = prim_utils.get_prim_at_path(prim_path) + # check if prim has rigid-body applied on it + if not UsdPhysics.RigidBodyAPI(rigid_body_prim): + raise ValueError(f"No rigid body schema present for prim '{prim_path}'.") + else: + usd_rigid_body_api = UsdPhysics.RigidBodyAPI(rigid_body_prim) + # retrieve the rigid-body api + physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI(rigid_body_prim) + if not physx_rigid_body_api: + physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI.Apply(rigid_body_prim) + # set enable/disable rigid body API + if rigid_body_enabled is not None: + usd_rigid_body_api.GetRigidBodyEnabledAttr().Set(rigid_body_enabled) + # set solver position iteration + if solver_position_iteration_count is not None: + physx_rigid_body_api.GetSolverPositionIterationCountAttr().Set(solver_position_iteration_count) + # set solver velocity iteration + if solver_velocity_iteration_count is not None: + physx_rigid_body_api.GetSolverVelocityIterationCountAttr().Set(solver_velocity_iteration_count) + # set linear damping + if linear_damping is not None: + physx_rigid_body_api.GetLinearDampingAttr().Set(linear_damping) + # set angular damping + if angular_damping is not None: + physx_rigid_body_api.GetAngularDampingAttr().Set(angular_damping) + # set max linear velocity + if max_linear_velocity is not None: + physx_rigid_body_api.GetMaxLinearVelocityAttr().Set(max_linear_velocity) + # set max angular velocity + if max_angular_velocity is not None: + max_angular_velocity = math.degrees(max_angular_velocity) + physx_rigid_body_api.GetMaxAngularVelocityAttr().Set(max_angular_velocity) + # set sleep threshold + if sleep_threshold is not None: + physx_rigid_body_api.GetSleepThresholdAttr().Set(sleep_threshold) + # set stabilization threshold + if stabilization_threshold is not None: + physx_rigid_body_api.GetStabilizationThresholdAttr().Set(stabilization_threshold) + # set max depenetration velocity + if max_depenetration_velocity is not None: + physx_rigid_body_api.GetMaxDepenetrationVelocityAttr().Set(max_depenetration_velocity) + # set max contact impulse + if max_contact_impulse is not None: + physx_rigid_body_api.GetMaxContactImpulseAttr().Set(max_contact_impulse) + # set enable gyroscopic forces + if enable_gyroscopic_forces is not None: + physx_rigid_body_api.GetEnableGyroscopicForcesAttr().Set(enable_gyroscopic_forces) + # set disable gravity + if disable_gravity is not None: + physx_rigid_body_api.GetDisableGravityAttr().Set(disable_gravity) + # set retain accelerations + if retain_accelerations is not None: + physx_rigid_body_api.GetRetainAccelerationsAttr().Set(retain_accelerations) + + +def set_collision_properties( + prim_path: str, + collision_enabled: Optional[bool] = None, + contact_offset: Optional[float] = None, + rest_offset: Optional[float] = None, + torsional_patch_radius: Optional[float] = None, + min_torsional_patch_radius: Optional[float] = None, +): + """Set PhysX properties of collider prim. + + Args: + prim_path (str): The prim path of parent. + collision_enabled (Optional[bool], optional): Whether to enable/disable collider. + contact_offset (Optional[float], optional): Contact offset of a collision shape. + rest_offset (Optional[float], optional): Rest offset of a collision shape. + torsional_patch_radius (Optional[float], optional): Defines the radius of the contact patch + used to apply torsional friction. + min_torsional_patch_radius (Optional[float], optional): Defines the minimum radius of the + contact patch used to apply torsional friction. + + Raises: + ValueError: When no collision schema found at specified prim path. + """ + # get USD prim + collider_prim = prim_utils.get_prim_at_path(prim_path) + # check if prim has collision applied on it + if not UsdPhysics.CollisionAPI(collider_prim): + raise ValueError(f"No collider schema present for prim '{prim_path}'.") + # retrieve the collision api + physx_collision_api = PhysxSchema.PhysxCollisionAPI(collider_prim) + if not physx_collision_api: + physx_collision_api = PhysxSchema.PhysxCollisionAPI.Apply(collider_prim) + # set enable/disable collision API + if collision_enabled is not None: + physx_collision_api.GetCollisionEnabledAttr().Set(collision_enabled) + # set contact offset + if contact_offset is not None: + physx_collision_api.GetContactOffsetAttr().Set(contact_offset) + # set rest offset + if rest_offset is not None: + physx_collision_api.GetRestOffsetAttr().Set(rest_offset) + # set torsional patch radius + if torsional_patch_radius is not None: + physx_collision_api.GetTorsionalPatchRadiusAttr().Set(torsional_patch_radius) + # set min torsional patch radius + if min_torsional_patch_radius is not None: + physx_collision_api.GetMinTorsionalPatchRadiusAttr().Set(min_torsional_patch_radius) + + +def set_nested_articulation_properties(prim_path: str, **kwargs) -> None: + """Set PhysX parameters on all articulations under specified prim-path. + + Note: + Check the method meth:`set_articulation_properties` for keyword arguments. + + Args: + prim_path (str): The prim path under which to search and apply articulation properties. + + Keyword Args: + articulation_enabled (Optional[bool]): Whether the articulation should be enabled/disabled. + solver_position_iteration_count (Optional[int]): Solver position iteration counts for the body. + solver_velocity_iteration_count (Optional[int]): Solver velocity iteration counts for the body. + sleep_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an + actor may go to sleep. + stabilization_threshold (Optional[float]): The mass-normalized kinetic energy threshold below + which an articulation may participate in stabilization. + enable_self_collisions (Optional[bool]): Boolean defining whether self collisions should be + enabled or disabled. + """ + # get USD prim + prim = prim_utils.get_prim_at_path(prim_path) + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + # set articulation properties + try: + set_articulation_properties(prim_utils.get_prim_path(child_prim), **kwargs) + except ValueError: + pass + # add all children to tree + all_prims += child_prim.GetChildren() + + +def set_nested_rigid_body_properties(prim_path: str, **kwargs): + """Set PhysX parameters on all rigid bodies under specified prim-path. + + Note: + Check the method meth:`set_rigid_body_properties` for keyword arguments. + + Args: + prim_path (str): The prim path under which to search and apply rigid-body properties. + + Keyword Args: + rigid_body_enabled (Optional[bool]): Whether to enable or disable rigid body API. + solver_position_iteration_count (Optional[int]): Solver position iteration counts for the body. + solver_velocity_iteration_count (Optional[int]): Solver velocity iteration counts for the body. + linear_damping (Optional[float]): Linear damping coefficient. + angular_damping (Optional[float]): Angular damping coefficient. + max_linear_velocity (Optional[float]): Max allowable linear velocity for rigid body. + max_angular_velocity (Optional[float]): Max allowable angular velocity for rigid body. + sleep_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an actor + may go to sleep. + stabilization_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which + an actor may participate in stabilization. + max_depenetration_velocity (Optional[float]): The maximum depenetration velocity permitted to + be introduced by the solver. + max_contact_impulse (Optional[float]): The limit on the impulse that may be applied at a contact. + enable_gyroscopic_forces (Optional[bool]): Enables computation of gyroscopic forces on the + rigid body. + disable_gravity (Optional[bool]): Disable gravity for the actor. + retain_accelerations (Optional[bool]): Carries over forces/accelerations over sub-steps. + """ + # get USD prim + prim = prim_utils.get_prim_at_path(prim_path) + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + # set rigid-body properties + try: + set_rigid_body_properties(prim_utils.get_prim_path(child_prim), **kwargs) + except ValueError: + pass + # add all children to tree + all_prims += child_prim.GetChildren() + + +def set_nested_collision_properties(prim_path: str, **kwargs): + """Set the collider properties of all meshes under a specified prim path. + + Note: + Check the method meth:`set_collision_properties` for keyword arguments. + + Args: + prim_path (str): The prim path under which to search and apply collider properties. + + Keyword Args: + collision_enabled (Optional[bool], optional): Whether to enable/disable collider. + contact_offset (Optional[float], optional): Contact offset of a collision shape. + rest_offset (Optional[float], optional): Rest offset of a collision shape. + torsional_patch_radius (Optional[float], optional): Defines the radius of the contact patch + used to apply torsional friction. + min_torsional_patch_radius (Optional[float], optional): Defines the minimum radius of the + contact patch used to apply torsional friction. + """ + # get USD prim + prim = prim_utils.get_prim_at_path(prim_path) + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + # set collider properties + try: + set_collision_properties(prim_utils.get_prim_path(child_prim), **kwargs) + except ValueError: + pass + # add all children to tree + all_prims += child_prim.GetChildren() diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/math.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/math.py new file mode 100644 index 0000000000..6c87a7ad35 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/math.py @@ -0,0 +1,587 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Provides utilities for math operations. + +Some of these are imported from the module `omni.isaac.core.utils.torch` for convenience. +""" + + +import numpy as np +import torch +import torch.nn.functional +from typing import Optional, Sequence, Tuple, Union + +from omni.isaac.core.utils.torch.maths import normalize, scale_transform, unscale_transform +from omni.isaac.core.utils.torch.rotations import ( + quat_apply, + quat_conjugate, + quat_from_angle_axis, + quat_mul, + quat_rotate, + quat_rotate_inverse, +) + +__all__ = [ + # General + "wrap_to_pi", + "saturate", + "normalize", + "scale_transform", + "unscale_transform", + "copysign", + # Rotation + "quat_mul", + "quat_inv", + "quat_conjugate", + "quat_apply", + "quat_rotate", + "quat_rotate_inverse", + "quat_from_euler_xyz", + "quat_apply_yaw", + "quat_box_minus", + "euler_xyz_from_quat", + "axis_angle_from_quat", + # Transformations + "combine_frame_transforms", + "subtract_frame_transforms", + "compute_pose_error", + "apply_delta_pose", + # Sampling + "default_orientation", + "random_orientation", + "random_yaw_orientation", + "sample_triangle", + "sample_uniform", +] + +""" +General +""" + + +@torch.jit.script +def wrap_to_pi(angles: torch.Tensor) -> torch.Tensor: + """Wraps input angles (in radians) to the range [-pi, pi]. + + Args: + angles (torch.Tensor): Input angles. + + Returns: + torch.Tensor: Angles in the range [-pi, pi]. + """ + angles %= 2 * np.pi + angles -= 2 * np.pi * (angles > np.pi) + return angles + + +@torch.jit.script +def saturate(x: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor) -> torch.Tensor: + """Clamps a given input tensor to (lower, upper). + + It uses pytorch broadcasting functionality to deal with batched input. + + Args: + x: Input tensor of shape (N, dims). + lower: The minimum value of the tensor. Shape (dims,) + upper: The maximum value of the tensor. Shape (dims,) + + Returns: + Clamped transform of the tensor. Shape (N, dims) + """ + return torch.max(torch.min(x, upper), lower) + + +@torch.jit.script +def copysign(mag: float, other: torch.Tensor) -> torch.Tensor: + """Create a new floating-point tensor with the magnitude of input and the sign of other, element-wise. + + Note: + The implementation follows from `torch.copysign`. The function allows a scalar magnitude. + + Args: + mag (float): The magnitude scalar. + other (torch.Tensor): The tensor containing values whose signbits are applied to magnitude. + + Returns: + torch.Tensor: The output tensor. + """ + + mag = torch.tensor(mag, device=other.device, dtype=torch.float).repeat(other.shape[0]) + return torch.abs(mag) * torch.sign(other) + + +""" +Rotation +""" + + +@torch.jit.script +def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor: + """Convert rotations given as quaternions to rotation matrices. + + Args: + quaternions: quaternions with real part first, + as tensor of shape (..., 4). + Returns: + Rotation matrices as tensor of shape (..., 3, 3). + + Reference: + Based on PyTorch3D (https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L41-L70) + """ + r, i, j, k = torch.unbind(quaternions, -1) + # pyre-fixme[58]: `/` is not supported for operand types `float` and `Tensor`. + two_s = 2.0 / (quaternions * quaternions).sum(-1) + + o = torch.stack( + ( + 1 - two_s * (j * j + k * k), + two_s * (i * j - k * r), + two_s * (i * k + j * r), + two_s * (i * j + k * r), + 1 - two_s * (i * i + k * k), + two_s * (j * k - i * r), + two_s * (i * k - j * r), + two_s * (j * k + i * r), + 1 - two_s * (i * i + j * j), + ), + -1, + ) + return o.reshape(quaternions.shape[:-1] + (3, 3)) + + +def convert_quat( + quat: Union[torch.tensor, Sequence[float]], to: Optional[str] = "xyzw" +) -> Union[torch.tensor, np.ndarray]: + """Converts quaternion from one convention to another. + + The convention to convert TO is specified as an optional argument. If to == 'xyzw', + then the input is in 'wxyz' format, and vice-versa. + + Args: + quat (Union[torch.tensor, Sequence[float]]): Input quaternion of shape (..., 4). + to (Optional[str], optional): Convention to convert quaternion to.. Defaults to "xyzw". + + Raises: + ValueError: Invalid input argument `to`, i.e. not "xyzw" or "wxyz". + ValueError: Invalid shape of input `quat`, i.e. not (..., 4,). + + Returns: + Union[torch.tensor, np.ndarray]: The converted quaternion in specified convention. + """ + # convert to numpy (sanity check) + if not isinstance(quat, torch.Tensor): + quat = np.asarray(quat) + # check input is correct + if quat.shape[-1] != 4: + msg = f"convert_quat(): Expected input quaternion shape mismatch: {quat.shape} != (..., 4)." + raise ValueError(msg) + # convert to specified quaternion type + if to == "xyzw": + return quat[..., [1, 2, 3, 0]] + elif to == "wxyz": + return quat[..., [3, 0, 1, 2]] + else: + raise ValueError("convert_quat(): Choose a valid `to` argument (xyzw or wxyz).") + + +@torch.jit.script +def quat_inv(q: torch.Tensor) -> torch.Tensor: + """Compute the inverse of a quaternion. + + Args: + q (torch.Tensor): The input quaternion (w, x, y, z). + + Returns: + torch.Tensor: The inverse quaternion (w, x, y, z). + """ + return normalize(quat_conjugate(q)) + + +@torch.jit.script +def quat_from_euler_xyz(roll: torch.Tensor, pitch: torch.Tensor, yaw: torch.Tensor) -> torch.Tensor: + """Convert rotations given as Euler angles in radians to Quaternions. + + Note: + The euler angles are assumed in XYZ convention. + + Args: + roll: Rotation around x-axis (in radians). Shape: [N,] + pitch: Rotation around y-axis (in radians). Shape: [N,] + yaw: Rotation around z-axis (in radians). Shape: [N,] + + Returns: + torch.Tensor: Quaternion with real part in the start. Shape: [N, 4,] + """ + cy = torch.cos(yaw * 0.5) + sy = torch.sin(yaw * 0.5) + cr = torch.cos(roll * 0.5) + sr = torch.sin(roll * 0.5) + cp = torch.cos(pitch * 0.5) + sp = torch.sin(pitch * 0.5) + # compute quaternion + qw = cy * cr * cp + sy * sr * sp + qx = cy * sr * cp - sy * cr * sp + qy = cy * cr * sp + sy * sr * cp + qz = sy * cr * cp - cy * sr * sp + + return torch.stack([qw, qx, qy, qz], dim=-1) + + +@torch.jit.script +def euler_xyz_from_quat(quat: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """ + Convert rotations given as quaternions to Euler angles in radians. + + Note: + The euler angles are assumed in XYZ convention. + + Reference: + https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + + Args: + quat: Quaternion with real part in the start. Shape: [N, 4,] + + Returns: + Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: A tuple containing roll-pitch-yaw. + """ + q_w, q_x, q_y, q_z = quat[:, 0], quat[:, 1], quat[:, 2], quat[:, 3] + # roll (x-axis rotation) + sin_roll = 2.0 * (q_w * q_x + q_y * q_z) + cos_roll = 1 - 2 * (q_x * q_x + q_y * q_y) + roll = torch.atan2(sin_roll, cos_roll) + + # pitch (y-axis rotation) + sin_pitch = 2.0 * (q_w * q_y - q_z * q_x) + pitch = torch.where(torch.abs(sin_pitch) >= 1, copysign(np.pi / 2.0, sin_pitch), torch.asin(sin_pitch)) + + # yaw (z-axis rotation) + sin_yaw = 2.0 * (q_w * q_z + q_x * q_y) + cos_yaw = 1 - 2 * (q_y * q_y + q_z * q_z) + yaw = torch.atan2(sin_yaw, cos_yaw) + + return roll % (2 * np.pi), pitch % (2 * np.pi), yaw % (2 * np.pi) + + +@torch.jit.script +def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: + """Rotate a vector only around the yaw-direction. + + Args: + quat (torch.Tensor): Input orientation to extract yaw from. + vec (torch.Tensor): Input vector. + + Returns: + torch.Tensor: Rotated vector. + """ + quat_yaw = quat.clone().view(-1, 4) + quat_yaw[:, 1:3] = 0.0 # set x, y components as zero + quat_yaw = normalize(quat_yaw) + return quat_apply(quat_yaw, vec) + + +@torch.jit.script +def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """Implements box-minus operator (quaternion difference) + + Args: + q1 (torch.Tensor): A (N, 4) tensor for quaternion (x, y, z, w) + q2 (torch.Tensor): A (N, 4) tensor for quaternion (x, y, z, w) + + Returns: + torch.Tensor: q1 box-minus q2 + + Reference: + https://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf + """ + 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.atan(norm_im / re) / norm_im, torch.sign(re)) + return scale.unsqueeze(-1) * im + + +@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 (torch.Tensor): quaternions with real part first, as tensor of shape (..., 4). + eps (float): The tolerance for Taylor approximation. Defaults to 1.0e-6. + + Returns: + torch.Tensor: Rotations given as a vector in axis angle form, as a tensor + of shape (..., 3), where the magnitude is the angle turned + anti-clockwise in radians around the vector's direction. + + Reference: + Based on PyTorch3D (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 + + 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( + torch.abs(angle.abs()) > eps, torch.sin(half_angle) / angle, 0.5 - angle * angle / 48 + ) + return quat[..., 1:4] / sin_half_angles_over_angles.unsqueeze(-1) + + +""" +Transformations +""" + + +@torch.jit.script +def combine_frame_transforms( + t01: torch.Tensor, q01: torch.Tensor, t12: torch.Tensor = None, q12: torch.Tensor = None +) -> Tuple[torch.Tensor, torch.Tensor]: + """Combine transformations between two reference frames into a stationary frame. + + It performs the following transformation operation: :math:`T_{02} = T_{01} \\times T_{12}`, + where :math:`T_{AB}` is the homogenous transformation matrix from frame A to B. + + Args: + t01 (torch.Tensor): Position of frame 1 w.r.t. frame 0. + q01 (torch.Tensor): Quaternion orientation of frame 1 w.r.t. frame 0. + t12 (torch.Tensor): Position of frame 2 w.r.t. frame 1. + q12 (torch.Tensor): Quaternion orientation of frame 2 w.r.t. frame 1. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: A tuple containing the position and orientation of + frame 2 w.r.t. frame 0. + """ + # compute orientation + if q12 is not None: + q02 = quat_mul(q01, q12) + else: + q02 = q01 + # compute translation + if t12 is not None: + t02 = t01 + quat_apply(q01, t12) + else: + t02 = t01 + + return t02, q02 + + +@torch.jit.script +def subtract_frame_transforms( + t01: torch.Tensor, q01: torch.Tensor, t02: torch.Tensor, q02: torch.Tensor +) -> Tuple[torch.Tensor, torch.Tensor]: + """Subtract transformations between two reference frames into a stationary frame. + + It performs the following transformation operation: :math:`T_{12} = T_{01}^{-1} \\times T_{02}`, + where :math:`T_{AB}` is the homogenous transformation matrix from frame A to B. + + Args: + t01 (torch.Tensor): Position of frame 1 w.r.t. frame 0. + q01 (torch.Tensor): Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). + t02 (torch.Tensor): Position of frame 2 w.r.t. frame 0. + q02 (torch.Tensor): Quaternion orientation of frame 2 w.r.t. frame 0 in (w, x, y, z). + + Returns: + Tuple[torch.Tensor, torch.Tensor]: A tuple containing the position and orientation of + frame 2 w.r.t. frame 1. + """ + # compute orientation + q12 = quat_mul(quat_inv(q01), q02) + # compute translation + t12 = t02 - t01 + + return t12, q12 + + +@torch.jit.script +def compute_pose_error(t01: torch.Tensor, q01: torch.Tensor, t02: torch.Tensor, q02: torch.Tensor, rot_error_type: str): + """Compute the position and orientation error between source and target frames. + + Args: + t01 (torch.Tensor): Position of source frame. + q01 (torch.Tensor): Quaternion orientation of source frame. + t02 (torch.Tensor): Position of target frame. + q02 (torch.Tensor): Quaternion orientation of target frame. + rot_error_type (str): The rotation error type to return: "quat", "axis_angle". + + Returns: + Tuple[torch.Tensor, torch.Tensor]: A tuple containing position and orientation error. + """ + # Compute quaternion error (i.e., difference quaternion) + # Reference: https://personal.utdallas.edu/~sxb027100/dock/quaternion.html + # q_current_norm = q_current * q_current_conj + source_quat_norm = quat_mul(q01, quat_conjugate(q01))[:, 0] + # q_current_inv = q_current_conj / q_current_norm + source_quat_inv = quat_conjugate(q01) / source_quat_norm.unsqueeze(-1) + # q_error = q_target * q_current_inv + quat_error = quat_mul(q02, source_quat_inv) + + # Compute position error + pos_error = t02 - t01 + + # return error based on specified type + if rot_error_type == "quat": + return pos_error, quat_error + elif rot_error_type == "axis_angle": + # Convert to axis-angle error + axis_angle_error = axis_angle_from_quat(quat_error) + return pos_error, axis_angle_error + else: + raise ValueError(f"Unsupported orientation error type: {rot_error_type}. Valid: 'quat', 'axis_angle'.") + + +@torch.jit.script +def apply_delta_pose( + source_pos: torch.Tensor, source_rot, delta_pose: torch.Tensor, eps: float = 1.0e-6 +) -> Tuple[torch.Tensor, torch.Tensor]: + """Applies delta pose transformation on source pose. + + The first three elements of `delta_pose` are interpreted as cartesian position displacement. + The remaining three elements of `delta_pose` are interpreted as orientation displacement + in the angle-axis format. + + Args: + frame_pos (torch.Tensor): Position of source frame. Shape: [N, 3] + frame_rot (torch.Tensor): Quaternion orientation of source frame in (w, x, y,z). + delta_pose (torch.Tensor): Position and orientation displacements. Shape [N, 6]. + eps (float): The tolerance to consider orientation displacement as zero. + + Returns: + torch.Tensor: A tuple containing the displaced position and orientation frames. Shape: ([N, 3], [N, 4]) + """ + # number of poses given + num_poses = source_pos.shape[0] + device = source_pos.device + + # interpret delta_pose[:, 0:3] as target position displacements + target_pos = source_pos + delta_pose[:, 0:3] + # interpret delta_pose[:, 3:6] as target rotation displacements + rot_actions = delta_pose[:, 3:6] + angle = torch.linalg.vector_norm(rot_actions, dim=1) + axis = rot_actions / angle.unsqueeze(-1) + # change from axis-angle to quat convention + identity_quat = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).repeat(num_poses, 1) + rot_delta_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > eps, quat_from_angle_axis(angle, axis), identity_quat + ) + # TODO: Check if this is the correct order for this multiplication. + target_rot = quat_mul(rot_delta_quat, source_rot) + + return target_pos, target_rot + + +""" +Sampling +""" + + +@torch.jit.script +def default_orientation(num: int, device: str) -> torch.Tensor: + """Returns identity rotation transform. + + Args: + num (int): The number of rotations to sample. + device (str): Device to create tensor on. + + Returns: + torch.Tensor: Identity quaternion (w, x, y, z). + """ + quat = torch.zeros((num, 4), dtype=torch.float, device=device) + quat[..., 0] = 1.0 + + return quat + + +@torch.jit.script +def random_orientation(num: int, device: str) -> torch.Tensor: + """Returns sampled rotation in 3D as quaternion. + + Reference: + https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.random.html + + Args: + num (int): The number of rotations to sample. + device (str): Device to create tensor on. + + Returns: + torch.Tensor: Sampled quaternion (w, x, y, z). + """ + # sample random orientation from normal distribution + quat = torch.randn((num, 4), dtype=torch.float, device=device) + # normalize the quaternion + quat = torch.nn.functional.normalize(quat, p=2.0, dim=-1, eps=1e-12) + + return quat + + +@torch.jit.script +def random_yaw_orientation(num: int, device: str) -> torch.Tensor: + """Returns sampled rotation around z-axis. + + Args: + num (int): The number of rotations to sample. + device (str): Device to create tensor on. + + Returns: + torch.Tensor: Sampled quaternion (w, x, y, z). + """ + roll = torch.zeros(num, dtype=torch.float, device=device) + pitch = torch.zeros(num, dtype=torch.float, device=device) + yaw = 2 * np.pi * torch.rand(num, dtype=torch.float, device=device) + + return quat_from_euler_xyz(roll, pitch, yaw) + + +def sample_triangle(lower: float, upper: float, size: Union[int, Tuple[int, ...]], device: str) -> torch.Tensor: + """Randomly samples tensor from a triangular distribution. + + Args: + lower (float): The lower range of the sampled tensor. + upper (float): The upper range of the sampled tensor. + size (Union[int, Tuple[int, ...]]): The shape of the tensor. + device (str): Device to create tensor on. + + Returns: + torch.Tensor: Sampled tensor of shape :obj:`size`. + """ + # convert to tuple + if isinstance(size, int): + size = (size,) + # create random tensor in the range [-1, 1] + r = 2 * torch.rand(*size, device=device) - 1 + # convert to triangular distribution + r = torch.where(r < 0.0, -torch.sqrt(-r), torch.sqrt(r)) + # rescale back to [0, 1] + r = (r + 1.0) / 2.0 + # rescale to range [lower, upper] + return (upper - lower) * r + lower + + +def sample_uniform( + lower: Union[torch.Tensor, float], upper: Union[torch.Tensor, float], size: Union[int, Tuple[int, ...]], device: str +) -> torch.Tensor: + """Sample uniformly within a range. + + Args: + lower (Union[torch.Tensor, float]): Lower bound of uniform range. + upper (Union[torch.Tensor, float]): Upper bound of uniform range. + size (Union[int, Tuple[int, ...]]): The shape of the tensor. + device (str): Device to create tensor on. + + Returns: + torch.Tensor: Sampled tensor of shape :obj:`size`. + """ + # convert to tuple + if isinstance(size, int): + size = (size,) + # return tensor + return torch.rand(*size, device=device) * (upper - lower) + lower diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/__init__.py new file mode 100644 index 0000000000..f367be2ab2 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module introduces the base managers for defining MDPs.""" + +from .observation_manager import ObservationManager +from .reward_manager import RewardManager + +__all__ = ["RewardManager", "ObservationManager"] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/observation_manager.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/observation_manager.py new file mode 100644 index 0000000000..4a3c9752b1 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/observation_manager.py @@ -0,0 +1,384 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import copy +import functools +import inspect +import torch +from prettytable import PrettyTable +from typing import Any, Callable, Dict, Sequence, Tuple + + +class ObservationManager: + """Manager for computing observation signals for a given world. + + Observations are organized into groups based on their intended usage. This allows having different observation + groups for different types of learning such as asymmetric actor-critic and student-teacher training. Each + group contains a dictionary of observation terms which contain information about the observation function + to call, the noise corruption model to use, and the sensor to retrieve data from. + + The configuration for the observation manager is a dictionary of dictionaries. The top level dictionary + contains a key for each observation group. Each group dictionary contains configuration settings and + each observation term activated in the group. + + The following configuration settings are available for each group: + + - ``enable_corruption``: A boolean indicating whether to enable noise corruption for the group. If not + specified, False is assumed. + - ``return_dict_obs_in_group``: A boolean indicating whether to return the observations as a dictionary + with the keys being the observation term names, or as a single tensor with the observations concatenated + tensor along their last dimension. If not specified, False is assumed. + + + Each observation term dictionary contains the following keys: + + - ``func``: The name of the observation term function. This is the name of the function to call to compute + the observation. The function must be defined in the class that inherits from this class. If not specified, + the function name is assumed to be the same as the term name. + - ``scale``: The scale to apply to the observation before adding noise. If not specified, no scaling is + applied. + - ``clip``: The clipping range to apply to the observation after adding noise. This is a tuple of the + form (min, max). If not specified, no clipping is applied. + - ``noise``: The noise corruption model to use. If not specified, no corruption is done. This is a dictionary + with the following keys: + + - ``type``: The type of noise corruption model to use. This can be either: + + - ``deterministic``: Deterministic additive noise. + - ``gaussian``: Gaussian noise is added. + - ``uniform``: Uniform noise is added. + + - ``params``: The parameters for the noise corruption model. This is a dictionary with the keys + depending on the type of noise corruption model: + + - For deterministic noise: + + - ``value``: The value to add to the observation. + + - For gaussian noise: + + - ``mean``: The mean of the Gaussian distribution. + - ``std``: The standard deviation of the Gaussian distribution. + + - For uniform noise: + + - ``min``: The minimum value of the uniform distribution. + - ``max``: The maximum value of the uniform distribution. + + Usage: + .. code-block:: python + + import torch + from collections import namedtuple + + from omni.isaac.orbit.utils.mdp.observation_manager import ObservationManager + + + class DummyObservationManager(ObservationManager): + def obs_term_1(self, env): + return torch.ones(env.num_envs, 4, device=self.device) + + def obs_term_2(self, env, bbq: bool): + return bbq * torch.ones(env.num_envs, 1, device=self.device) + + def obs_term_3(self, env, hot: bool): + return hot * 2 * torch.ones(env.num_envs, 1, device=self.device) + + def obs_term_4(self, env, hot: bool, bland: float): + return hot * bland * torch.ones(env.num_envs, 5, device=self.device) + + + # dummy environment with 20 instances + env = namedtuple("IsaacEnv", ["num_envs"])(20) + # dummy device + device = "cpu" + + # create observations manager + cfg = { + "return_dict_obs_in_group": False, + "policy": { + "enable_corruption": True, + "obs_term_1": { + "scale": 10, + "noise": {"name": "uniform", "min": -0.1, "max": 0.1} + }, + }, + "critic": { + "enable_corruption": False, + "obs_term_1": {"scale": 10}, + "obs_term_2": {"scale": 5, "bbq": True}, + "obs_term_3": {"scale": 0.0, "hot": False} + }, + } + obs_man = DummyObservationManager(cfg, env, device) + + # print observation manager + # shows sequence of observation terms for each group and their configuration + print(obs_man) + + # here we reset all environment ids, but it is possible to reset only a subset + # of the environment ids based on the episode termination. + obs_man.reset_idx(env_ids=list(range(env.num_envs))) + + # check number of active terms + assert len(obs_man.active_terms["policy"]) == 1 + assert len(obs_man.active_terms["critic"]) == 3 + + # compute observation using manager + observations = obs_man.compute() + # check the observation shape + assert (env.num_envs, 4) == observations["policy"].shape + assert (env.num_envs, 6) == observations["critic"].shape + + """ + + def __init__(self, cfg: Dict[str, Dict[str, Any]], env, device: str): + """Initialize observation manager. + + Args: + cfg (Dict[str, Dict[str, Any]]): A dictionary of configuration settings for each group. + env (_type_): The object instance (typically the environment) from which data is read to + compute the observation terms. + device (str): The computation device for observations. + """ + # store input + self._cfg = copy.deepcopy(cfg) + self._env = env + self._device = device + # store observation manager settings + self._return_dict_obs_in_group = self._cfg.get("return_dict_obs_in_group", False) + # parse config to create observation terms information + self._prepare_observation_terms() + # compute combined vector for obs group + self._group_obs_dim: Dict[str, Tuple[int, ...]] = dict() + for group_name, group_term_dims in self._group_obs_term_dim.items(): + 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()) + + def __str__(self) -> str: + """Returns: A string representation for reward manager.""" + msg = f" contains {len(self._group_obs_term_names)} groups.\n" + + # add info for each group + for group_name, group_dim in self._group_obs_dim.items(): + # create table for term information + table = PrettyTable() + table.title = f"Active Observation Terms in Group: '{group_name}' (shape: {group_dim})" + table.field_names = ["Index", "Name", "Function", "Shape", "Corruptor", "Clipping", "Scaling", "Parameters"] + # set alignment of table columns + table.align["Name"] = "l" + # add info for each term + obs_terms = zip( + self._group_obs_term_names[group_name], + self._group_obs_term_functions[group_name], + self._group_obs_term_dim[group_name], + self._group_obs_term_corruptors[group_name], + self._group_obs_term_clip_ranges[group_name], + self._group_obs_term_scales[group_name], + self._group_obs_term_params[group_name], + ) + for index, (name, func, dims, corruptor, clip_range, scale, params) in enumerate(obs_terms): + # resolve inputs to simplify prints + tab_func = func.__name__ + tab_dims = tuple(dims) + tab_corruptor = corruptor.__str__() + tab_scale = 1.0 if scale is None else scale + tab_params = None if not any(params) else params + # add row + table.add_row([index, name, tab_func, tab_dims, tab_corruptor, clip_range, tab_scale, tab_params]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + """ + Properties. + """ + + @property + def device(self) -> str: + """Name of device for computation.""" + return self._device + + @property + def active_terms(self) -> Dict[str, Sequence[str]]: + """Name of active observation terms in each group.""" + return self._group_obs_term_names + + @property + def group_obs_dim(self) -> Dict[str, Tuple[int, ...]]: + """Shape of observation tensor in each group.""" + return self._group_obs_dim + + """ + Operations. + """ + + def reset_idx(self, env_ids: torch.Tensor): + """Reset the terms computation for input environment indices. + + Args: + env_ids (torch.Tensor): Indices of environment instances to reset. + """ + # Might need this when dealing with history and delays. + pass + + def compute(self) -> Dict[str, torch.Tensor]: + """Compute the observations per group. + + The method computes the observations for each group and returns a dictionary with keys as + the group names and values as the computed observations. The observations are computed + by calling the registered functions for each term in the group. The functions are called + in the order of the terms in the group. The functions are expected to return a tensor + with shape ``(num_envs, ...)``. The tensors are then concatenated along the last dimension to + form the observations for the group. + + If a corruption/noise model is registered for a term, the function is called to corrupt + the observation. The corruption function is expected to return a tensor with the same + shape as the observation. The observations are clipped and scaled as per the configuration + settings. By default, no scaling or clipping is applied. + + Returns: + Dict[str, torch.Tensor]: A dictionary with keys as the group names and values as the + computed observations. + """ + self._obs_buffer = dict() + # iterate over all the terms in each group + for group_name, group_term_names in self._group_obs_term_names.items(): + # buffer to store obs per group + group_obs = dict.fromkeys(group_term_names, None) + # read attributes for each term + obs_terms = zip( + group_term_names, + self._group_obs_term_corruptors[group_name], + self._group_obs_term_clip_ranges[group_name], + self._group_obs_term_scales[group_name], + self._group_obs_term_params[group_name], + self._group_obs_term_functions[group_name], + ) + # evaluate terms: compute, add noise, clip, scale. + for name, corruptor, clip_range, scale, params, func in obs_terms: + # compute term's value + obs: torch.Tensor = func(self._env, **params) + # apply post-processing + if corruptor: + obs = corruptor(obs) + if clip_range: + obs = obs.clip_(min=clip_range[0], max=clip_range[1]) + if scale: + obs = obs.mul_(scale) + # TODO: Introduce delay and filtering models. + # Ref: https://robosuite.ai/docs/modules/sensors.html#observables + # add value to list + group_obs[name] = obs + # concatenate all observations in the group together + if self._return_dict_obs_in_group: + self._obs_buffer[group_name] = group_obs + else: + self._obs_buffer[group_name] = torch.cat(list(group_obs.values()), dim=-1) + # return all group observations + return self._obs_buffer + + """ + Noise models. + """ + + @staticmethod + def _add_deterministic_noise(obs: torch.Tensor, value: float): + """Add a constant noise.""" + return obs + value + + @staticmethod + def _add_uniform_noise(obs: torch.Tensor, min: float, max: float): + """Adds a noise sampled from a uniform distribution (-min_noise, max_noise).""" + return obs + torch.rand_like(obs) * (max - min) + min + + @staticmethod + def _add_gaussian_noise(obs: torch.Tensor, mean: float, std: float): + return obs + mean + std * torch.randn_like(obs) + + """ + Helper functions. + """ + + def _prepare_observation_terms(self): + """Prepares a list of observation terms functions. + + Raises: + AttributeError: If the observation term's function not found in the class. + """ + # create buffers to store information for each observation group + # TODO: Make this more convenient by using data structures. + self._group_obs_term_names: Dict[str, Sequence[str]] = dict() + self._group_obs_term_dim: Dict[str, Sequence[int]] = dict() + self._group_obs_term_params: Dict[str, Sequence[Dict[str, float]]] = dict() + self._group_obs_term_clip_ranges: Dict[str, Sequence[Tuple[float, float]]] = dict() + self._group_obs_term_scales: Dict[str, Sequence[float]] = dict() + self._group_obs_term_corruptors: Dict[str, Sequence[Callable]] = dict() + self._group_obs_term_functions: Dict[str, Sequence[Callable]] = dict() + + for group_name, group_cfg in self._cfg.items(): + # skip non-group settings (those should be read above) + if not isinstance(group_cfg, dict): + continue + # initialize list for the group settings + self._group_obs_term_names[group_name] = list() + self._group_obs_term_dim[group_name] = list() + self._group_obs_term_params[group_name] = list() + self._group_obs_term_clip_ranges[group_name] = list() + self._group_obs_term_scales[group_name] = list() + self._group_obs_term_corruptors[group_name] = list() + self._group_obs_term_functions[group_name] = list() + # read common config for the group + enable_corruption = group_cfg.pop("enable_corruption", False) + # parse group observation settings + for term_name, term_cfg in group_cfg.items(): + # skip non-obs settings (those should be read above) + if not isinstance(term_cfg, dict): + continue + # read term's function + # if not specified, assume the function name is the same as the term name + term_func = term_cfg.get("func", term_name) + # check if obs manager has the term + if hasattr(self, term_func): + # check noise settings + noise_cfg = term_cfg.pop("noise", None) + if enable_corruption and noise_cfg: + # read noise parameters + noise_name = noise_cfg.pop("name") + noise_params = noise_cfg + # create a wrapper s.t. Callable[[torch.Tensor], torch.Tensor] + noise_func = getattr(self, f"_add_{noise_name}_noise") + noise_func = functools.partial(noise_func, **noise_params) + # make string representation nicer + noise_func.__str__ = lambda: f"Noise: {noise_name}, Params: {noise_params}" + # add function to list + self._group_obs_term_corruptors[group_name].append(noise_func) + else: + self._group_obs_term_corruptors[group_name].append(None) + # check clip_range and scale settings + self._group_obs_term_clip_ranges[group_name].append(term_cfg.pop("clip", None)) + self._group_obs_term_scales[group_name].append(term_cfg.pop("scale", None)) + # check function for observation term + func = getattr(self, term_name) + # check if term's arguments are matched by params + term_params = list(term_cfg.keys()) + args = inspect.getfullargspec(func).args + # ignore first two arguments for (self, env) + # Think: Check for cases when kwargs are set inside the function? + if len(args) > 2: + if set(args[2:]) != set(term_params): + msg = f"Observation term '{term_name}' expects parameters: {args[2:]}, but {term_params} provided." + raise ValueError(msg) + # add function to list + self._group_obs_term_names[group_name].append(term_name) + self._group_obs_term_functions[group_name].append(func) + self._group_obs_term_params[group_name].append(term_cfg) + # call function the first time to fill up dimensions + obs_dims = tuple(func(self._env, **term_cfg).shape[1:]) + self._group_obs_term_dim[group_name].append(obs_dims) + else: + raise AttributeError(f"Observation term with the name '{term_name}' has no implementation.") diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/reward_manager.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/reward_manager.py new file mode 100644 index 0000000000..e00197bf42 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/mdp/reward_manager.py @@ -0,0 +1,286 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import copy +import inspect +import torch +from prettytable import PrettyTable +from typing import Any, Dict, List, Optional + + +class RewardManager: + """Manager for computing reward signals for a given world. + + The reward manager computes the total reward as a sum of the weighted reward terms. The reward + terms are parsed from a nested dictionary containing the reward manger's settings and reward + terms configuration. The manager looks for member functions with the name of the reward term's + key in the settings. + + The following configuration settings are available: + + * ``only_positive_rewards``: A boolean indicating whether to return only positive rewards (if the + accumulated reward is negative, then zero is returned). This is useful to facilitate learning + towards solving the task first while pushing the negative penalties for later :cite:p:`rudin2022advanced`. + + Each reward term configuration must contain the key ``weight`` which decides the linear weighting + of the reward term. Terms with weights as zero are ignored by the rewards manager. Additionally, + the configuration dictionary can contain other parameters for the reward term's function, such + as kernel parameters. + + .. note:: + The reward manager multiplies the reward terms by the ``weight`` key in the configuration + dictionary with the time-step interval ``dt`` of the environment. This is done to ensure + that the computed reward terms are balanced with respect to the chosen time-step interval. + + + Usage: + .. code-block:: python + + from collections import namedtuple + + from omni.isaac.orbit.utils.mdp.reward_manager import RewardManager + + + class DummyRewardManager(RewardManager): + def reward_term_1(self, env): + return 1 + + def reward_term_2(self, env, bbq: bool): + return 1.0 * bbq + + def reward_term_3(self, env, hot: bool): + return 4.0 * hot + + def reward_term_4(self, env, hot: bool, bland: float): + return bland * hot * 0.5 + + + # dummy environment with 20 instances + env = namedtuple("IsaacEnv", ["num_envs", "dt"])(20, 0.01) + # dummy device + device = "cpu" + + # create reward manager + cfg = { + "only_positive_rewards": False, + "reward_term_1": {"weight": 10.0}, + "reward_term_2": {"weight": 5.0, "bbq": True}, + "reward_term_3": {"weight": 0.0, "hot": True}, + "reward_term_4": {"weight": 1.0, "hot": False, "bland": 2.0}, + } + rew_man = DummyRewardManager(cfg, env, env.num_envs, env.dt, device) + + # print reward manager + # shows active reward terms and their weights + print(rew_man) + + # check number of active reward terms + assert len(rew_man.active_terms) == 3 + + # here we reset all environment ids, but it is possible to reset only a subset + # of the environment ids based on the episode termination. + rew_man.reset_idx(env_ids=list(range(env.num_envs))) + + # compute reward using manager + rewards = rew_man.compute() + # check the rewards shape + assert rewards.shape == (env.num_envs,) + + """ + + def __init__(self, cfg: Dict[str, Dict[str, Any]], env, num_envs: int, dt: float, device: str): + """Construct a list of reward functions which are used to compute the total reward. + + Args: + cfg (Dict[str, Dict[str, Any]]): Configuration for reward terms. + env (IsaacEnv): A world instance used for accessing state. + num_envs (int): Number of environment instances. + dt (float): The time-stepping for the environment. + device (int): The device on which create buffers. + """ + # store input + self._cfg = copy.deepcopy(cfg) + self._env = env + self._num_envs = num_envs # We can get this from env? + self._dt = dt # We can get this from env? + self._device = device + # store reward manager settings + self._enable_only_positive_rewards = self._cfg.pop("only_positive_rewards", False) + # parse config to create reward terms information + self._prepare_reward_terms() + # prepare extra info to store individual reward term information + self._episode_sums = dict() + for term_name in self._reward_term_names: + self._episode_sums[term_name] = torch.zeros(self._num_envs, dtype=torch.float, device=self._device) + # create buffer for managing reward per environment + self._reward_buf = torch.zeros(self._num_envs, dtype=torch.float, device=self._device) + + def __str__(self) -> str: + """Returns: A string representation for reward manager.""" + msg = f" contains {len(self._reward_term_names)} active terms.\n" + + # create table for term information + table = PrettyTable() + table.title = "Active Reward Terms" + table.field_names = ["Index", "Name", "Weight", "Parameters"] + # set alignment of table columns + table.align["Name"] = "l" + table.align["Weight"] = "r" + # add info on each term + reward_terms = zip(self._reward_term_names, self._reward_term_weights, self._reward_term_params) + for index, (name, weight, params) in enumerate(reward_terms): + if any(params): + table.add_row([index, name, weight, params]) + else: + table.add_row([index, name, weight, None]) + # convert table to string + msg += table.get_string() + + return msg + + """ + Properties. + """ + + @property + def device(self) -> str: + """Name of device for computation.""" + return self._device + + @property + def active_terms(self) -> List[str]: + """Name of active reward terms.""" + return self._reward_term_names + + @property + def episode_sums(self) -> Dict[str, torch.Tensor]: + """Contains the current episodic sum of individual reward terms.""" + return self._episode_sums + + """ + Operations. + """ + + def reset_idx(self, env_ids: torch.Tensor, episodic_extras: Optional[dict] = None): + """Reset the reward terms computation for input environment indices. + + If `episodic_extras` is not None, then the collected sum of individual reward terms is stored + in the dictionary. This is useful for logging episodic information. + + Args: + env_ids (torch.Tensor): Indices of environment instances to reset. + episodic_extras (Optional[dict], optional): Dictionary to store episodic information. + Defaults to None. + """ + if episodic_extras is None: + # reset the collected sum + for term_name in self._episode_sums: + self._episode_sums[term_name][env_ids] = 0.0 + else: + # episode length (in seconds from env) + episode_length_s = self._env.cfg.env.episode_length_s + # save collected sum and reset the rolling sums + for term_name in self._episode_sums: + # -- save the collected sum + cumulated_score = self._episode_sums[term_name][env_ids] + episodic_extras[f"rew_{term_name}"] = torch.mean(cumulated_score / episode_length_s) + # -- reset the collected sum + self._episode_sums[term_name][env_ids] = 0.0 + + def compute(self) -> torch.Tensor: + """Computes the reward signal as linearly weighted sum of individual terms. + + This function calls each reward term managed by the class and adds them to compute the net + reward signal. It also updates the episodic sums corresponding to individual reward terms. + + Returns: + torch.Tensor: The net reward signal of shape (num_envs,). + """ + # reset computation + self._reward_buf[:] = 0.0 + # iterate over all the reward terms + for name, weight, params, func in zip( + self._reward_term_names, self._reward_term_weights, self._reward_term_params, self._reward_term_functions + ): + # termination rewards: handled after clipping + if name == "termination": + continue + # compute term's value + value = func(self._env, **params) * weight + # update total reward + self._reward_buf += value + # update episodic sum + self._episode_sums[name] += value + # if enabled, consider rewards only when they yield a positive sum + # TODO: (trick from Nikita) Add more documentation on why this might be helpful! + if self._enable_only_positive_rewards: + self._reward_buf = self._reward_buf.clip_(min=0.0) + # add termination reward after clipping has been performed + if "termination" in self._reward_term_names: + weight = self._reward_term_weights["termination"] + params = self._reward_term_params["termination"] + func = self._reward_term_functions["termination"] + # compute term's value + value = func(self._env, **params) * weight + # update total reward + self._reward_buf += value + # update episodic sum + self._episode_sums["termination"] += value + + return self._reward_buf + + """ + Helper functions. + """ + + def _prepare_reward_terms(self): + """Prepares a list of reward functions. + + Raises: + KeyError: If reward term configuration does not have they key 'weight'. + ValueError: If reward term configuration does not satisfy its function signature. + AttributeError: If the reward term's function not found in the class. + """ + # remove zero scales and multiply non-zero ones by dt + # note: we multiply weights by dt to make them agnostic to control decimation + for term_name in list(self._cfg): + # extract term config + term_cfg = self._cfg[term_name] + # check for weight attribute + if "weight" in term_cfg: + if term_cfg["weight"] == 0: + self._cfg.pop(term_name) + else: + term_cfg["weight"] *= self._dt + else: + raise KeyError(f"The key 'weight' not found for reward term: '{term_name}'.") + + # parse remaining reward terms and decimate their information + # TODO: Make this more convenient by using data structures. + self._reward_term_names = list() + self._reward_term_weights = list() + self._reward_term_params = list() + self._reward_term_functions = list() + + for term_name, term_cfg in self._cfg.items(): + self._reward_term_names.append(term_name) + self._reward_term_weights.append(term_cfg.pop("weight")) + self._reward_term_params.append(term_cfg) + # check if rewards manager has the term + if hasattr(self, term_name): + func = getattr(self, term_name) + # check if reward term's arguments are matched by params + term_params = list(term_cfg.keys()) + args = inspect.getfullargspec(func).args + # ignore first two arguments for (self, env) + # Think: Check for cases when kwargs are set inside the function? + if len(args) > 2: + if set(args[2:]) != set(term_params): + msg = f"Reward term '{term_name}' expects parameters: {args[2:]}, but {term_params} provided." + raise ValueError(msg) + # add function to list + self._reward_term_functions.append(func) + else: + raise AttributeError(f"Reward term with the name '{term_name}' has no implementation.") diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/string.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/string.py new file mode 100644 index 0000000000..dd8b2d9aa2 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/string.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Transformations of strings.""" + + +import re +from typing import Optional + + +def to_camel_case(snake_str: str, to: Optional[str] = "cC") -> str: + """Converts a string from snake case to camel case. + + Args: + snake_str (str): A string in snake case (i.e. with '_') + to (Optional[str], optional): Convention to convert string to. Defaults to "cC". + + Raises: + ValueError: Invalid input argument `to`, i.e. not "cC" or "CC". + + Returns: + str: A string in camel-case format. + """ + # check input is correct + if to not in ["cC", "CC"]: + msg = "to_camel_case(): Choose a valid `to` argument (CC or cC)" + raise ValueError(msg) + # convert string to lower case and split + components = snake_str.lower().split("_") + if to == "cC": + # We capitalize the first letter of each component except the first one + # with the 'title' method and join them together. + return components[0] + "".join(x.title() for x in components[1:]) + else: + # Capitalize first letter in all the components + return "".join(x.title() for x in components) + + +def to_snake_case(camel_str: str) -> str: + """Converts a string from camel case to snake case. + + Args: + camel_str (str): A string in camel case. + + Returns: + str: A string in snake case (i.e. with '_') + """ + camel_str = re.sub("(.)([A-Z][a-z]+)", r"\1_\2", camel_str) + return re.sub("([a-z0-9])([A-Z])", r"\1_\2", camel_str).lower() diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/timer.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/timer.py new file mode 100644 index 0000000000..6b800da663 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/timer.py @@ -0,0 +1,126 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Defines timer class for performance measurements.""" + + +import time +from contextlib import ContextDecorator +from typing import Any, Optional + + +class TimerError(Exception): + """A custom exception used to report errors in use of Timer class""" + + pass + + +class Timer(ContextDecorator): + """A timer for performance measurements. + + A class to keep track of time for performance measurement. + It allows timing via context managers and decorators as well. + + As a regular object: + + .. code-block:: python + + import time + + from omni.isaac.orbit.utils.timer import Timer + + timer = Timer() + timer.start() + time.sleep(1) + print(1 <= timer.time_elapsed <= 2) # Output: True + + time.sleep(1) + timer.stop() + print(2 <= stopwatch.total_run_time) # Output: True + + As a context manager: + + .. code-block:: python + + import time + + from omni.isaac.orbit.utils.timer import Timer + + with Timer() as timer: + time.sleep(1) + print(1 <= timer.time_elapsed <= 2) # Output: True + + Reference: https://gist.github.com/sumeet/1123871 + """ + + def __init__(self, msg: Optional[str] = None): + """ + Initializes the class variables + """ + self._msg = msg + self._start_time = None + self._stop_time = None + self._elapsed_time = None + + def __str__(self) -> str: + """ + Returns: + str -- String representation of the class object. + """ + return f"{self.time_elapsed:0.6f} seconds" + + """ + Properties + """ + + @property + def time_elapsed(self) -> float: + """The number of seconds that have elapsed since this timer started timing. + + Note: + This is used for checking how much time has elapsed while the timer is still running. + """ + return time.perf_counter() - self._start_time + + @property + def total_run_time(self) -> float: + """The number of seconds that elapsed from when the timer started to when it ended.""" + return self._elapsed_time + + """ + Operations + """ + + def start(self): + """Start timing.""" + if self._start_time is not None: + raise TimerError("Timer is running. Use .stop() to stop it") + + self._start_time = time.perf_counter() + + def stop(self): + """Stop timing""" + if self._start_time is None: + raise TimerError("Timer is not running. Use .start() to start it") + + self._stop_time = time.perf_counter() + self._elapsed_time = self._stop_time - self._start_time + self._start_time = None + + """ + Context managers + """ + + def __enter__(self) -> "Timer": + """Start timing and return this `Timer` instance.""" + self.start() + return self + + def __exit__(self, *exc_info: Any): + """Stop timing.""" + self.stop() + # print message + if self._msg is not None: + print(self._msg, f": {self._elapsed_time:0.6f} seconds") diff --git a/source/extensions/omni.isaac.orbit/setup.py b/source/extensions/omni.isaac.orbit/setup.py new file mode 100644 index 0000000000..27e236a817 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/setup.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Installation script for the 'omni.isaac.orbit' python package.""" + + +from setuptools import setup + +# Minimum dependencies required prior to installation +INSTALL_REQUIRES = [ + # generic + "numpy", + "torch", + "prettytable==3.3.0", + # devices + "hidapi", +] + +# Installation operation +setup( + name="omni-isaac-orbit", + author="NVIDIA, ETH Zurich, and University of Toronto", + maintainer="Mayank Mittal", + maintainer_email="mittalma@ethz.ch", + url="https://github.com/NVIDIA-Omniverse/orbit", + license="BSD-3-Clause", + version="0.1.0", + description="Python module for the core framework interfaces of ORBIT.", + keywords=["robotics", "simulation", "sensors"], + include_package_data=True, + python_requires=">=3.7.*", + install_requires=INSTALL_REQUIRES, + packages=["omni.isaac.orbit"], + classifiers=["Natural Language :: English", "Programming Language :: Python :: 3.7"], + zip_safe=False, +) diff --git a/source/extensions/omni.isaac.orbit/test/test_camera.py b/source/extensions/omni.isaac.orbit/test/test_camera.py new file mode 100644 index 0000000000..a14edf02c7 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_camera.py @@ -0,0 +1,443 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + + +import logging + +from omni.isaac.kit import SimulationApp + +# launch the simulator +config = {"headless": True} +simulation_app = SimulationApp(config) + +# disable matplotlib debug messages +mpl_logger = logging.getLogger("matplotlib") +mpl_logger.setLevel(logging.WARNING) + +"""Rest everything follows.""" + + +import numpy as np +import os +import random +import scipy.spatial.transform as tf +import unittest + +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +import omni.replicator.core as rep +from omni.isaac.core.prims import RigidPrim +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view +from pxr import Gf, UsdGeom + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.sensors.camera import Camera, PinholeCameraCfg +from omni.isaac.orbit.utils.math import convert_quat +from omni.isaac.orbit.utils.timer import Timer + + +class TestCameraSensor(unittest.TestCase): + """Test fixture for checking camera interface.""" + + @classmethod + def tearDownClass(cls): + """Closes simulator after running all test fixtures.""" + simulation_app.close() + + def setUp(self) -> None: + """Create a blank new stage for each test.""" + # Simulation time-step + self.dt = 0.1 + # Load kit helper + self.sim = SimulationContext( + stage_units_in_meters=1.0, physics_dt=self.dt, rendering_dt=self.dt, backend="numpy" + ) + # Set camera view + set_camera_view(eye=[15.0, 15.0, 15.0], target=[0.0, 0.0, 0.0]) + # Spawn things into stage + self._populate_scene() + # Wait for spawning + stage_utils.update_stage() + + def tearDown(self) -> None: + """Stops simulator after each test.""" + # close all the opened viewport from before. + rep.vp_manager.destroy_hydra_textures() + # stop simulation + self.sim.stop() + self.sim.clear() + + def test_camera_resolution(self): + """Checks that a camera provides image at the resolution specified.""" + # Create camera instance + camera_cfg = PinholeCameraCfg( + sensor_tick=0, + height=480, + width=640, + data_types=["rgb", "distance_to_image_plane"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera = Camera(cfg=camera_cfg, device="cpu") + camera.spawn("/World/CameraSensor") + + # Play simulator + self.sim.reset() + # Initialize sensor + camera.initialize() + # perform rendering + self.sim.step() + # update camera + camera.update(self.dt) + + # expected camera image shape + height_expected, width_expected = camera.image_shape + # check that the camera image shape is correct + for im_data in camera.data.output.values(): + if not isinstance(im_data, np.ndarray): + continue + height, width = im_data.shape[:2] + self.assertEqual(height, height_expected) + self.assertEqual(width, width_expected) + + def test_default_camera(self): + """Checks that the pre-existing stage camera is configured correctly.""" + # Create directory to dump results + test_dir = os.path.dirname(os.path.abspath(__file__)) + output_dir = os.path.join(test_dir, "output", "camera", "kit_persp") + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + # Create replicator writer + rep_writer = rep.BasicWriter(output_dir=output_dir, frame_padding=3) + # Create camera instance + camera_cfg = PinholeCameraCfg( + sensor_tick=0, + height=180, + width=320, + data_types=["rgb", "distance_to_image_plane", "normals", "distance_to_camera"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera = Camera(cfg=camera_cfg, device="cpu") + # Note: the camera is spawned by default in the stage + # camera.spawn("/World/CameraSensor") + + # Play simulator + self.sim.reset() + # Initialize sensor + camera.initialize("/OmniverseKit_Persp") + + # Simulate physics + for i in range(10): + # perform rendering + self.sim.step() + # update camera + camera.update(self.dt) + # Save images + rep_writer.write(camera.data.output) + # Check image data + # expected camera image shape + height_expected, width_expected = camera.image_shape + # check that the camera image shape is correct + for im_data in camera.data.output.values(): + if not isinstance(im_data, np.ndarray): + continue + height, width = im_data.shape[:2] + self.assertEqual(height, height_expected) + self.assertEqual(width, width_expected) + + def test_single_cam(self): + """Checks that the single camera gets created properly with a rig.""" + # Create directory to dump results + test_dir = os.path.dirname(os.path.abspath(__file__)) + output_dir = os.path.join(test_dir, "output", "camera", "single") + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + # Create replicator writer + rep_writer = rep.BasicWriter(output_dir=output_dir, frame_padding=3) + # Create camera instance + camera_cfg = PinholeCameraCfg( + sensor_tick=0, + height=180, + width=320, + data_types=[ + "rgb", + "distance_to_image_plane", + "normals", + "distance_to_camera", + # "instance_segmentation", + # "semantic_segmentation", + "bounding_box_2d_tight", + "bounding_box_2d_loose", + "bounding_box_2d_tight", + "bounding_box_3d", + ], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera = Camera(cfg=camera_cfg, device="cpu") + # Note: the camera is spawned by default in the stage + camera.spawn("/World/CameraSensor") + + # Play simulator + self.sim.reset() + # Initialize sensor + camera.initialize() + # Set camera position directly + # Note: Not a recommended way but was feeling lazy to do it properly. + camera.set_world_pose_from_view(eye=[15.0, 15.0, 15.0], target=[0.0, 0.0, 0.0]) + + # Simulate physics + for i in range(4): + # perform rendering + self.sim.step() + # update camera + camera.update(self.dt) + # Save images + rep_writer.write(camera.data.output) + # Check image data + # expected camera image shape + height_expected, width_expected = camera.image_shape + # check that the camera image shape is correct + for im_data in camera.data.output.values(): + if not isinstance(im_data, np.ndarray): + continue + height, width = im_data.shape[:2] + self.assertEqual(height, height_expected) + self.assertEqual(width, width_expected) + + def test_multiple_cam(self): + """Checks that the multiple cameras created properly with and without rig.""" + # Create camera instance + # -- default viewport + camera_def_cfg = PinholeCameraCfg( + sensor_tick=0, + height=180, + width=320, + data_types=["rgb"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera_def = Camera(cfg=camera_def_cfg, device="cpu") + # -- camera 1 + camera1_cfg = PinholeCameraCfg( + sensor_tick=0, + height=180, + width=320, + data_types=["rgb", "distance_to_image_plane", "normals"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera_1 = Camera(cfg=camera1_cfg, device="cpu") + # -- camera 2 + camera2_cfg = PinholeCameraCfg( + sensor_tick=0, + height=256, + width=256, + data_types=["rgb", "distance_to_image_plane", "normals", "distance_to_camera"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera_2 = Camera(cfg=camera2_cfg, device="cpu") + # Note: the camera is spawned by default in the stage + camera_1.spawn("/World/CameraSensor1") + camera_2.spawn("/World/CameraSensor2") + + # Play simulator + self.sim.reset() + # Initialize sensor + camera_def.initialize("/OmniverseKit_Persp") + camera_1.initialize() + camera_2.initialize() + + # Simulate physics + for i in range(10): + # perform rendering + self.sim.step() + # update camera + camera_def.update(self.dt) + camera_1.update(self.dt) + camera_2.update(self.dt) + # Check image data + for cam in [camera_def, camera_1, camera_2]: + # expected camera image shape + height_expected, width_expected = cam.image_shape + # check that the camera image shape is correct + for im_data in cam.data.output.values(): + if not isinstance(im_data, np.ndarray): + continue + height, width = im_data.shape[:2] + self.assertEqual(height, height_expected) + self.assertEqual(width, width_expected) + + def test_intrinsic_matrix(self): + """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" + # Create camera instance + camera_cfg = PinholeCameraCfg( + sensor_tick=0, + height=240, + width=320, + data_types=["rgb", "distance_to_image_plane"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera = Camera(cfg=camera_cfg, device="cpu") + # Note: the camera is spawned by default in the stage + camera.spawn("/World/CameraSensor") + + # 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 = np.array(rs_intrinsic_matrix).reshape(3, 3) + # Set matrix into simulator + camera.set_intrinsic_matrix(rs_intrinsic_matrix) + + # Play simulator + self.sim.reset() + # Initialize sensor + camera.initialize() + # Simulate physics + for _ in range(10): + # perform rendering + self.sim.step() + # update camera + camera.update(self.dt) + # Check that matrix is correct + K = camera.data.intrinsic_matrix + # TODO: This is not correctly setting all values in the matrix. + self.assertAlmostEqual(rs_intrinsic_matrix[0, 0], K[0, 0], 4) + # self.assertAlmostEqual(rs_intrinsic_matrix[1, 1], K[1, 1], 4) + # Display results + print(f">>> Desired intrinsic matrix: \n{rs_intrinsic_matrix}") + print(f">>> Current intrinsic matrix: \n{camera.data.intrinsic_matrix}") + + def test_pose_ros(self): + """Checks that the camera's set and retrieve methods work for pose in ROS convention.""" + # Create camera instance + camera_cfg = PinholeCameraCfg( + sensor_tick=0, + height=240, + width=320, + data_types=["rgb", "distance_to_image_plane"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera = Camera(cfg=camera_cfg, device="cpu") + # Note: the camera is spawned by default in the stage + camera.spawn("/World/CameraSensor") + + # Play simulator + self.sim.reset() + # Initialize sensor + camera.initialize() + + # Simulate physics + for _ in range(10): + # set camera pose randomly + camera_position = np.random.random() * 5.0 + camera_orientation = convert_quat(tf.Rotation.random().as_quat(), "wxyz") + camera.set_world_pose_ros(pos=camera_position, quat=camera_orientation) + # perform rendering + self.sim.step() + # update camera + camera.update(self.dt) + # Check that pose is correct + np.testing.assert_almost_equal(camera.data.position, camera_position, 4) + np.testing.assert_almost_equal(camera.data.orientation, camera_orientation, 4) + + def test_throughput(self): + """Checks that the single camera gets created properly with a rig.""" + # Create directory to dump results + test_dir = os.path.dirname(os.path.abspath(__file__)) + output_dir = os.path.join(test_dir, "output", "camera", "throughput") + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + # Create replicator writer + rep_writer = rep.BasicWriter(output_dir=output_dir, frame_padding=3) + # Create camera instance + camera_cfg = PinholeCameraCfg( + sensor_tick=0, + height=480, + width=640, + data_types=["rgb", "distance_to_image_plane"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera = Camera(cfg=camera_cfg, device="cpu") + # Note: the camera is spawned by default in the stage + camera.spawn("/World/CameraSensor") + + # Play simulator + self.sim.reset() + # Initialize sensor + camera.initialize() + + # 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} "): + rep_writer.write(camera.data.output) + print("----------------------------------------") + # Check image data + # expected camera image shape + height_expected, width_expected = camera.image_shape + # check that the camera image shape is correct + for im_data in camera.data.output.values(): + if not isinstance(im_data, np.ndarray): + continue + height, width = im_data.shape[:2] + self.assertEqual(height, height_expected) + self.assertEqual(width, width_expected) + + """ + Helper functions. + """ + + @staticmethod + def _populate_scene(): + """Add prims to the scene.""" + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane") + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 1.0, "intensity": 300.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 1.0, "intensity": 300.0, "color": (1.0, 1.0, 1.0)}, + ) + # Random objects + for i in range(8): + # sample random position + position = np.random.rand(3) - np.asarray([0.5, 0.5, -1.0]) + position *= np.asarray([15.0, 15.0, 5.0]) + # create prim + prim_type = random.choice(["Cube", "Sphere", "Cylinder"]) + _ = prim_utils.create_prim( + f"/World/Objects/Obj_{i:02d}", + prim_type, + translation=position, + scale=(2.5, 2.5, 2.5), + semantic_label=prim_type, + ) + # add rigid properties + rigid_obj = RigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) + # cast to geom prim + geom_prim = getattr(UsdGeom, prim_type)(rigid_obj.prim) + # set random color + color = Gf.Vec3f(random.random(), random.random(), random.random()) + geom_prim.CreateDisplayColorAttr() + geom_prim.GetDisplayColorAttr().Set([color]) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit/test/test_configclass.py b/source/extensions/omni.isaac.orbit/test/test_configclass.py new file mode 100644 index 0000000000..dd90a23ee6 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_configclass.py @@ -0,0 +1,304 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import copy +import unittest +from dataclasses import asdict, field +from functools import wraps + +from omni.isaac.orbit.utils.configclass import configclass +from omni.isaac.orbit.utils.dict import class_to_dict, update_class_from_dict + +""" +Dummy configuration: Basic +""" + + +def double(x): + """Dummy function.""" + return 2 * x + + +@configclass +class ViewerCfg: + eye: list = [7.5, 7.5, 7.5] # field missing on purpose + lookat: list = field(default_factory=[0.0, 0.0, 0.0]) + + +@configclass +class EnvCfg: + num_envs: int = double(28) # uses function for assignment + episode_length: int = 2000 + viewer: ViewerCfg = ViewerCfg() + + +@configclass +class RobotDefaultStateCfg: + pos = (0.0, 0.0, 0.0) # type annotation missing on purpose (immutable) + rot: tuple = (1.0, 0.0, 0.0, 0.0) + dof_pos: tuple = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + dof_vel = [0.0, 0.0, 0.0, 0.0, 0.0, 1.0] # type annotation missing on purpose (mutable) + + +@configclass +class BasicDemoCfg: + """Dummy configuration class.""" + + device_id: int = 0 + env: EnvCfg = EnvCfg() + robot_default_state: RobotDefaultStateCfg = RobotDefaultStateCfg() + + +""" +Dummy configuration: Functions +""" + + +def dummy_function1() -> int: + """Dummy function 1.""" + return 1 + + +def dummy_function2() -> int: + """Dummy function 2.""" + return 2 + + +def dummy_wrapper(func): + """Decorator for wrapping function.""" + + @wraps(func) + def wrapper(): + return func() + 1 + + return wrapper + + +@dummy_wrapper +def wrapped_dummy_function3(): + """Dummy function 3.""" + return 3 + + +@dummy_wrapper +def wrapped_dummy_function4(): + """Dummy function 4.""" + return 4 + + +@configclass +class FunctionsDemoCfg: + """Dummy configuration class with functions as attributes.""" + + func = dummy_function1 + wrapped_func = wrapped_dummy_function3 + func_in_dict = {"func": dummy_function1} + + +""" +Test solutions: Basic +""" + +basic_demo_cfg_correct = { + "env": {"num_envs": 56, "episode_length": 2000, "viewer": {"eye": [7.5, 7.5, 7.5], "lookat": [0.0, 0.0, 0.0]}}, + "robot_default_state": { + "pos": (0.0, 0.0, 0.0), + "rot": (1.0, 0.0, 0.0, 0.0), + "dof_pos": (0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + "dof_vel": [0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + }, + "device_id": 0, +} + +basic_demo_cfg_change_correct = { + "env": {"num_envs": 22, "episode_length": 2000, "viewer": {"eye": (2.0, 2.0, 2.0), "lookat": [0.0, 0.0, 0.0]}}, + "robot_default_state": { + "pos": (0.0, 0.0, 0.0), + "rot": (1.0, 0.0, 0.0, 0.0), + "dof_pos": (0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + "dof_vel": [0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + }, + "device_id": 0, +} + +""" +Test solutions: Functions +""" + +functions_demo_cfg_correct = { + "func": "__main__:dummy_function1", + "wrapped_func": "__main__:wrapped_dummy_function3", + "func_in_dict": {"func": "__main__:dummy_function1"}, +} + +functions_demo_cfg_for_updating = { + "func": "__main__:dummy_function2", + "wrapped_func": "__main__:wrapped_dummy_function4", + "func_in_dict": {"func": "__main__:dummy_function2"}, +} + +""" +Test fixtures. +""" + + +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()) + + 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"]) + + def test_dict_conversion_order(self): + """Tests that order is conserved when converting to dictionary.""" + true_outer_order = ["device_id", "env", "robot_default_state"] + 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("robot_default_state") + # 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_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) + print("Updated config: ", cfg.to_dict()) + self.assertDictEqual(cfg.to_dict(), basic_demo_cfg_change_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 of the same configclass.""" + # 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)) + + 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_functions_config(self): + """Tests having functions as values in the configuration instance.""" + cfg = FunctionsDemoCfg() + # check calling + self.assertEqual(cfg.func(), 1) + self.assertEqual(cfg.wrapped_func(), 4) + self.assertEqual(cfg.func_in_dict["func"](), 1) + # print dictionary + print(class_to_dict(cfg)) + + 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) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit/test/test_height_scanner.py b/source/extensions/omni.isaac.orbit/test/test_height_scanner.py new file mode 100644 index 0000000000..558fa63c6a --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_height_scanner.py @@ -0,0 +1,348 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + + +import logging + +from omni.isaac.kit import SimulationApp + +# launch the simulator +config = {"headless": True} +simulation_app = SimulationApp(config) + +# disable matplotlib debug messages +mpl_logger = logging.getLogger("matplotlib") +mpl_logger.setLevel(logging.WARNING) + +"""Rest everything follows.""" + + +import matplotlib.pyplot as plt +import numpy as np +import os +import random +import scipy.spatial.transform as tf +import unittest + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.objects.cuboid import DynamicCuboid +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.sensors.height_scanner import HeightScanner, HeightScannerCfg +from omni.isaac.orbit.sensors.height_scanner.utils import create_points_from_grid, plot_height_grid +from omni.isaac.orbit.utils.math import convert_quat +from omni.isaac.orbit.utils.timer import Timer + + +class TestHeightScannerSensor(unittest.TestCase): + """Test fixture for checking height scanner interface.""" + + @classmethod + def tearDownClass(cls): + """Closes simulator after running all test fixtures.""" + simulation_app.close() + + def setUp(self) -> None: + """Create a blank new stage for each test.""" + # Simulation time-step + self.dt = 0.1 + # Load kit helper + self.sim = SimulationContext( + stage_units_in_meters=1.0, physics_dt=self.dt, rendering_dt=self.dt, backend="numpy" + ) + # Set camera view + set_camera_view(eye=[5.0, 5.0, 5.0], target=[0.0, 0.0, 0.0]) + # Spawn things into stage + self._populate_scene() + + # Add height scanner + # -- create query points from a gri + self.grid_size = (1.0, 0.6) + self.grid_resolution = 0.1 + scan_points = create_points_from_grid(self.grid_size, self.grid_resolution) + # -- create sensor instance + scanner_config = HeightScannerCfg( + sensor_tick=0.0, + offset=(0.0, 0.0, 0.0), + points=scan_points, + max_distance=0.45, + ) + self.height_scanner = HeightScanner(scanner_config) + # -- spawn sensor + self.height_scanner.spawn("/World/heightScanner") + self.height_scanner.set_visibility(True) + + def tearDown(self) -> None: + """Stops simulator after each test.""" + self.sim.stop() + self.sim.clear() + + def test_height_scanner_visibility(self): + """Checks that height map visibility method works.""" + # Play simulator + self.sim.reset() + # Setup the stage + self.height_scanner.initialize() + # flag for visualizing sensor + toggle = True + + # Simulate physics + for i in range(100): + # set visibility + if i % 100 == 0: + toggle = not toggle + print(f"Setting visibility: {toggle}") + self.height_scanner.set_visibility(toggle) + # perform rendering + self.sim.step() + # compute yaw -> quaternion + yaw = 10 * i + quat = tf.Rotation.from_euler("z", yaw, degrees=True).as_quat() + quat = convert_quat(quat, "wxyz") + # update sensor + self.height_scanner.update(self.dt, [0.0, 0.0, 0.5], quat) + + def test_static_height_scanner(self): + """Checks that static height map scanner is set correctly and provides right measurements.""" + # Create directory to dump results + test_dir = os.path.dirname(os.path.abspath(__file__)) + plot_dir = os.path.join(test_dir, "output", "height_scan", "static") + if not os.path.exists(plot_dir): + os.makedirs(plot_dir, exist_ok=True) + + # Play simulator + self.sim.reset() + # Setup the stage + self.height_scanner.initialize() + + # Simulate physics + for i in range(5): + # perform rendering + self.sim.step() + # update camera + self.height_scanner.update(self.dt, [0.0, 0.0, 0.5], [1.0, 0.0, 0.0, 0.0]) + # print state + print(self.height_scanner) + # create figure + fig = plt.figure() + ax = plt.gca() + # plot the scanned distance + caxes = plot_height_grid(self.height_scanner.data.hit_distance, self.grid_size, self.grid_resolution, ax=ax) + fig.colorbar(caxes, ax=ax) + # add grid + ax.grid(color="w", linestyle="--", linewidth=1) + # disreset figure + plt.savefig(os.path.join(plot_dir, f"{i:03d}.png"), bbox_inches="tight", pad_inches=0.1) + plt.close() + + def test_dynamic_height_scanner(self): + """Checks that height map scanner works when base frame is rotating.""" + # Create directory to dump results + test_dir = os.path.dirname(os.path.abspath(__file__)) + plot_dir = os.path.join(test_dir, "output", "height_scan", "dynamic") + if not os.path.exists(plot_dir): + os.makedirs(plot_dir, exist_ok=True) + + # Play simulator + self.sim.reset() + # Setup the stage + self.height_scanner.initialize() + + # Simulate physics + for i in range(5): + # perform rendering + self.sim.step() + # compute yaw -> quaternion + yaw = 10 * i + quat = tf.Rotation.from_euler("z", yaw, degrees=True).as_quat() + quat = convert_quat(quat, "wxyz") + # update sensor + self.height_scanner.update(self.dt, [0.0, 0.0, 0.5], quat) + # create figure + fig = plt.figure() + ax = plt.gca() + # plot the scanned distance + caxes = plot_height_grid(self.height_scanner.data.hit_distance, self.grid_size, self.grid_resolution, ax=ax) + fig.colorbar(caxes, ax=ax) + # add grid + ax.grid(color="w", linestyle="--", linewidth=1) + # disreset figure + plt.savefig(os.path.join(plot_dir, f"{i:03d}.png"), bbox_inches="tight", pad_inches=0.1) + plt.close() + + def test_height_scanner_filtering(self): + """Checks that static height map scanner filters out the ground prim. + + The first time, all the cube prims are ignored. After that, they are ignored one-by-one cyclically. + """ + # Create directory to dump results + test_dir = os.path.dirname(os.path.abspath(__file__)) + plot_dir = os.path.join(test_dir, "output", "height_scan", "filter") + if not os.path.exists(plot_dir): + os.makedirs(plot_dir, exist_ok=True) + + # Configure filter prims + self.height_scanner.set_filter_prims([f"/World/Cube/cube{i:02d}" for i in range(4)]) + + # Play simulator + self.sim.reset() + # Setup the stage + self.height_scanner.initialize() + + # Simulate physics + for i in range(6): + # set different filter prims + if i > 0: + cube_id = i - 1 + self.height_scanner.set_filter_prims([f"/World/Cube/cube{cube_id:02}"]) + if i > 4: + self.height_scanner.set_filter_prims(None) + # perform rendering + self.sim.step() + # update sensor + self.height_scanner.update(self.dt, [0.0, 0.0, 0.5], [1.0, 0.0, 0.0, 0.0]) + # create figure + fig = plt.figure() + ax = plt.gca() + # plot the scanned distance + caxes = plot_height_grid(self.height_scanner.data.hit_distance, self.grid_size, self.grid_resolution, ax=ax) + fig.colorbar(caxes, ax=ax) + # add grid + ax.grid(color="w", linestyle="--", linewidth=1) + # disreset figure + plt.savefig(os.path.join(plot_dir, f"{i:03d}.png"), bbox_inches="tight", pad_inches=0.1) + plt.close() + + def test_scanner_throughput(self): + """Measures the scanner throughput while using scan points used for ANYmal robot.""" + # Add height scanner + # -- create sensor instance + scanner_config = HeightScannerCfg( + sensor_tick=0.0, + offset=(0.0, 0.0, 0.0), + points=self._compute_anymal_height_scanner_points(), + max_distance=1.0, + ) + self.anymal_height_scanner = HeightScanner(scanner_config) + # -- spawn sensor + self.anymal_height_scanner.spawn("/World/heightScannerAnymal") + self.anymal_height_scanner.set_visibility(True) + + # Play simulator + self.sim.reset() + # Setup the stage + self.anymal_height_scanner.initialize() + + # Turn rendering on + self.anymal_height_scanner.set_visibility(True) + # Simulate physics + for i in range(2): + # perform rendering + self.sim.step() + # update sensor + with Timer(f"[No Vis , Step {i:02d}]: Scanning time for {scanner_config.points.shape[0]} points"): + self.anymal_height_scanner.update(self.dt, [0.0, 0.0, 0.5], [1.0, 0.0, 0.0, 0.0]) + + # Turn rendering off + self.anymal_height_scanner.set_visibility(False) + # Simulate physics + for i in range(2): + # perform rendering + self.sim.step() + # update sensor + with Timer(f"[With Vis, Step {i:02d}] Scanning time for {scanner_config.points.shape[0]} points"): + self.anymal_height_scanner.update(self.dt, [0.0, 0.0, 0.5], [1.0, 0.0, 0.0, 0.0]) + + """ + Helper functions. + """ + + @staticmethod + def _populate_scene(): + """Add prims to the scene.""" + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane") + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 1.0, "intensity": 30000.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 1.0, "intensity": 30000.0, "color": (1.0, 1.0, 1.0)}, + ) + # Cubes + num_cubes = 4 + for i in range(num_cubes): + # resolve position to put them on vertex of a regular polygon + theta = 2 * np.pi / num_cubes + c, s = np.cos(theta * i), np.sin(theta * i) + rotm = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + position = np.matmul(rotm, np.asarray([0.25, 0.25, 0.1 + 0.05 * i])) + color = np.array([random.random(), random.random(), random.random()]) + # create prim + _ = DynamicCuboid( + prim_path=f"/World/Cube/cube{i:02d}", position=position, size=position[2] * 2, color=color + ) + + @staticmethod + def _compute_anymal_height_scanner_points() -> np.ndarray: + """Computes the query height-scan points relative to base frame of robot. + + Returns: + np.ndarray: A numpy array of shape (N, 3) comprising of quey scan points. + """ + # offset from the base frame - over each leg + offsets = [[0.45, 0.3, 0.0], [-0.46, 0.3, 0.0], [0.45, -0.3, 0.0], [-0.46, -0.3, 0.0]] + offsets = np.asarray(offsets) + # local grid over each offset point + measurement_points = [ + [0.1, 0.0, 0.0], + [0.0, 0.1, 0.0], + [-0.1, 0.0, 0.0], + [0.0, -0.1, 0.0], + [0.1, 0.1, 0.0], + [-0.1, 0.1, 0.0], + [0.1, -0.1, 0.0], + [-0.1, -0.1, 0.0], + [0.2, 0.0, 0.0], + [0.0, 0.2, 0.0], + [-0.2, 0.0, 0.0], + [0.0, -0.2, 0.0], + [0.2, 0.2, 0.0], + [-0.2, 0.2, 0.0], + [0.2, -0.2, 0.0], + [-0.2, -0.2, 0.0], + [0.3, 0.0, 0.0], + [0.3, 0.1, 0.0], + [0.3, 0.2, 0.0], + [0.3, -0.1, 0.0], + [0.3, -0.2, 0.0], + [-0.3, 0.0, 0.0], + [-0.3, 0.1, 0.0], + [-0.3, 0.2, 0.0], + [-0.3, -0.1, 0.0], + [-0.3, -0.2, 0.0], + [0.0, 0.0, 0.0], + ] + measurement_points = np.asarray(measurement_points) + # create a joined list over offsets and local measurement points + # we use broadcasted addition to make this operation quick + scan_points = (offsets[:, None, :] + measurement_points).reshape(-1, 3) + + return scan_points + + +if __name__ == "__main__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit/test/test_keyboard.py b/source/extensions/omni.isaac.orbit/test/test_keyboard.py new file mode 100644 index 0000000000..ac518e9b96 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_keyboard.py @@ -0,0 +1,82 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script shows how to use a teleoperation device with Isaac Sim. + +The teleoperation device is a keyboard device that allows the user to control the robot. +It is possible to add additional callbacks to it for user-defined operations. +""" + +"""Launch Isaac Sim Simulator first.""" + +from omni.isaac.kit import SimulationApp + +# launch the simulator +simulation_app = SimulationApp({"headless": False}) + +"""Rest everything follows.""" + +from omni.isaac.core.simulation_context import SimulationContext + +from omni.isaac.orbit.devices import Se3Keyboard + + +def print_cb(): + """Dummy callback function executed when the key 'L' is pressed.""" + print("Print callback") + + +def quit_cb(): + """Dummy callback function executed when the key 'ESC' is pressed.""" + print("Quit callback") + simulation_app.close() + + +def main(): + # Load kit helper + sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01) + + # Create teleoperation interface + teleop_interface = Se3Keyboard(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) + teleop_interface.add_callback("ESCAPE", quit_cb) + + print("Press 'L' to print a message. Press 'ESC' to quit.") + + # Reset interface internals + teleop_interface.reset() + + # Play simulation + sim.reset() + + # Simulate + 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() + continue + # get keyboard command + delta_pose, gripper_command = teleop_interface.advance() + # print command + if gripper_command: + print(f"Gripper command: {gripper_command}") + # step simulation + sim.step() + # check if simulator is stopped + if sim.is_stopped(): + break + + +if __name__ == "__main__": + # Runs the main function + main() + # Close the simulator + simulation_app.close() diff --git a/source/extensions/omni.isaac.orbit/test/test_kit_utils.py b/source/extensions/omni.isaac.orbit/test/test_kit_utils.py new file mode 100644 index 0000000000..f802ae12de --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_kit_utils.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from omni.isaac.kit import SimulationApp + +# launch the simulator +config = {"headless": False} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import unittest + +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + + +class TestKitUtilities(unittest.TestCase): + """Test fixture for checking Kit utilities in Orbit.""" + + @classmethod + def tearDownClass(cls): + """Closes simulator after running all test fixtures.""" + simulation_app.close() + + def setUp(self) -> None: + """Create a blank new stage for each test.""" + # Simulation time-step + self.dt = 0.1 + # Load kit helper + self.sim = SimulationContext( + stage_units_in_meters=1.0, physics_dt=self.dt, rendering_dt=self.dt, backend="numpy" + ) + # Set camera view + set_camera_view(eye=[1.0, 1.0, 1.0], target=[0.0, 0.0, 0.0]) + # Spawn things into stage + self._populate_scene() + # Wait for spawning + stage_utils.update_stage() + + def tearDown(self) -> None: + """Stops simulator after each test.""" + # stop simulation + self.sim.stop() + self.sim.clear() + + def test_rigid_body_properties(self): + """Disable setting of rigid body properties.""" + # create marker + prim_utils.create_prim( + "/World/marker", usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd" + ) + # set marker properties + kit_utils.set_nested_rigid_body_properties("/World/marker", rigid_body_enabled=False) + kit_utils.set_nested_collision_properties("/World/marker", collision_enabled=False) + + # play simulation + self.sim.reset() + for _ in range(5): + self.sim.step() + + """ + Helper functions. + """ + + @staticmethod + def _populate_scene(): + """Add prims to the scene.""" + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane") + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 1.0, "intensity": 300.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 1.0, "intensity": 300.0, "color": (1.0, 1.0, 1.0)}, + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit/test/test_observation_manager.py b/source/extensions/omni.isaac.orbit/test/test_observation_manager.py new file mode 100644 index 0000000000..cad69fd03e --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_observation_manager.py @@ -0,0 +1,97 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +import unittest +from collections import namedtuple + +from omni.isaac.orbit.utils.mdp.observation_manager import ObservationManager + + +class DefaultObservationManager(ObservationManager): + def grilled_chicken(self, env): + return torch.ones(env.num_envs, 4, device=self.device) + + def grilled_chicken_with_bbq(self, env, bbq: bool): + return bbq * torch.ones(env.num_envs, 1, device=self.device) + + def grilled_chicken_with_curry(self, env, hot: bool): + return hot * 2 * torch.ones(env.num_envs, 1, device=self.device) + + def grilled_chicken_with_yoghurt(self, env, hot: bool, bland: float): + return hot * bland * torch.ones(env.num_envs, 5, device=self.device) + + +class TestObservationManager(unittest.TestCase): + """Test cases for various situations with observation manager.""" + + def setUp(self) -> None: + self.env = namedtuple("IsaacEnv", ["num_envs"])(20) + self.device = "cpu" + + def test_str(self): + cfg = { + "policy": { + "grilled_chicken": {"scale": 10}, + "grilled_chicken_with_bbq": {"scale": 5, "bbq": True}, + "grilled_chicken_with_yoghurt": {"scale": 1.0, "hot": False, "bland": 2.0}, + } + } + self.obs_man = DefaultObservationManager(cfg, self.env, self.device) + self.assertEqual(len(self.obs_man.active_terms["policy"]), 3) + # print the expected string + print() + print(self.obs_man) + + def test_config_terms(self): + cfg = {"policy": {"grilled_chicken": {"scale": 10}, "grilled_chicken_with_curry": {"scale": 0.0, "hot": False}}} + self.obs_man = DefaultObservationManager(cfg, self.env, self.device) + + self.assertEqual(len(self.obs_man.active_terms["policy"]), 2) + + def test_compute(self): + cfg = {"policy": {"grilled_chicken": {"scale": 10}, "grilled_chicken_with_curry": {"scale": 0.0, "hot": False}}} + self.obs_man = DefaultObservationManager(cfg, self.env, self.device) + # compute observation using manager + observations = self.obs_man.compute() + # check the observation shape + self.assertEqual((self.env.num_envs, 5), observations["policy"].shape) + + def test_active_terms(self): + cfg = { + "policy": { + "grilled_chicken": {"scale": 10}, + "grilled_chicken_with_bbq": {"scale": 5, "bbq": True}, + "grilled_chicken_with_curry": {"scale": 0.0, "hot": False}, + } + } + self.obs_man = DefaultObservationManager(cfg, self.env, self.device) + + self.assertEqual(len(self.obs_man.active_terms["policy"]), 3) + + def test_invalid_observation_name(self): + cfg = { + "policy": { + "grilled_chicken": {"scale": 10}, + "grilled_chicken_with_bbq": {"scale": 5, "bbq": True}, + "grilled_chicken_with_no_bbq": {"scale": 0.1, "hot": False}, + } + } + with self.assertRaises(AttributeError): + self.obs_man = DefaultObservationManager(cfg, self.env, self.device) + + def test_invalid_observation_config(self): + cfg = { + "policy": { + "grilled_chicken_with_bbq": {"scale": 0.1, "hot": False}, + "grilled_chicken_with_yoghurt": {"scale": 2.0, "hot": False}, + } + } + with self.assertRaises(ValueError): + self.obs_man = DefaultObservationManager(cfg, self.env, self.device) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit/test/test_reward_manager.py b/source/extensions/omni.isaac.orbit/test/test_reward_manager.py new file mode 100644 index 0000000000..720a7551e2 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_reward_manager.py @@ -0,0 +1,97 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import unittest +from collections import namedtuple + +from omni.isaac.orbit.utils.mdp.reward_manager import RewardManager + + +class DefaultRewardManager(RewardManager): + def grilled_chicken(self, env): + return 1 + + def grilled_chicken_with_bbq(self, env, bbq: bool): + return 0 + + def grilled_chicken_with_curry(self, env, hot: bool): + return 0 + + def grilled_chicken_with_yoghurt(self, env, hot: bool, bland: float): + return 0 + + +class TestRewardManager(unittest.TestCase): + """Test cases for various situations with reward manager.""" + + def setUp(self) -> None: + self.env = namedtuple("IsaacEnv", [])() + self.device = "cpu" + self.num_envs = 20 + self.dt = 0.1 + + def test_str(self): + cfg = { + "grilled_chicken": {"weight": 10}, + "grilled_chicken_with_bbq": {"weight": 5, "bbq": True}, + "grilled_chicken_with_yoghurt": {"weight": 1.0, "hot": False, "bland": 2.0}, + } + self.rew_man = DefaultRewardManager(cfg, self.env, self.num_envs, self.dt, self.device) + self.assertEqual(len(self.rew_man.active_terms), 3) + # print the expected string + print() + print(self.rew_man) + + def test_config_terms(self): + cfg = {"grilled_chicken": {"weight": 10}, "grilled_chicken_with_curry": {"weight": 0.0, "hot": False}} + self.rew_man = DefaultRewardManager(cfg, self.env, self.num_envs, self.dt, self.device) + + self.assertEqual(len(self.rew_man.active_terms), 1) + + def test_compute(self): + cfg = {"grilled_chicken": {"weight": 10}, "grilled_chicken_with_curry": {"weight": 0.0, "hot": False}} + self.rew_man = DefaultRewardManager(cfg, self.env, self.num_envs, self.dt, self.device) + # compute expected reward + expected_reward = cfg["grilled_chicken"]["weight"] * self.dt + # compute reward using manager + rewards = self.rew_man.compute() + # check the reward for environment index 0 + self.assertEqual(float(rewards[0]), expected_reward) + + def test_active_terms(self): + cfg = { + "grilled_chicken": {"weight": 10}, + "grilled_chicken_with_bbq": {"weight": 5, "bbq": True}, + "grilled_chicken_with_curry": {"weight": 0.0, "hot": False}, + } + self.rew_man = DefaultRewardManager(cfg, self.env, self.num_envs, self.dt, self.device) + + self.assertEqual(len(self.rew_man.active_terms), 2) + + def test_invalid_reward_name(self): + cfg = { + "grilled_chicken": {"weight": 10}, + "grilled_chicken_with_bbq": {"weight": 5, "bbq": True}, + "grilled_chicken_with_no_bbq": {"weight": 0.1, "hot": False}, + } + with self.assertRaises(AttributeError): + self.rew_man = DefaultRewardManager(cfg, self.env, self.num_envs, self.dt, self.device) + + def test_invalid_reward_weight_config(self): + cfg = {"grilled_chicken": {}} + with self.assertRaises(KeyError): + self.rew_man = DefaultRewardManager(cfg, self.env, self.num_envs, self.dt, self.device) + + def test_invalid_reward_config(self): + cfg = { + "grilled_chicken_with_bbq": {"weight": 0.1, "hot": False}, + "grilled_chicken_with_yoghurt": {"weight": 2.0, "hot": False}, + } + with self.assertRaises(ValueError): + self.rew_man = DefaultRewardManager(cfg, self.env, self.num_envs, self.dt, self.device) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit/test/test_timer.py b/source/extensions/omni.isaac.orbit/test/test_timer.py new file mode 100644 index 0000000000..868a06b18f --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/test_timer.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import time +import unittest + +from omni.isaac.orbit.utils.timer import Timer + + +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(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_tiemr_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__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit_envs/config/extension.toml b/source/extensions/omni.isaac.orbit_envs/config/extension.toml new file mode 100644 index 0000000000..eb03b95b1b --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/config/extension.toml @@ -0,0 +1,21 @@ +[package] + +# Note: Semantic Versioning is used: https://semver.org/ +version = "0.1.0" + +# Description +title = "ORBIT Environments" +description="Extension containing suite of environments for robot learning." +readme = "docs/README.md" +repository = "https://github.com/NVIDIA-Omniverse/Orbit" +category = "robotics" +keywords = ["robotics", "rl", "il", "learning"] + +[dependencies] +"omni.isaac.orbit" = {} +"omni.isaac.core" = {} +"omni.isaac.gym" = {} +"omni.replicator.isaac" = {} + +[[python.module]] +name = "omni.isaac.orbit_envs" diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/ant_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/ant_ppo.yaml new file mode 100644 index 0000000000..94ac7e99d5 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/ant_ppo.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: ant + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: 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: 3e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 500 + save_best_after: 200 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 4096 # 32768 + mini_epochs: 8 + critic_coef: 2 + clip_value: True + seq_len: 4 + bounds_loss_coef: 0.0001 diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/cabinet_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/cabinet_ppo.yaml new file mode 100644 index 0000000000..4b1a72f25b --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/cabinet_ppo.yaml @@ -0,0 +1,75 @@ +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: cabinet + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 + reward_shaper: + scale_value: 1 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 100000000 + max_epochs: 300 #300 + save_best_after: 100 + save_frequency: 100 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 32 #64 + minibatch_size: 4096 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_len: 4 + bounds_loss_coef: 0.0001 diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/cartpole_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/cartpole_ppo.yaml new file mode 100644 index 0000000000..37a877282b --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/cartpole_ppo.yaml @@ -0,0 +1,78 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [32, 32] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cartpole + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 + reward_shaper: + scale_value: 1.0 + normalize_advantage: False + gamma: 0.99 + tau : 0.95 + learning_rate: 3e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 450 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 8192 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_len: 4 + bounds_loss_coef: 0 diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/cartpole_sac.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/cartpole_sac.yaml new file mode 100644 index 0000000000..039dab5c90 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/cartpole_sac.yaml @@ -0,0 +1,87 @@ +params: + seed: 12344 + + # environment wrapper clipping + env: + clip_observations: 5.0 + clip_actions: 1.0 + + algo: + name: sac + + model: + name: soft_actor_critic + + network: + name: soft_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: [32, 32] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + log_std_bounds: [-5, 2] + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cartpole + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + # ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 + reward_shaper: + scale_value: 0.1 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 3e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 65 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + batch_size: 8192 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_len: 4 + bounds_loss_coef: 0.0001 + + num_seed_steps: 5 + num_steps_per_episode: 16 + critic_tau: 0.005 + init_alpha: 1 + learnable_temperature: True + replay_buffer_size: 1000000 + actor_lr: 0.0005 + critic_lr: 0.0005 + alpha_lr: 0.005 diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/humanoid_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/humanoid_ppo.yaml new file mode 100644 index 0000000000..a392a16b8f --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/humanoid_ppo.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [400, 200, 100] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: humanoid + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: 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: 3e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 500 + save_best_after: 500 + save_frequency: 100 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 32 + minibatch_size: 4096 # 32768 + mini_epochs: 8 + critic_coef: 2 + clip_value: True + seq_len: 4 + bounds_loss_coef: 0.0001 diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/lift_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/lift_ppo.yaml new file mode 100644 index 0000000000..fe6933fff4 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/lift_ppo.yaml @@ -0,0 +1,78 @@ +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 # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: 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: 3e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.008 + score_to_win: 100000000 + max_epochs: 5000 + save_best_after: 200 + save_frequency: 100 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 2048 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_len: 4 + bounds_loss_coef: 0.0001 diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/reach_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/reach_ppo.yaml new file mode 100644 index 0000000000..d996a8e75d --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/reach_ppo.yaml @@ -0,0 +1,78 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 100.0 + clip_actions: 100.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [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: 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: False + num_actors: -1 + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 3e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.008 + score_to_win: 100000000 + max_epochs: 500 + save_best_after: 100 + save_frequency: 50 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 4096 #2048 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_len: 4 + bounds_loss_coef: 0.0001 diff --git a/source/extensions/omni.isaac.orbit_envs/data/robomimic/lift_bc.json b/source/extensions/omni.isaac.orbit_envs/data/robomimic/lift_bc.json new file mode 100644 index 0000000000..e72c2d7b23 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/robomimic/lift_bc.json @@ -0,0 +1,263 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc", + "validate": true, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 50, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "validation_epoch_every_n_steps": 10, + "env": null, + "additional_envs": null, + "render": false, + "render_video": true, + "keep_all_videos": false, + "video_skip": 5, + "rollout": { + "enabled": false, + "n": 50, + "horizon": 400, + "rate": 50, + "warmstart": 0, + "terminate_on_success": true + } + }, + "train": { + "data": null, + "output_dir": "../bc_trained_models", + "num_data_workers": 0, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "seq_length": 1, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 200, + "seed": 1 + }, + "algo": { + "optim_params": { + "policy": { + "learning_rate": { + "initial": 0.0001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [ + 1024, + 1024 + ], + "gaussian": { + "enabled": false, + "fixed_std": false, + "init_std": 0.1, + "min_std": 0.01, + "std_activation": "softplus", + "low_noise_eval": true + }, + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": false, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "rnn": { + "enabled": false, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "tool_dof_pos_scaled", + "tool_positions", + "object_positions", + "object_desired_positions" + ], + "rgb": [], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + }, + "conv_activation": "relu", + "conv_kwargs": { + "out_channels": [ + 32, + 64, + 64 + ], + "kernel_size": [ + 8, + 4, + 2 + ], + "stride": [ + 4, + 2, + 1 + ] + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + } + } + } +} diff --git a/source/extensions/omni.isaac.orbit_envs/data/robomimic/lift_bcq.json b/source/extensions/omni.isaac.orbit_envs/data/robomimic/lift_bcq.json new file mode 100644 index 0000000000..8eb938a503 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/robomimic/lift_bcq.json @@ -0,0 +1,299 @@ +{ + "algo_name": "bcq", + "experiment": { + "name": "bcq", + "validate": true, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 50, + "epochs": [], + "on_best_validation": true, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": false + }, + "epoch_every_n_steps": 100, + "validation_epoch_every_n_steps": 10, + "env": null, + "additional_envs": null, + "render": false, + "render_video": true, + "keep_all_videos": false, + "video_skip": 5, + "rollout": { + "enabled": false, + "n": 50, + "horizon": 400, + "rate": 50, + "warmstart": 0, + "terminate_on_success": true + } + }, + "train": { + "data": null, + "output_dir": "../bcq_trained_models", + "num_data_workers": 0, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "seq_length": 1, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 200, + "seed": 1 + }, + "algo": { + "optim_params": { + "critic": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + }, + "action_sampler": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + }, + "actor": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + } + }, + "discount": 0.99, + "n_step": 1, + "target_tau": 0.005, + "infinite_horizon": false, + "critic": { + "use_huber": false, + "max_gradient_norm": null, + "value_bounds": null, + "num_action_samples": 10, + "num_action_samples_rollout": 100, + "ensemble": { + "n": 2, + "weight": 0.75 + }, + "distributional": { + "enabled": false, + "num_atoms": 51 + }, + "layer_dims": [ + 300, + 400 + ] + }, + "action_sampler": { + "actor_layer_dims": [ + 1024, + 1024 + ], + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": true, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "freeze_encoder_epoch": -1 + }, + "actor": { + "enabled": false, + "perturbation_scale": 0.05, + "layer_dims": [ + 300, + 400 + ] + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "tool_dof_pos_scaled", + "tool_positions", + "object_positions", + "object_desired_positions" + ], + "rgb": [], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + }, + "conv_activation": "relu", + "conv_kwargs": { + "out_channels": [ + 32, + 64, + 64 + ], + "kernel_size": [ + 8, + 4, + 2 + ], + "stride": [ + 4, + 2, + 1 + ] + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + } + } + } +} diff --git a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/anymal_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/anymal_ppo.yaml new file mode 100644 index 0000000000..0d30c08844 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/anymal_ppo.yaml @@ -0,0 +1,46 @@ +# seed for the experiment +seed: 42 +# device for the rl-agent +device: 'cuda:0' + +policy: + init_noise_std: 1.0 + actor_hidden_dims: [128, 128, 128] + critic_hidden_dims: [128, 128, 128] + activation: "elu" # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid + + # only required when "policy_class_name" is'ActorCriticRecurrent': + # rnn_type: 'lstm' + # rnn_hidden_size: 512 + # rnn_num_layers: 1 + +algorithm: + # training params + 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 # mini batch size: num_envs * nsteps / nminibatches + learning_rate: 1.0e-3 # 5.e-4 + schedule: "adaptive" # adaptive, fixed + gamma: 0.99 + lam: 0.95 + desired_kl: 0.01 + max_grad_norm: 1.0 + +runner: + policy_class_name: "ActorCritic" + algorithm_class_name: "PPO" + num_steps_per_env: 24 # per iteration + max_iterations: 500 # number of policy updates + + # logging + save_interval: 50 # check for potential saves every this many iterations + experiment_name: "anymal_c" + run_name: "" + # load and resume + resume: False + load_run: -1 # -1: last run + checkpoint: -1 # -1: last saved model + resume_path: None # updated from load_run and chkpt diff --git a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/cabinet_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/cabinet_ppo.yaml new file mode 100644 index 0000000000..d6c9b0a255 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/cabinet_ppo.yaml @@ -0,0 +1,55 @@ +# seed for the experiment +seed: 67854 +# device for the rl-agent +device: 'cuda:0' + +policy: + init_noise_std: 1.0 + actor_hidden_dims: [256, 128, 64] + critic_hidden_dims: [256, 128, 64] + activation: "elu" # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid + + # only required when "policy_class_name" is'ActorCriticRecurrent': + # rnn_type: 'lstm' + # rnn_hidden_size: 512 + # rnn_num_layers: 1 + +algorithm: + # training params + value_loss_coef: 1.0 + use_clipped_value_loss: True + # Seems to be the same as clip_range + clip_param: 0.2 + # Seems to be the same as ent_coef + entropy_coef: 0.00 + # ? seems to be the same as n_epochs + num_learning_epochs: 8 + # seems to be the same as batch_size + num_mini_batches: 8 # mini batch size: num_envs * nsteps / nminibatches + # seems to be same as learning_ratena + learning_rate: 5.0e-4 # 5.e-4 + schedule: "adaptive" # adaptive, fixed + # same as gamma + gamma: 0.99 + # seems to be the same as gae_lambda + lam: 0.95 + # seems to be the same as target_kl + desired_kl: 0.008 + # seems to be the same as max_grad_norm + max_grad_norm: 1.0 + +runner: + policy_class_name: "ActorCritic" + algorithm_class_name: "PPO" + num_steps_per_env: 64 # per iteration + max_iterations: 200 # number of policy updates + + # logging + save_interval: 50 # check for potential saves every this many iterations + experiment_name: "reach" + run_name: "" + # load and resume + resume: False + load_run: -1 # -1: last run + checkpoint: -1 # -1: last saved model + resume_path: None # updated from load_run and chkpt diff --git a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/cartpole_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/cartpole_ppo.yaml new file mode 100644 index 0000000000..f4cda1c346 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/cartpole_ppo.yaml @@ -0,0 +1,46 @@ +# seed for the experiment +seed: 42 +# device for the rl-agent +device: 'cuda:0' + +policy: + init_noise_std: 1.0 + actor_hidden_dims: [256, 128, 64] + critic_hidden_dims: [256, 128, 64] + activation: "elu" # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid + + # only required when "policy_class_name" is'ActorCriticRecurrent': + # rnn_type: 'lstm' + # rnn_hidden_size: 512 + # rnn_num_layers: 1 + +algorithm: + # training params + value_loss_coef: 1.0 + use_clipped_value_loss: True + clip_param: 0.2 + entropy_coef: 0.01 + num_learning_epochs: 8 + num_mini_batches: 4 # mini batch size: num_envs * nsteps / nminibatches + learning_rate: 1.0e-3 # 5.e-4 + schedule: "adaptive" # adaptive, fixed + gamma: 0.99 + lam: 0.95 + desired_kl: 0.01 + max_grad_norm: 1.0 + +runner: + policy_class_name: "ActorCritic" + algorithm_class_name: "PPO" + num_steps_per_env: 16 # per iteration + max_iterations: 500 # number of policy updates + + # logging + save_interval: 50 # check for potential saves every this many iterations + experiment_name: "lift" + run_name: "" + # load and resume + resume: False + load_run: -1 # -1: last run + checkpoint: -1 # -1: last saved model + resume_path: None # updated from load_run and chkpt diff --git a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml new file mode 100644 index 0000000000..20f63fce44 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml @@ -0,0 +1,46 @@ +# seed for the experiment +seed: 42 +# device for the rl-agent +device: 'cuda:0' + +policy: + init_noise_std: 1.0 + actor_hidden_dims: [256, 128, 64] + critic_hidden_dims: [256, 128, 64] + activation: "elu" # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid + + # only required when "policy_class_name" is'ActorCriticRecurrent': + # rnn_type: 'lstm' + # rnn_hidden_size: 512 + # rnn_num_layers: 1 + +algorithm: + # training params + 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 # mini batch size: num_envs * nsteps / nminibatches + learning_rate: 1.0e-3 # 5.e-4 + schedule: "adaptive" # adaptive, fixed + gamma: 0.99 + lam: 0.95 + desired_kl: 0.01 + max_grad_norm: 1.0 + +runner: + policy_class_name: "ActorCritic" + algorithm_class_name: "PPO" + num_steps_per_env: 192 # per iteration + max_iterations: 1500 # number of policy updates + + # logging + save_interval: 50 # check for potential saves every this many iterations + experiment_name: "lift" + run_name: "" + # load and resume + resume: False + load_run: -1 # -1: last run + checkpoint: -1 # -1: last saved model + resume_path: None # updated from load_run and chkpt diff --git a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/reach_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/reach_ppo.yaml new file mode 100644 index 0000000000..70f0114748 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/reach_ppo.yaml @@ -0,0 +1,55 @@ +# seed for the experiment +seed: 42 +# device for the rl-agent +device: 'cuda:0' + +policy: + init_noise_std: 1.0 + actor_hidden_dims: [256, 128, 64] + critic_hidden_dims: [256, 128, 64] + activation: "elu" # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid + + # only required when "policy_class_name" is'ActorCriticRecurrent': + # rnn_type: 'lstm' + # rnn_hidden_size: 512 + # rnn_num_layers: 1 + +algorithm: + # training params + value_loss_coef: 1.0 + use_clipped_value_loss: True + # Seems to be the same as clip_range + clip_param: 0.2 + # Seems to be the same as ent_coef + entropy_coef: 0.01 + # ? seems to be the same as n_epochs + num_learning_epochs: 8 + # seems to be the same as batch_size + num_mini_batches: 64 # mini batch size: num_envs * nsteps / nminibatches + # seems to be same as learning_rate + learning_rate: 3.0e-4 # 5.e-4 + schedule: "adaptive" # adaptive, fixed + # same as gamma + gamma: 0.99 + # seems to be the same as gae_lambda + lam: 0.95 + # seems to be the same as target_kl + desired_kl: 0.01 + # seems to be the same as max_grad_norm + max_grad_norm: 1.0 + +runner: + policy_class_name: "ActorCritic" + algorithm_class_name: "PPO" + num_steps_per_env: 64 # per iteration + max_iterations: 250 # number of policy updates + + # logging + save_interval: 50 # check for potential saves every this many iterations + experiment_name: "reach" + run_name: "" + # load and resume + resume: False + load_run: -1 # -1: last run + checkpoint: -1 # -1: last saved model + resume_path: None # updated from load_run and chkpt diff --git a/source/extensions/omni.isaac.orbit_envs/data/sb3/ant_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/sb3/ant_ppo.yaml new file mode 100644 index 0000000000..dd581c480c --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/sb3/ant_ppo.yaml @@ -0,0 +1,22 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L161 + +n_timesteps: !!float 2e6 +policy: 'MlpPolicy' +batch_size: 128 +n_steps: 512 +gamma: 0.99 +gae_lambda: 0.9 +n_epochs: 20 +ent_coef: 0.0 +sde_sample_freq: 4 +max_grad_norm: 0.5 +vf_coef: 0.5 +learning_rate: !!float 3e-5 +use_sde: True +clip_range: 0.4 +policy_kwargs: "dict( + log_std_init=-2, + ortho_init=False, + activation_fn=nn.ReLU, + net_arch=[dict(pi=[256, 256], vf=[256, 256])] + )" diff --git a/source/extensions/omni.isaac.orbit_envs/data/sb3/cabinet_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/sb3/cabinet_ppo.yaml new file mode 100644 index 0000000000..5d9e2fde51 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/sb3/cabinet_ppo.yaml @@ -0,0 +1,29 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +# 512×1500×64 +seed: 42 +n_timesteps: !!float 49152000 +policy: 'MlpPolicy' +n_steps: 64 +batch_size: 4096 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 20 +ent_coef: 0.0 +learning_rate: !!float 5e-4 +clip_range: 0.2 +policy_kwargs: "dict( + log_std_init=-2, + ortho_init=False, + activation_fn=nn.ELU, + net_arch=[256, 128, 64] + )" + + +target_kl: 0.008 +max_grad_norm: 1.0 + +# Uses VecNormalize class to normalize obs +normalize_input: True +# Uses VecNormalize class to normalize rew +normalize_value: True +clip_obs: 5.0 diff --git a/source/extensions/omni.isaac.orbit_envs/data/sb3/cartpole_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/sb3/cartpole_ppo.yaml new file mode 100644 index 0000000000..4fe67a2e11 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/sb3/cartpole_ppo.yaml @@ -0,0 +1,30 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +seed: 42 + +# 512×500×16 +n_timesteps: !!float 2e6 +policy: 'MlpPolicy' +n_steps: 16 +batch_size: 8192 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 8 +ent_coef: 0.0 +learning_rate: !!float 3e-4 +clip_range: 0.2 +policy_kwargs: "dict( + log_std_init=-2, + ortho_init=False, + activation_fn=nn.ELU, + net_arch=[32, 32] + )" + + +target_kl: 0.008 +max_grad_norm: 1.0 + +# Uses VecNormalize class to normalize obs +normalize_input: True +# Uses VecNormalize class to normalize rew +normalize_value: True +clip_obs: 5 diff --git a/source/extensions/omni.isaac.orbit_envs/data/sb3/cartpole_sac.yaml b/source/extensions/omni.isaac.orbit_envs/data/sb3/cartpole_sac.yaml new file mode 100644 index 0000000000..2ffe71beee --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/sb3/cartpole_sac.yaml @@ -0,0 +1,22 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +seed: 67854 + +# 512×500×16 +n_timesteps: 4096000 +policy: 'MlpPolicy' +train_freq: 16 +batch_size: 8192 +gamma: 0.99 +gradient_steps: 8 +ent_coef: 0.0 +learning_rate: !!float 3e-4 +policy_kwargs: "dict( + log_std_init=-2, + activation_fn=nn.ELU, + net_arch=[32, 32] + )" +# Uses VecNormalize class to normalize obs +normalize_input: True +# Uses VecNormalize class to normalize rew +normalize_value: True +clip_obs: 5 diff --git a/source/extensions/omni.isaac.orbit_envs/data/sb3/humanoid_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/sb3/humanoid_ppo.yaml new file mode 100644 index 0000000000..600d2a27f5 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/sb3/humanoid_ppo.yaml @@ -0,0 +1,20 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L457 + +policy: 'MlpPolicy' +n_timesteps: !!float 1e7 +batch_size: 256 +n_steps: 512 +gamma: 0.95 +learning_rate: 3.56987e-05 +ent_coef: 0.00238306 +clip_range: 0.3 +n_epochs: 5 +gae_lambda: 0.9 +max_grad_norm: 2 +vf_coef: 0.431892 +policy_kwargs: "dict( + log_std_init=-2, + ortho_init=False, + activation_fn=nn.ReLU, + net_arch=[dict(pi=[256, 256], vf=[256, 256])] + )" diff --git a/source/extensions/omni.isaac.orbit_envs/data/sb3/reach_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/sb3/reach_ppo.yaml new file mode 100644 index 0000000000..f46963048b --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/data/sb3/reach_ppo.yaml @@ -0,0 +1,30 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +# epoch * n_steps * nenvs: 500×512*8*8 +n_timesteps: !!float 16384000 +policy: 'MlpPolicy' +n_steps: 64 +# mini batch size: num_envs * nsteps / nminibatches 2048×512÷2048 +batch_size: 192 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 8 +ent_coef: 0.00 +vf_coef: 0.0001 +learning_rate: !!float 3e-4 +clip_range: 0.2 +policy_kwargs: "dict( + activation_fn=nn.ELU, + net_arch=[32, 32, dict(pi=[256, 128, 64], vf=[256, 128, 64])] + )" + + +target_kl: 0.01 +max_grad_norm: 1.0 + + + +# # Uses VecNormalize class to normalize obs +# normalize_input: True +# # Uses VecNormalize class to normalize rew +# normalize_value: True +# clip_obs: 5 diff --git a/source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst b/source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst new file mode 100644 index 0000000000..65195e1610 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst @@ -0,0 +1,18 @@ +Changelog +--------- + +0.1.0 (2023-01-17) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Initial release of the extension. +* Includes the following environments: + + * ``Isaac-Cartpole-v0``: A cartpole environment with a continuous action space. + * ``Isaac-Ant-v0``: A 3D ant environment with a continuous action space. + * ``Isaac-Humanoid-v0``: A 3D humanoid environment with a continuous action space. + * ``Isaac-Reach-Franka-v0``: A end-effector pose tracking task for the Franka arm. + * ``Isaac-Lift-Franka-v0``: A 3D object lift and reposing task for the Franka arm. + * ``Isaac-Velocity-Anymal-C-v0``: An SE(2) velocity tracking task for legged robot on flat terrain. diff --git a/source/extensions/omni.isaac.orbit_envs/docs/README.md b/source/extensions/omni.isaac.orbit_envs/docs/README.md new file mode 100644 index 0000000000..4c4d90d217 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/docs/README.md @@ -0,0 +1,47 @@ +# Orbit: Environment Suite + +Using the core framework developed as part of Orbit, we provide various learning environments for robotics research. +These environments follow the `gym.Env` API from OpenAI Gym version `0.21.0`. The environments are registered using +the Gym registry. + +Each environment's name is composed of `Isaac---v`, where `` indicates the skill to learn +in the environment, `` indicates the embodiment of the acting agent, and `` represents the version of +the environment (which can be used to suggest different observation or action spaces). + +The environments are configured using either Python classes (wrapped using `configclass` decorator) or through +YAML files. The template structure of the environment is always put at the same level as the environment file +itself. However, its various instances are included in directories within the environment directory itself. +This looks like as follows: + +```tree +omni/isaac/orbit_envs/locomotion/ +├── __init__.py +└── velocity + ├── a1 + │ └── flat_terrain_cfg.py + ├── anymal_c + │ └── flat_terrain_cfg.py + ├── __init__.py + ├── velocity_cfg.py + └── velocity_env.py +``` + +The environments are then registered in the `omni/isaac/orbit_envs/__init__.py`: + +```python +gym.register( + id="Isaac-Velocity-Anymal-C-v0", + entry_point="omni.isaac.orbit_envs.locomotion.velocity:VelocityEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.locomotion.velocity.anymal_c.flat_terrain_cfg:FlatTerrainCfg"}, +) + +gym.register( + id="Isaac-Velocity-A1-v0", + entry_point="omni.isaac.orbit_envs.locomotion.velocity:VelocityEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.locomotion.velocity.a1.flat_terrain_cfg:FlatTerrainCfg"}, +) +``` + +> **Note:** As a practice, we specify all the environments in a single file to avoid name conflicts between different +> tasks or environments. However, this practice is debatable and we are open to suggestions to deal with a large +> scaling in the number of tasks or environments. diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/__init__.py new file mode 100644 index 0000000000..c6e98a5b02 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/__init__.py @@ -0,0 +1,87 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Module containing environments with OpenAI Gym interface. + + +We use OpenAI Gym registry to register the environment and their default configuration file. +The default configuration file is passed to the argument "kwargs" in the Gym specification registry. +The string is parsed into respective configuration container which needs to be passed to the environment +class. This is done using the function :meth:`load_default_env_cfg` in the sub-module +:mod:`omni.isaac.orbit.utils.parse_cfg`. + +Note: + This is a slight abuse of kwargs since they are meant to be directly passed into the environment class. + Instead, we remove the key :obj:`cfg_file` from the "kwargs" dictionary and the user needs to provide + the kwarg argument :obj:`cfg` while creating the environment. + +Usage: + >>> import gym + >>> import omni.isaac.orbit_envs + >>> from omni.isaac.orbit_envs.utils.parse_cfg import load_default_env_cfg + >>> + >>> task_name = "Isaac-Cartpole-v0" + >>> cfg = load_default_env_cfg(task_name) + >>> env = gym.make(task_name, cfg=cfg) +""" + + +import gym +import os + +# Conveniences to other module directories via relative paths +ORBIT_ENVS_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) +"""Path to the extension source directory.""" + +ORBIT_ENVS_DATA_DIR = os.path.join(ORBIT_ENVS_EXT_DIR, "data") +"""Path to the extension data directory.""" + +## +# Classic control +## + +gym.register( + id="Isaac-Cartpole-v0", + entry_point="omni.isaac.orbit_envs.classic.cartpole:CartpoleEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.classic.cartpole:cartpole_cfg.yaml"}, +) + +gym.register( + id="Isaac-Ant-v0", + entry_point="omni.isaac.orbit_envs.classic.ant:AntEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.classic.ant:ant_cfg.yaml"}, +) + +gym.register( + id="Isaac-Humanoid-v0", + entry_point="omni.isaac.orbit_envs.classic.humanoid:HumanoidEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.classic.humanoid:humanoid_cfg.yaml"}, +) + +## +# Locomotion +## + +gym.register( + id="Isaac-Velocity-Anymal-C-v0", + entry_point="omni.isaac.orbit_envs.locomotion.velocity:VelocityEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.locomotion.velocity:VelocityEnvCfg"}, +) + +## +# Manipulation +## + +gym.register( + id="Isaac-Reach-Franka-v0", + entry_point="omni.isaac.orbit_envs.manipulation.reach:ReachEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.manipulation.reach:ReachEnvCfg"}, +) + +gym.register( + id="Isaac-Lift-Franka-v0", + entry_point="omni.isaac.orbit_envs.manipulation.lift:LiftEnv", + kwargs={"cfg_entry_point": "omni.isaac.orbit_envs.manipulation.lift:LiftEnvCfg"}, +) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/__init__.py new file mode 100644 index 0000000000..2b676f335e --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/__init__.py @@ -0,0 +1,18 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Classic environments for control. + +These environments are based on the MuJoCo environments provided by OpenAI. + +Reference: + https://github.com/openai/gym/tree/master/gym/envs/mujoco +""" + +from .ant import AntEnv +from .cartpole import CartpoleEnv +from .humanoid import HumanoidEnv + +__all__ = ["CartpoleEnv", "AntEnv", "HumanoidEnv"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/__init__.py new file mode 100644 index 0000000000..033951879d --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Ant locomotion environment (similar to OpenAI Gym Ant-v2). +""" + +from .ant_env import AntEnv + +__all__ = ["AntEnv"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/ant_cfg.yaml b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/ant_cfg.yaml new file mode 100644 index 0000000000..c9f45e6cf2 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/ant_cfg.yaml @@ -0,0 +1,75 @@ +# parameters for the MDP +env: + # general + num_envs: 1024 + env_spacing: 5 + episode_length: 1000 + + # step parameters + control_frequency_inv: 2 # 60 Hz + # actions parameters + power_scale: 0.5 + # observations parameters + angular_velocity_scale: 1.0 + dof_velocity_scale: 0.2 + contact_force_scale: 0.1 + + # reward parameters + heading_weight: 0.5 + up_weight: 0.1 + alive_weight: 0.5 + # cost parameters + actions_cost: 0.005 + energy_cost: 0.05 + joints_at_limit_cost: 0.1 + # termination parameters + death_cost: -2.0 + termination_height: 0.31 + +# parameters for setting up the scene +scene: + + ant: + # articulation + solver_position_iteration_count: 4 + solver_velocity_iteration_count: 0 + sleep_threshold: 0.005 + stabilization_threshold: 0.001 + enable_self_collisions: True + # per-body + enable_gyroscopic_forces: True + max_depenetration_velocity: 10.0 + # per-shape + contact_offset: 0.02 + rest_offset: 0.0 + +# parameters for physics engine +sim: + dt: 0.0083 # 1/120 s + substeps: 1 + gravity: [0.0, 0.0, -9.81] + + enable_scene_query_support: False + use_gpu_pipeline: True + use_flatcache: True + device: "cuda:0" + + physx: + solver_type: 1 + use_gpu: True # set to False to run on CPU + bounce_threshold_velocity: 0.2 + friction_offset_threshold: 0.04 + friction_correlation_distance: 0.025 + enable_stabilization: True + + # GPU buffers + gpu_max_rigid_contact_count: 524288 + gpu_max_rigid_patch_count: 81920 + gpu_found_lost_pairs_capacity: 1024 + gpu_found_lost_aggregate_pairs_capacity: 262144 + gpu_total_aggregate_pairs_capacity: 2048 + gpu_max_soft_body_contacts: 1048576 + gpu_max_particle_contacts: 1048576 + gpu_heap_capacity: 67108864 + gpu_temp_buffer_capacity: 16777216 + gpu_max_num_partitions: 8 diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/ant_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/ant_env.py new file mode 100644 index 0000000000..7eea1490e6 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/ant/ant_env.py @@ -0,0 +1,315 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gym.spaces +import math +import torch +from typing import List + +import omni.isaac.core.utils.nucleus as nucleus_utils +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.torch as torch_utils +from omni.isaac.core.articulations import ArticulationView + +import omni.isaac.orbit.utils.kit as kit_utils + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvIndices, VecEnvObs +from omni.isaac.orbit_envs.isaac_env_cfg import EnvCfg, IsaacEnvCfg + + +class AntEnv(IsaacEnv): + """Environment for an Ant with four legs on a flat terrain. + + Reference: + https://github.com/openai/gym/blob/master/gym/envs/mujoco/ant_v3.py + """ + + def __init__(self, cfg: dict, headless: bool = False): + """Initializes the environment. + + Args: + cfg (dict): The configuration dictionary. + headless (bool, optional): Whether to enable rendering or not. Defaults to False. + """ + # copy configuration + self.cfg_dict = cfg.copy() + # configuration for the environment + isaac_cfg = IsaacEnvCfg( + env=EnvCfg(num_envs=self.cfg_dict["env"]["num_envs"], env_spacing=self.cfg_dict["env"]["env_spacing"]) + ) + isaac_cfg.sim.from_dict(self.cfg_dict["sim"]) + # initialize the base class to setup the scene. + super().__init__(isaac_cfg, headless=headless) + + # define views over instances + self.ants = ArticulationView(prim_paths_expr=self.env_ns + "/.*/Ant/torso", reset_xform_properties=False) + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + self.sim.reset() + # initialize all the handles + self.ants.initialize(self.sim.physics_sim_view) + # set the default state + self.ants.post_reset() + + # get quantities from scene we care about + self._dof_limits = self.ants.get_dof_limits()[0, :].to(self.device) + self._initial_root_tf = self.ants.get_world_poses(clone=True) + self._initial_dof_pos = self.ants.get_joint_positions(clone=True) + + # initialize buffers + self.actions = torch.zeros((self.num_envs, 8), device=self.device) + # create constants required later during simulation. + self._define_environment_constants() + # create other useful variables + self.potentials = torch.full( + (self.num_envs,), -1000.0 / self.physics_dt, dtype=torch.float32, device=self.device + ) + self.prev_potentials = self.potentials.clone() + + # compute the observation space + self.observation_space = gym.spaces.Box(low=-math.inf, high=math.inf, shape=(60,)) + # compute the action space + self.action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(8,)) + # store maximum episode length + self.max_episode_length = self.cfg_dict["env"]["episode_length"] + + """ + Implementation specifics. + """ + + def _design_scene(self) -> List[str]: + # get nucleus assets path + assets_root_path = nucleus_utils.get_assets_root_path() + if assets_root_path is None: + raise RuntimeError( + "Unable to access the Nucleus server from Omniverse. For more information, please check: " + "https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus" + ) + # ground plane + kit_utils.create_ground_plane( + "/World/defaultGroundPlane", static_friction=0.5, dynamic_friction=0.5, restitution=0.8 + ) + # robot + robot_usd_path = assets_root_path + "/Isaac/Robots/Ant/ant_instanceable.usd" + prim_utils.create_prim( + prim_path=self.template_env_ns + "/Ant", usd_path=robot_usd_path, translation=(0.0, 0.0, 0.5) + ) + # apply articulation settings + kit_utils.set_articulation_properties( + prim_path=self.template_env_ns + "/Ant/torso", + solver_position_iteration_count=self.cfg_dict["scene"]["ant"]["solver_position_iteration_count"], + solver_velocity_iteration_count=self.cfg_dict["scene"]["ant"]["solver_velocity_iteration_count"], + sleep_threshold=self.cfg_dict["scene"]["ant"]["sleep_threshold"], + stabilization_threshold=self.cfg_dict["scene"]["ant"]["stabilization_threshold"], + enable_self_collisions=self.cfg_dict["scene"]["ant"]["enable_self_collisions"], + ) + # apply rigid body settings + kit_utils.set_nested_rigid_body_properties( + prim_path=self.template_env_ns + "/Ant", + enable_gyroscopic_forces=self.cfg_dict["scene"]["ant"]["enable_gyroscopic_forces"], + max_depenetration_velocity=self.cfg_dict["scene"]["ant"]["max_depenetration_velocity"], + ) + # apply collider properties + kit_utils.set_nested_collision_properties( + prim_path=self.template_env_ns + "/Ant", + contact_offset=self.cfg_dict["scene"]["ant"]["contact_offset"], + rest_offset=self.cfg_dict["scene"]["ant"]["rest_offset"], + ) + # return global prims + return ["/World/defaultGroundPlane"] + + def _reset_idx(self, env_ids: VecEnvIndices): + # get num envs to reset + num_resets = len(env_ids) + # randomize the MDP + # -- DOF position + dof_pos = torch_utils.torch_rand_float(-0.2, 0.2, (num_resets, self.ants.num_dof), device=self.device) + dof_pos[:] = torch_utils.tensor_clamp( + self._initial_dof_pos[env_ids] + dof_pos, self._dof_limits[:, 0], self._dof_limits[:, 1] + ) + self.ants.set_joint_positions(dof_pos, indices=env_ids) + # -- DOF velocity + dof_vel = torch_utils.torch_rand_float(-0.1, 0.1, (num_resets, self.ants.num_dof), device=self.device) + self.ants.set_joint_velocities(dof_vel, indices=env_ids) + # -- Root pose + root_pos, root_rot = self._initial_root_tf[0].clone()[env_ids], self._initial_root_tf[1].clone()[env_ids] + self.ants.set_world_poses(root_pos, root_rot, indices=env_ids) + # -- Root velocity + root_vel = torch.zeros((num_resets, 6), device=self.device) + self.ants.set_velocities(root_vel, indices=env_ids) + # -- Reset potentials + to_target = self._GOAL_POS[env_ids] - root_pos + to_target[:, 2] = 0.0 + self.prev_potentials[env_ids] = -torch.norm(to_target, p=2, dim=-1) / self.physics_dt + self.potentials[env_ids] = self.prev_potentials[env_ids].clone() + # -- MDP reset + self.actions[env_ids, :] = 0 + self.reset_buf[env_ids] = 0 + self.episode_length_buf[env_ids] = 0 + + def _step_impl(self, actions: torch.Tensor): + # store actions into local + self.actions = actions.clone().to(device=self.device) + # pre-step: set actions into buffer + dof_forces = self.actions * self._JOINT_GEARS * self.cfg_dict["env"]["power_scale"] + indices = torch.arange(self.num_envs, dtype=torch.int32, device=self.device) + self.ants.set_joint_efforts(dof_forces, indices=indices) + # perform physics stepping + for _ in range(self.cfg_dict["env"]["control_frequency_inv"]): + # step simulation + self.sim.step(render=self.enable_render) + # check if simulation is still running + if self.sim.is_stopped(): + return + # post-step: compute MDP + self._cache_common_quantities() + self._compute_rewards() + self._check_termination() + # add information to extra if timeout occurred due to episode length + # Note: this is used by algorithms like PPO where time-outs are handled differently + # For more info: https://github.com/DLR-RM/stable-baselines3/issues/633 + self.extras["time_outs"] = self.episode_length_buf >= self.cfg_dict["env"]["episode_length"] + + def _get_observations(self) -> VecEnvObs: + # extract euler angles (in start frame) + roll, _, yaw = torch_utils.get_euler_xyz(self._torso_quat_start) + # compute heading direction + # TODO: Check why is this using z direction instead of y. + walk_target_angle = torch.atan2( + self._GOAL_POS[:, 2] - self._torso_pos_start[:, 2], self._GOAL_POS[:, 0] - self._torso_pos_start[:, 0] + ) + angle_to_target = walk_target_angle - yaw + + # obs_buf shapes: 1, 3, 3, 1, 1, 1, 1, 1, num_dofs(8), num_dofs(8), num_dofs(8) + obs_buf = torch.cat( + ( + self._torso_pos_start[:, 2].view(-1, 1), + self._lin_vel_start, + self._ang_vel_start * self.cfg_dict["env"]["angular_velocity_scale"], + yaw.unsqueeze(-1), + roll.unsqueeze(-1), + angle_to_target.unsqueeze(-1), + self._up_proj.unsqueeze(-1), + self._heading_proj.unsqueeze(-1), + self._dof_pos_scaled, + self._dof_vel_scaled, + self._feet_force_torques * self.cfg_dict["env"]["contact_force_scale"], + self.actions, + ), + dim=-1, + ) + + return {"policy": obs_buf} + + """ + Helper functions - MDP. + """ + + def _cache_common_quantities(self) -> None: + """Compute common quantities from simulator used for computing MDP signals.""" + # extract data from simulator + torso_pos_world, torso_quat_world = self.ants.get_world_poses(clone=False) + lin_vel_world = self.ants.get_linear_velocities(clone=False) + ang_vel_world = self.ants.get_angular_velocities(clone=False) + dof_pos = self.ants.get_joint_positions(clone=False) + dof_vel = self.ants.get_joint_velocities(clone=False) + # TODO: Remove direct usage of `_physics_view` when method exists in :class:`ArticulationView` + feet_force_torques = self.ants._physics_view.get_force_sensor_forces() + + # scale the dof + self._dof_pos_scaled = torch_utils.scale_transform(dof_pos, self._dof_limits[:, 0], self._dof_limits[:, 1]) + self._dof_vel_scaled = dof_vel * self.cfg_dict["env"]["dof_velocity_scale"] + # feet contact forces + self._feet_force_torques = feet_force_torques.reshape(self.num_envs, -1) + + # convert base pose w.r.t. start frame + self._torso_pos_start = torso_pos_world + self._torso_quat_start = torch_utils.quat_mul(torso_quat_world, self._INV_START_QUAT) + # convert velocity (in base frame w.r.t. start frame) + self._lin_vel_start = torch_utils.quat_rotate_inverse(self._torso_quat_start, lin_vel_world) + self._ang_vel_start = torch_utils.quat_rotate_inverse(self._torso_quat_start, ang_vel_world) + # convert basis vectors w.r.t. start frame + up_vec = torch_utils.quat_rotate(self._torso_quat_start, self._UP_VEC) + heading_vec = torch_utils.quat_rotate(self._torso_quat_start, self._HEADING_VEC) + + # compute relative movement to the target + self._to_target = self._GOAL_POS - self._torso_pos_start + self._to_target[:, 2] = 0.0 + to_target_dir = torch_utils.normalize(self._to_target) + # compute projection of current heading to desired heading vector + self._up_proj = up_vec[:, 2] + self._heading_proj = torch.bmm(heading_vec.view(self.num_envs, 1, 3), to_target_dir.view(self.num_envs, 3, 1)) + self._heading_proj = self._heading_proj.view(self.num_envs) + + def _compute_rewards(self) -> None: + # heading in the right direction + heading_reward = torch.where( + self._heading_proj > 0.8, + self.cfg_dict["env"]["heading_weight"], + self.cfg_dict["env"]["heading_weight"] * self._heading_proj.double() / 0.8, + ) + # aligning up axis of robot and environment + up_reward = torch.where(self._up_proj > 0.93, self.cfg_dict["env"]["up_weight"], 0.0) + + # penalty for high action commands + actions_cost = torch.sum(self.actions**2, dim=-1) + # energy penalty for movement (power = torque * vel) + electricity_cost = torch.sum(torch.abs(self.actions * self._dof_vel_scaled), dim=-1) + # reaching close to dof limits + # TODO: Shouldn't this be absolute dof pos? Only checks for upper limit right now. + dof_at_limit_cost = torch.sum(self._dof_pos_scaled > 0.99, dim=-1) + # reward for duration of staying alive + alive_reward = self.cfg_dict["env"]["alive_weight"] + + # compute x,y-potential score towards the goal + self.prev_potentials = self.potentials.clone() + self.potentials = -torch.norm(self._to_target, p=2, dim=-1) / self.physics_dt + # reward for progressing towards the goal (through L2 potential) + progress_reward = self.potentials - self.prev_potentials + + # compute reward + total_reward = ( + progress_reward + + alive_reward + + up_reward + + heading_reward + - self.cfg_dict["env"]["actions_cost"] * actions_cost + - self.cfg_dict["env"]["energy_cost"] * electricity_cost + - self.cfg_dict["env"]["joints_at_limit_cost"] * dof_at_limit_cost + ) + # adjust reward for fallen agents + total_reward = torch.where( + self._torso_pos_start[:, 2] < self.cfg_dict["env"]["termination_height"], + self.cfg_dict["env"]["death_cost"], + total_reward, + ) + # set reward into buffer + self.reward_buf[:] = total_reward + + def _check_termination(self) -> None: + # compute resets + # -- base has collapsed + resets = torch.where( + self._torso_pos_start[:, 2] < self.cfg_dict["env"]["termination_height"], 1, self.reset_buf + ) + # -- episode length + resets = torch.where(self.episode_length_buf >= self.max_episode_length, 1, resets) + # set reset into buffer + self.reset_buf[:] = resets + + def _define_environment_constants(self): + """Defines useful constants used by the implementation.""" + # desired goal position + self._GOAL_POS = torch.tensor([1000, 0, 0], dtype=torch.float32, device=self.device).repeat((self.num_envs, 1)) + # gear ratio for joint control + self._JOINT_GEARS = torch.tensor([15] * self.ants.num_dof, dtype=torch.float32, device=self.device) + # initial spawn orientation + self._START_QUAT = torch.tensor([1, 0, 0, 0], device=self.device, dtype=torch.float32) + self._INV_START_QUAT = torch_utils.quat_conjugate(self._START_QUAT).repeat((self.num_envs, 1)) + # heading direction for the robot + self._HEADING_VEC = torch.tensor([1, 0, 0], dtype=torch.float32, device=self.device).repeat((self.num_envs, 1)) + # up direction for the simulator + self._UP_VEC = torch.tensor([0, 0, 1], dtype=torch.float32, device=self.device).repeat((self.num_envs, 1)) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/__init__.py new file mode 100644 index 0000000000..671505bb15 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Cartpole balancing environment. +""" + +from .cartpole_env import CartpoleEnv + +__all__ = ["CartpoleEnv"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/cartpole_cfg.yaml b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/cartpole_cfg.yaml new file mode 100644 index 0000000000..8d31111eb4 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/cartpole_cfg.yaml @@ -0,0 +1,63 @@ +# parameters for the MDP +env: + # general + num_envs: 512 + env_spacing: 4.0 + episode_length: 500 # sim steps + + # step parameters + control_frequency_inv: 2 # 60 Hz + + # reset parameters + reset_dist: 3.0 + # actions parameters + max_effort: 400.0 + +# parameters for setting up the scene +scene: + + cartpole: + # articulation + solver_position_iteration_count: 4 + solver_velocity_iteration_count: 1 + sleep_threshold: 0.005 + stabilization_threshold: 0.001 + enable_self_collisions: False + # per-body + enable_gyroscopic_forces: True + max_depenetration_velocity: 100.0 + # per-shape + contact_offset: 0.02 + rest_offset: 0.001 + +# parameters for physics engine +sim: + dt: 0.0083 # 1/120 s + substeps: 1 + gravity: [0.0, 0.0, -9.81] + + enable_scene_query_support: False + use_gpu_pipeline: True + use_flatcache: True + device: "cuda:0" + + physx: + # Solver settings + solver_type: 1 + use_gpu: True # set to False to run on CPU + bounce_threshold_velocity: 0.2 + friction_offset_threshold: 0.04 + friction_correlation_distance: 0.025 + enable_stabilization: True + + # GPU buffers + gpu_max_rigid_contact_count: 524288 + gpu_max_rigid_patch_count: 81920 + gpu_found_lost_pairs_capacity: 1024 + gpu_found_lost_aggregate_pairs_capacity: 262144 + gpu_total_aggregate_pairs_capacity: 1024 + gpu_max_soft_body_contacts: 1048576 + gpu_max_particle_contacts: 1048576 + gpu_heap_capacity: 67108864 + gpu_temp_buffer_capacity: 16777216 + gpu_max_num_partitions: 8 diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/cartpole_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/cartpole_env.py new file mode 100644 index 0000000000..f3ae42b016 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/cartpole/cartpole_env.py @@ -0,0 +1,197 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gym.spaces +import math +import torch +from typing import List + +import omni.isaac.core.utils.nucleus as nucleus_utils +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.articulations import ArticulationView + +import omni.isaac.orbit.utils.kit as kit_utils + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvIndices, VecEnvObs +from omni.isaac.orbit_envs.isaac_env_cfg import EnvCfg, IsaacEnvCfg + + +class CartpoleEnv(IsaacEnv): + """Environment for 2-D cartpole. + + Reference: + https://github.com/openai/gym/blob/master/gym/envs/classic_control/cartpole.py + """ + + def __init__(self, cfg: dict, headless: bool = False): + """Initializes the environment. + + Args: + cfg (dict): The configuration dictionary. + headless (bool, optional): Whether to enable rendering or not. Defaults to False. + """ + # copy configuration + self.cfg_dict = cfg.copy() + # configuration for the environment + isaac_cfg = IsaacEnvCfg( + env=EnvCfg(num_envs=self.cfg_dict["env"]["num_envs"], env_spacing=self.cfg_dict["env"]["env_spacing"]) + ) + isaac_cfg.sim.from_dict(self.cfg_dict["sim"]) + # initialize the base class to setup the scene. + super().__init__(isaac_cfg, headless=headless) + + # define views over instances + self.cartpoles = ArticulationView(prim_paths_expr=self.env_ns + "/.*/Cartpole", reset_xform_properties=False) + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + self.sim.reset() + # initialize all the handles + self.cartpoles.initialize(self.sim.physics_sim_view) + # set the default state + self.cartpoles.post_reset() + + # get quantities from scene we care about + self._cart_dof_idx = self.cartpoles.get_dof_index("cartJoint") + self._pole_dof_idx = self.cartpoles.get_dof_index("poleJoint") + + # compute the observation space + self.observation_space = gym.spaces.Box(low=-math.inf, high=math.inf, shape=(4,)) + # compute the action space + self.action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(1,)) + # store maximum episode length + self.max_episode_length = self.cfg_dict["env"]["episode_length"] + + """ + Implementation specifics. + """ + + def _design_scene(self) -> List[str]: + # get nucleus assets path + assets_root_path = nucleus_utils.get_assets_root_path() + if assets_root_path is None: + raise RuntimeError( + "Unable to access the Nucleus server from Omniverse. For more information, please check: " + "https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus" + ) + # ground plane + kit_utils.create_ground_plane("/World/defaultGroundPlane") + # robot + robot_usd_path = assets_root_path + "/Isaac/Robots/Cartpole/cartpole.usd" + prim_utils.create_prim( + prim_path=self.template_env_ns + "/Cartpole", usd_path=robot_usd_path, translation=(0.0, 0.0, 2.0) + ) + # apply articulation settings + kit_utils.set_articulation_properties( + prim_path=self.template_env_ns + "/Cartpole", + solver_position_iteration_count=self.cfg_dict["scene"]["cartpole"]["solver_position_iteration_count"], + solver_velocity_iteration_count=self.cfg_dict["scene"]["cartpole"]["solver_velocity_iteration_count"], + sleep_threshold=self.cfg_dict["scene"]["cartpole"]["sleep_threshold"], + stabilization_threshold=self.cfg_dict["scene"]["cartpole"]["stabilization_threshold"], + enable_self_collisions=self.cfg_dict["scene"]["cartpole"]["enable_self_collisions"], + ) + # apply rigid body settings + kit_utils.set_nested_rigid_body_properties( + prim_path=self.template_env_ns + "/Cartpole", + enable_gyroscopic_forces=self.cfg_dict["scene"]["cartpole"]["enable_gyroscopic_forces"], + max_depenetration_velocity=self.cfg_dict["scene"]["cartpole"]["max_depenetration_velocity"], + ) + # apply collider properties + kit_utils.set_nested_collision_properties( + prim_path=self.template_env_ns + "/Cartpole", + contact_offset=self.cfg_dict["scene"]["cartpole"]["contact_offset"], + rest_offset=self.cfg_dict["scene"]["cartpole"]["rest_offset"], + ) + # return global prims + return ["/World/defaultGroundPlane"] + + def _reset_idx(self, env_ids: VecEnvIndices): + # get num envs to reset + num_resets = len(env_ids) + # randomize the MDP + # -- DOF position + dof_pos = torch.zeros((num_resets, self.cartpoles.num_dof), device=self.device) + dof_pos[:, self._cart_dof_idx] = 1.0 * (1.0 - 2.0 * torch.rand(num_resets, device=self.device)) + dof_pos[:, self._pole_dof_idx] = 0.125 * math.pi * (1.0 - 2.0 * torch.rand(num_resets, device=self.device)) + self.cartpoles.set_joint_positions(dof_pos, indices=env_ids) + # -- DOF velocity + dof_vel = torch.zeros((num_resets, self.cartpoles.num_dof), device=self.device) + dof_vel[:, self._cart_dof_idx] = 0.5 * (1.0 - 2.0 * torch.rand(num_resets, device=self.device)) + dof_vel[:, self._pole_dof_idx] = 0.25 * math.pi * (1.0 - 2.0 * torch.rand(num_resets, device=self.device)) + self.cartpoles.set_joint_velocities(dof_vel, indices=env_ids) + # -- MDP reset + self.reset_buf[env_ids] = 0 + self.episode_length_buf[env_ids] = 0 + + def _step_impl(self, actions: torch.Tensor): + # pre-step: set actions into buffer + self.actions = actions.clone().to(device=self.device) + dof_forces = torch.zeros( + (self.cartpoles.count, self.cartpoles.num_dof), dtype=torch.float32, device=self.device + ) + dof_forces[:, self._cart_dof_idx] = self.cfg_dict["env"]["max_effort"] * self.actions[:, 0] + indices = torch.arange(self.cartpoles.count, dtype=torch.int32, device=self.device) + self.cartpoles.set_joint_efforts(dof_forces, indices=indices) + # perform physics stepping + for _ in range(self.cfg_dict["env"]["control_frequency_inv"]): + # step simulation + self.sim.step(render=self.enable_render) + # check that simulation is playing + if self.sim.is_stopped(): + return + # post-step: compute MDP + self._compute_rewards() + self._check_termination() + # add information to extra if timeout occurred due to episode length + # Note: this is used by algorithms like PPO where time-outs are handled differently + # For more info: https://github.com/DLR-RM/stable-baselines3/issues/633 + self.extras["time_outs"] = self.episode_length_buf >= self.cfg_dict["env"]["episode_length"] + + def _get_observations(self) -> VecEnvObs: + # access buffers from simulator + dof_pos = self.cartpoles.get_joint_positions(clone=False) + dof_vel = self.cartpoles.get_joint_velocities(clone=False) + # concatenate and return + obs_buf = torch.cat([dof_pos, dof_vel], dim=-1) + + return {"policy": obs_buf} + + """ + Helper functions. + """ + + def _compute_rewards(self) -> None: + # access buffers from simulator + dof_pos = self.cartpoles.get_joint_positions(clone=False) + dof_vel = self.cartpoles.get_joint_velocities(clone=False) + # extract values from buffer + cart_pos = dof_pos[:, self._cart_dof_idx] + pole_pos = dof_pos[:, self._pole_dof_idx] + cart_vel = dof_vel[:, self._cart_dof_idx] + pole_vel = dof_vel[:, self._pole_dof_idx] + # compute reward + reward = 1.0 - pole_pos * pole_pos - 0.01 * torch.abs(cart_vel) - 0.005 * torch.abs(pole_vel) + reward = torch.where( + torch.abs(cart_pos) > self.cfg_dict["env"]["reset_dist"], torch.ones_like(reward) * -2.0, reward + ) + reward = torch.where(torch.abs(pole_pos) > math.pi / 2, torch.ones_like(reward) * -2.0, reward) + # set reward into buffer + self.reward_buf[:] = reward + + def _check_termination(self) -> None: + # access buffers from simulator + dof_pos = self.cartpoles.get_joint_positions(clone=False) + # extract values from buffer + cart_pos = dof_pos[:, self._cart_dof_idx] + pole_pos = dof_pos[:, self._pole_dof_idx] + # compute resets + # -- cart moved towards the edges + resets = torch.where(torch.abs(cart_pos) > self.cfg_dict["env"]["reset_dist"], 1, 0) + # -- pole fell down + resets = torch.where(torch.abs(pole_pos) > math.pi / 2, 1, resets) + # -- episode length + resets = torch.where(self.episode_length_buf >= self.max_episode_length, 1, resets) + # set resets into buffer + self.reset_buf[:] = resets diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/__init__.py new file mode 100644 index 0000000000..ab31c303ab --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Humanoid locomotion environment (similar to OpenAI Gym Humanoid-v2). +""" + +from .humanoid_env import HumanoidEnv + +__all__ = ["HumanoidEnv"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/humanoid_cfg.yaml b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/humanoid_cfg.yaml new file mode 100644 index 0000000000..f9e1d9d6de --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/humanoid_cfg.yaml @@ -0,0 +1,75 @@ +# parameters for the MDP +env: + # general + num_envs: 1024 + env_spacing: 5 + episode_length: 1000 + + # step parameters + control_frequency_inv: 2 # 60 Hz + # actions parameters + power_scale: 1.0 + # observations parameters + angular_velocity_scale: 0.25 + dof_velocity_scale: 0.1 + contact_force_scale: 0.01 + + # reward parameters + heading_weight: 0.5 + up_weight: 0.1 + alive_weight: 2.0 + # cost parameters + actions_cost: 0.01 + energy_cost: 0.05 + joints_at_limit_cost: 0.25 + # termination parameters + death_cost: -1.0 + termination_height: 0.8 + +# parameters for setting up the scene +scene: + + humanoid: + # articulation + solver_position_iteration_count: 4 + solver_velocity_iteration_count: 0 + sleep_threshold: 0.005 + stabilization_threshold: 0.001 + enable_self_collisions: True + # per-body + enable_gyroscopic_forces: True + max_depenetration_velocity: 10.0 + # per-shape + contact_offset: 0.02 + rest_offset: 0.0 + +# parameters for physics engine +sim: + dt: 0.0083 # 1/120 s + substeps: 1 + gravity: [0.0, 0.0, -9.81] + + enable_scene_query_support: False + use_gpu_pipeline: True + use_flatcache: True + device: "cuda:0" + + physx: + solver_type: 1 + use_gpu: True # set to False to run on CPU + bounce_threshold_velocity: 0.2 + friction_offset_threshold: 0.04 + friction_correlation_distance: 0.025 + enable_stabilization: True + + # GPU buffers + gpu_max_rigid_contact_count: 524288 + gpu_max_rigid_patch_count: 81920 + gpu_found_lost_pairs_capacity: 1024 + gpu_found_lost_aggregate_pairs_capacity: 262144 + gpu_total_aggregate_pairs_capacity: 2048 + gpu_max_soft_body_contacts: 1048576 + gpu_max_particle_contacts: 1048576 + gpu_heap_capacity: 67108864 + gpu_temp_buffer_capacity: 16777216 + gpu_max_num_partitions: 8 diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/humanoid_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/humanoid_env.py new file mode 100644 index 0000000000..91299252cc --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/classic/humanoid/humanoid_env.py @@ -0,0 +1,348 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gym.spaces +import math +import torch +from typing import List + +import omni.isaac.core.utils.nucleus as nucleus_utils +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.torch as torch_utils +from omni.isaac.core.articulations import ArticulationView + +import omni.isaac.orbit.utils.kit as kit_utils + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvIndices, VecEnvObs +from omni.isaac.orbit_envs.isaac_env_cfg import EnvCfg, IsaacEnvCfg + + +class HumanoidEnv(IsaacEnv): + """Environment for an Humanoid on a flat terrain. + + Reference: + https://github.com/openai/gym/blob/master/gym/envs/mujoco/humanoid_v3.py + """ + + def __init__(self, cfg: dict, headless: bool = False): + """Initializes the environment. + + Args: + cfg (dict): The configuration dictionary. + headless (bool, optional): Whether to enable rendering or not. Defaults to False. + """ + # copy configuration + self.cfg_dict = cfg.copy() + # configuration for the environment + isaac_cfg = IsaacEnvCfg( + env=EnvCfg(num_envs=self.cfg_dict["env"]["num_envs"], env_spacing=self.cfg_dict["env"]["env_spacing"]) + ) + isaac_cfg.sim.from_dict(self.cfg_dict["sim"]) + # initialize the base class to setup the scene. + super().__init__(isaac_cfg, headless=headless) + + # define views over instances + self.humanoids = ArticulationView( + prim_paths_expr=self.env_ns + "/.*/Humanoid/torso", reset_xform_properties=False + ) + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + self.sim.reset() + # initialize all the handles + self.humanoids.initialize(self.sim.physics_sim_view) + # set the default state + self.humanoids.post_reset() + + # get quantities from scene we care about + self._dof_limits = self.humanoids.get_dof_limits()[0, :].to(self.device) + self._initial_root_tf = self.humanoids.get_world_poses(clone=True) + self._initial_dof_pos = self.humanoids.get_joint_positions(clone=True) + + # initialize buffers + self.actions = torch.zeros((self.num_envs, 21), device=self.device) + # create constants required later during simulation. + self._define_environment_constants() + # create other useful variables + self.potentials = torch.full( + (self.num_envs,), -1000.0 / self.physics_dt, dtype=torch.float32, device=self.device + ) + self.prev_potentials = self.potentials.clone() + + # compute the observation space + self.observation_space = gym.spaces.Box(low=-math.inf, high=math.inf, shape=(87,)) + # compute the action space + self.action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(21,)) + # store maximum episode length + self.max_episode_length = self.cfg_dict["env"]["episode_length"] + + """ + Implementation specifics. + """ + + def _design_scene(self) -> List[str]: + # get nucleus assets path + assets_root_path = nucleus_utils.get_assets_root_path() + if assets_root_path is None: + raise RuntimeError( + "Unable to access the Nucleus server from Omniverse. For more information, please check: " + "https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus" + ) + # ground plane + kit_utils.create_ground_plane( + "/World/defaultGroundPlane", static_friction=0.5, dynamic_friction=0.5, restitution=0.8 + ) + # robot + robot_usd_path = assets_root_path + "/Isaac/Robots/Humanoid/humanoid_instanceable.usd" + prim_utils.create_prim( + prim_path=self.template_env_ns + "/Humanoid", usd_path=robot_usd_path, translation=(0.0, 0.0, 1.34) + ) + # apply articulation settings + kit_utils.set_articulation_properties( + prim_path=self.template_env_ns + "/Humanoid/torso", + solver_position_iteration_count=self.cfg_dict["scene"]["humanoid"]["solver_position_iteration_count"], + solver_velocity_iteration_count=self.cfg_dict["scene"]["humanoid"]["solver_velocity_iteration_count"], + sleep_threshold=self.cfg_dict["scene"]["humanoid"]["sleep_threshold"], + stabilization_threshold=self.cfg_dict["scene"]["humanoid"]["stabilization_threshold"], + enable_self_collisions=self.cfg_dict["scene"]["humanoid"]["enable_self_collisions"], + ) + # apply rigid body settings + kit_utils.set_nested_rigid_body_properties( + prim_path=self.template_env_ns + "/Humanoid", + enable_gyroscopic_forces=self.cfg_dict["scene"]["humanoid"]["enable_gyroscopic_forces"], + max_depenetration_velocity=self.cfg_dict["scene"]["humanoid"]["max_depenetration_velocity"], + ) + # apply collider properties + kit_utils.set_nested_collision_properties( + prim_path=self.template_env_ns + "/Humanoid", + contact_offset=self.cfg_dict["scene"]["humanoid"]["contact_offset"], + rest_offset=self.cfg_dict["scene"]["humanoid"]["rest_offset"], + ) + # return global prims + return ["/World/defaultGroundPlane"] + + def _reset_idx(self, env_ids: VecEnvIndices): + # get num envs to reset + num_resets = len(env_ids) + # randomize the MDP + # -- DOF position + dof_pos = torch_utils.torch_rand_float(-0.2, 0.2, (num_resets, self.humanoids.num_dof), device=self.device) + dof_pos[:] = torch_utils.tensor_clamp( + self._initial_dof_pos[env_ids] + dof_pos, self._dof_limits[:, 0], self._dof_limits[:, 1] + ) + self.humanoids.set_joint_positions(dof_pos, indices=env_ids) + # -- DOF velocity + dof_vel = torch_utils.torch_rand_float(-0.1, 0.1, (num_resets, self.humanoids.num_dof), device=self.device) + self.humanoids.set_joint_velocities(dof_vel, indices=env_ids) + # -- Root pose + root_pos, root_rot = self._initial_root_tf[0].clone()[env_ids], self._initial_root_tf[1].clone()[env_ids] + self.humanoids.set_world_poses(root_pos, root_rot, indices=env_ids) + # -- Root velocity + root_vel = torch.zeros((num_resets, 6), device=self.device) + self.humanoids.set_velocities(root_vel, indices=env_ids) + # -- Reset potentials + to_target = self._GOAL_POS[env_ids] - root_pos + to_target[:, 2] = 0.0 + self.prev_potentials[env_ids] = -torch.norm(to_target, p=2, dim=-1) / self.physics_dt + self.potentials[env_ids] = self.prev_potentials[env_ids].clone() + # -- MDP reset + self.actions[env_ids, :] = 0 + self.reset_buf[env_ids] = 0 + self.episode_length_buf[env_ids] = 0 + + def _step_impl(self, actions: torch.Tensor): + # pre-step: set actions into buffer + self.actions = actions.clone().to(device=self.device) + dof_forces = self.actions * self._JOINT_GEARS * self.cfg_dict["env"]["power_scale"] + indices = torch.arange(self.num_envs, dtype=torch.int32, device=self.device) + self.humanoids.set_joint_efforts(dof_forces, indices=indices) + # perform physics stepping + for _ in range(self.cfg_dict["env"]["control_frequency_inv"]): + # step simulation + self.sim.step(render=self.enable_render) + # check if simulation is still running + if self.sim.is_stopped(): + return + # post-step: compute MDP + self._cache_common_quantities() + self._compute_rewards() + self._check_termination() + # add information to extra if timeout occurred due to episode length + # Note: this is used by algorithms like PPO where time-outs are handled differently + # For more info: https://github.com/DLR-RM/stable-baselines3/issues/633 + self.extras["time_outs"] = self.episode_length_buf >= self.cfg_dict["env"]["episode_length"] + + def _get_observations(self) -> VecEnvObs: + # extract euler angles (in start frame) + roll, _, yaw = torch_utils.get_euler_xyz(self._torso_quat_start) + # compute heading direction + # TODO: Check why is this using z direction instead of y. + walk_target_angle = torch.atan2( + self._GOAL_POS[:, 2] - self._torso_pos_start[:, 2], self._GOAL_POS[:, 0] - self._torso_pos_start[:, 0] + ) + angle_to_target = walk_target_angle - yaw + + # obs_buf shapes: 1, 3, 3, 1, 1, 1, 1, 1, num_dofs(21), num_dofs(21), num_dofs(21) + obs_buf = torch.cat( + ( + self._torso_pos_start[:, 2].view(-1, 1), + self._lin_vel_start, + self._ang_vel_start * self.cfg_dict["env"]["angular_velocity_scale"], + yaw.unsqueeze(-1), + roll.unsqueeze(-1), + angle_to_target.unsqueeze(-1), + self._up_proj.unsqueeze(-1), + self._heading_proj.unsqueeze(-1), + self._dof_pos_scaled, + self._dof_vel_scaled, + self._feet_force_torques * self.cfg_dict["env"]["contact_force_scale"], + self.actions, + ), + dim=-1, + ) + + return {"policy": obs_buf} + + """ + Helper functions - MDP. + """ + + def _cache_common_quantities(self) -> None: + """Compute common quantities from simulator used for computing MDP signals.""" + # extract data from simulator + torso_pos_world, torso_quat_world = self.humanoids.get_world_poses(clone=False) + lin_vel_world = self.humanoids.get_linear_velocities(clone=False) + ang_vel_world = self.humanoids.get_angular_velocities(clone=False) + dof_pos = self.humanoids.get_joint_positions(clone=False) + dof_vel = self.humanoids.get_joint_velocities(clone=False) + # TODO: Remove direct usage of `_physics_view` when method exists in :class:`ArticulationView` + feet_force_torques = self.humanoids._physics_view.get_force_sensor_forces() + + # scale the dof + self._dof_pos_scaled = torch_utils.scale_transform(dof_pos, self._dof_limits[:, 0], self._dof_limits[:, 1]) + self._dof_vel_scaled = dof_vel * self.cfg_dict["env"]["dof_velocity_scale"] + # feet contact forces + self._feet_force_torques = feet_force_torques.reshape(self.num_envs, -1) + + # convert base pose w.r.t. start frame + self._torso_pos_start = torso_pos_world + self._torso_quat_start = torch_utils.quat_mul(torso_quat_world, self._INV_START_QUAT) + # convert velocity (in base frame w.r.t. start frame) + self._lin_vel_start = torch_utils.quat_rotate_inverse(self._torso_quat_start, lin_vel_world) + self._ang_vel_start = torch_utils.quat_rotate_inverse(self._torso_quat_start, ang_vel_world) + # convert basis vectors w.r.t. start frame + up_vec = torch_utils.quat_rotate(self._torso_quat_start, self._UP_VEC) + heading_vec = torch_utils.quat_rotate(self._torso_quat_start, self._HEADING_VEC) + + # compute relative movement to the target + self._to_target = self._GOAL_POS - self._torso_pos_start + self._to_target[:, 2] = 0.0 + to_target_dir = torch_utils.normalize(self._to_target) + # compute projection of current heading to desired heading vector + self._up_proj = up_vec[:, 2] + self._heading_proj = torch.bmm(heading_vec.view(self.num_envs, 1, 3), to_target_dir.view(self.num_envs, 3, 1)) + self._heading_proj = self._heading_proj.view(self.num_envs) + + def _compute_rewards(self) -> None: + # heading in the right direction + heading_reward = torch.where( + self._heading_proj > 0.8, + self.cfg_dict["env"]["heading_weight"], + self.cfg_dict["env"]["heading_weight"] * self._heading_proj.double() / 0.8, + ) + # aligning up axis of robot and environment + up_reward = torch.where(self._up_proj > 0.93, self.cfg_dict["env"]["up_weight"], 0.0) + + # penalty for high action commands + actions_cost = torch.sum(self.actions**2, dim=-1) + # energy penalty for movement (power = torque * vel) + electricity_cost = torch.sum(torch.abs(self.actions * self._dof_vel_scaled), dim=-1) + # reaching close to dof limits + # TODO: Shouldn't this be absolute dof pos? Only checks for upper limit right now. + motor_effort_ratio = self._JOINT_GEARS / self._MAX_MOTOR_EFFORT + scaled_cost = (torch.abs(self._dof_pos_scaled) - 0.98) / 0.02 + dof_at_limit_cost = torch.sum( + (torch.abs(self._dof_pos_scaled) > 0.98) * scaled_cost * motor_effort_ratio.unsqueeze(0), dim=-1 + ) + # reward for duration of staying alive + alive_reward = self.cfg_dict["env"]["alive_weight"] + + # compute x,y-potential score towards the goal + self.prev_potentials = self.potentials.clone() + self.potentials = -torch.norm(self._to_target, p=2, dim=-1) / self.physics_dt + # reward for progressing towards the goal (through L2 potential) + progress_reward = self.potentials - self.prev_potentials + + # compute reward + total_reward = ( + progress_reward + + alive_reward + + up_reward + + heading_reward + - self.cfg_dict["env"]["actions_cost"] * actions_cost + - self.cfg_dict["env"]["energy_cost"] * electricity_cost + - self.cfg_dict["env"]["joints_at_limit_cost"] * dof_at_limit_cost + ) + # adjust reward for fallen agents + total_reward = torch.where( + self._torso_pos_start[:, 2] < self.cfg_dict["env"]["termination_height"], + self.cfg_dict["env"]["death_cost"], + total_reward, + ) + # set reward into buffer + self.reward_buf[:] = total_reward + + def _check_termination(self) -> None: + # compute resets + # -- base has collapsed + resets = torch.where( + self._torso_pos_start[:, 2] < self.cfg_dict["env"]["termination_height"], 1, self.reset_buf + ) + # -- episode length + resets = torch.where(self.episode_length_buf >= self.max_episode_length, 1, resets) + # set reset into buffer + self.reset_buf[:] = resets + + def _define_environment_constants(self): + """Defines useful constants used by the implementation.""" + # desired goal position + self._GOAL_POS = torch.tensor([1000, 0, 0], dtype=torch.float32, device=self.device).repeat((self.num_envs, 1)) + # gear ratio for joint control + self._JOINT_GEARS = torch.tensor( + [ + 67.5000, # lower_waist + 67.5000, # lower_waist + 67.5000, # right_upper_arm + 67.5000, # right_upper_arm + 67.5000, # left_upper_arm + 67.5000, # left_upper_arm + 67.5000, # pelvis + 45.0000, # right_lower_arm + 45.0000, # left_lower_arm + 45.0000, # right_thigh: x + 135.0000, # right_thigh: y + 45.0000, # right_thigh: z + 45.0000, # left_thigh: x + 135.0000, # left_thigh: y + 45.0000, # left_thigh: z + 90.0000, # right_knee + 90.0000, # left_knee + 22.5, # right_foot + 22.5, # right_foot + 22.5, # left_foot + 22.5, # left_foot + ], + dtype=torch.float32, + device=self.device, + ) + # the maximum motor effort applicable + self._MAX_MOTOR_EFFORT = torch.max(self._JOINT_GEARS) + # initial spawn orientation + self._START_QUAT = torch.tensor([1, 0, 0, 0], device=self.device, dtype=torch.float32) + self._INV_START_QUAT = torch_utils.quat_conjugate(self._START_QUAT).repeat((self.num_envs, 1)) + # heading direction for the robot + self._HEADING_VEC = torch.tensor([1, 0, 0], dtype=torch.float32, device=self.device).repeat((self.num_envs, 1)) + # up direction for the simulator + self._UP_VEC = torch.tensor([0, 0, 1], dtype=torch.float32, device=self.device).repeat((self.num_envs, 1)) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/isaac_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/isaac_env.py new file mode 100644 index 0000000000..d0e07ebfcc --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/isaac_env.py @@ -0,0 +1,407 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The superclass for Isaac Sim based environments.""" + + +import abc +import gym +import torch +from typing import Dict, Iterable, List, Optional, Tuple, Union + +import carb +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +import omni.isaac.core.utils.torch as torch_utils +import omni.replicator.core as rep +import omni.usd +from omni.isaac.cloner import GridCloner +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.carb import set_carb_setting +from omni.isaac.core.utils.extensions import disable_extension +from omni.isaac.core.utils.viewports import set_camera_view + +from .isaac_env_cfg import IsaacEnvCfg + +# Define type aliases here to avoid circular import +VecEnvIndices = Union[int, Iterable[int]] +"""Indices of the sub-environments. Used when we want to access one or more environments.""" + +VecEnvObs = Dict[str, Union[torch.Tensor, Dict[str, torch.Tensor]]] +"""Observation returned by the environment. It contains the observation for each sub-environment. + +The observations are stored in a dictionary. The keys are the group to which the observations belong. +This is useful for various learning setups beyond vanilla reinforcement learning, such as asymmetric +actor-critic, multi-agent, or hierarchical reinforcement learning. + +For example, for asymmetric actor-critic, the observation for the actor and the critic can be accessed +using the keys ``"policy"`` and ``"critic"`` respectively. + +Within each group, the observations can be stored either as a dictionary with keys as the names of each +observation term in the group, or a single tensor obtained from concatenating all the observation terms. +""" + +VecEnvStepReturn = Tuple[VecEnvObs, torch.Tensor, torch.Tensor, Dict] +"""The environment signals processed at the end of each step. It contains the observation, reward, termination +signal and additional information for each sub-environment.""" + + +class IsaacEnv(gym.Env): + """The superclass for Isaac Sim based environments. + + It encapsulates an environment using Omniverse Isaac Sim for under-the-hood + dynamics and rendering. An environment can be partially or fully-observed. + + Each observation from the environment is a batch of observations for each sub- + environments. The method :meth:`step` is also expected to receive a batch of actions + for each sub-environment. + + While the environment itself is implemented as a vectorized environment, we do not + inherit from :class:`gym.vector.VectorEnv`. This is mainly because the class adds + various methods (for wait and asynchronous updates) which are not required. + Additionally, each RL library typically has its own definition for a vectorized + environment. Thus, to reduce complexity, we directly use the :class:`gym.Env` over + here and leave it up to library-defined wrappers to take care of wrapping this + environment for their agents. + """ + + def __init__(self, cfg: IsaacEnvCfg, headless: bool = False): + """Initialize the environment. + + We currently support only PyTorch backend for the environment. In the future, we plan to extend this to use + other backends such as Warp. + + Args: + cfg (IsaacEnvCfg): Instance of the environment configuration. + headless (bool, optional): Whether to run with GUI or without GUI. Defaults to False. + + Raises: + RuntimeError: No stage is found in the simulation. + """ + # store inputs to class + self.cfg = cfg + self.enable_render = not headless + # extract commonly used parameters + self.num_envs = self.cfg.env.num_envs + self.device = self.cfg.sim.device + self.physics_dt = self.cfg.sim.dt + self.rendering_dt = self.cfg.sim.dt * self.cfg.sim.substeps + + # print useful information + print("[INFO]: Isaac Sim Environment:") + print(f"\t\t Number of instances : {self.num_envs}") + print(f"\t\t Environment device : {self.device}") + print(f"\t\t Physics step-size : {self.physics_dt}") + print(f"\t\t Rendering step-size : {self.rendering_dt}") + + # check that simulation is running + if stage_utils.get_current_stage() is None: + raise RuntimeError("The stage has not been created. Did you run the simulator?") + # flatten out the simulation dictionary + sim_params = self.cfg.sim.to_dict() + if sim_params is not None: + if "physx" in sim_params: + physx_params = sim_params.pop("physx") + sim_params.update(physx_params) + # create a simulation context to control the simulator + self.sim = SimulationContext( + stage_units_in_meters=1.0, + physics_dt=self.physics_dt, + rendering_dt=self.rendering_dt, + backend="torch", + sim_params=sim_params, + physics_prim_path="/physicsScene", + device=self.device, + ) + # set flags for simulator + self._configure_simulation_flags(sim_params) + # add flag for checking closing status + self._is_closed = False + + # initialize common environment buffers + self.reward_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.float) + self.reset_buf = torch.ones(self.num_envs, device=self.device, dtype=torch.long) + self.episode_length_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long) + # allocate dictionary to store metrics + self.extras = {} + # create dictionary for storing last observations + # note: Only used for the corner case of when in the UI, the stopped button is pressed. Then the + # physics handles become invalid. So it is not possible to call :meth:`_get_observations()` + self._last_obs_buf: VecEnvObs = None + + # set camera view + set_camera_view(eye=self.cfg.viewer.eye, target=self.cfg.viewer.lookat) + # create cloner for duplicating the scenes + cloner = GridCloner(spacing=self.cfg.env.env_spacing) + cloner.define_base_env(self.env_ns) + # create the xform prim to hold the template environment + if not prim_utils.is_prim_path_valid(self.template_env_ns): + prim_utils.define_prim(self.template_env_ns) + # setup single scene + global_prim_paths = self._design_scene() + # check if any global prim paths are defined + if global_prim_paths is None: + global_prim_paths = list() + # clone the scenes into the namespace "/World/envs" based on template namespace + self.envs_prim_paths = cloner.generate_paths(self.env_ns + "/env", self.num_envs) + self.envs_positions = cloner.clone( + source_prim_path=self.template_env_ns, prim_paths=self.envs_prim_paths, replicate_physics=True + ) + # convert environment positions to torch tensor + self.envs_positions = torch.tensor(self.envs_positions, dtype=torch.float, device=self.device) + # filter collisions within each environment instance + physics_scene_path = self.sim.get_physics_context().prim_path + cloner.filter_collisions( + physics_scene_path, "/World/collisions", prim_paths=self.envs_prim_paths, global_paths=global_prim_paths + ) + + """ + Properties + """ + + @property + def env_ns(self) -> str: + """The namespace ``/World/envs`` in which all environments created. + + The environments are present w.r.t. this namespace under "env_{N}" prim, + where N is a natural number. + """ + return "/World/envs" + + @property + def template_env_ns(self) -> str: + """The namespace ``/World/envs/env_0`` used for the template environment. + + All prims under this namespace are cloned during construction. + """ + return self.env_ns + "/env_0" + + """ + Operations - MDP + """ + + @staticmethod + def seed(seed: int = -1): + """Set the seed for the environment. + + Args: + seed (int, optional): The seed for random generator. Defaults to -1. + """ + rep.set_global_seed(seed) + return torch_utils.set_seed(seed) + + def reset(self) -> VecEnvObs: + """Flags all environments for reset. + + Note: + Once the buffers for resetting the environments are set, a simulation step is + taken to forward all other buffers. + + Returns: + VecEnvObs: Observations from the environment. + """ + # reset state of scene + indices = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) + self._reset_idx(indices) + # perform one step to have buffers into action + self.sim.step(render=False) + # compute common quantities + self._cache_common_quantities() + # compute observations + self._last_obs_buf = self._get_observations() + # return observations + return self._last_obs_buf + + def step(self, actions: torch.Tensor) -> VecEnvStepReturn: + """Reset any terminated environments and apply actions on the environment. + + This function deals with various timeline events (play, pause and stop) for clean execution. + When the simulation is stopped all the physics handles expire and we cannot perform any read or + write operations. The timeline event is only detected after every `sim.step()` call. Hence, at + every call we need to check the status of the simulator. The logic is as follows: + + 1. If the simulation is stopped, we complain about it and return the previous buffers. + 2. If the simulation is paused, we do not set any actions, but step the simulator. + 3. If the simulation is playing, we set the actions and step the simulator. + + Args: + actions (torch.Tensor): Actions to apply on the simulator. + + Note: + We perform resetting of the terminated environments before simulation + stepping. This is because the physics buffers are not forwarded until + the physics step occurs. + + Returns: + VecEnvStepReturn: A tuple containing: + - (VecEnvObs) observations from the environment + - (torch.Tensor) reward from the environment + - (torch.Tensor) whether the current episode is completed or not + - (dict) misc information + """ + # check if the simulation timeline is playing + # if stopped, we complain about it and return the previous mdp buffers + if self.sim.is_stopped(): + carb.log_warn("Simulation is stopped. Please exit the simulator...") + # if paused, we do not set any actions into the simulator, but step + elif not self.sim.is_playing(): + # step the simulator (but not the physics) to have UI still active + self.sim.render() + # check if the simulation timeline is stopped, do not update buffers + if not self.sim.is_stopped(): + self._last_obs_buf = self._get_observations() + else: + carb.log_warn("Simulation is stopped. Please exit the simulator...") + # if playing, we set the actions into the simulator and step + else: + # reset environments that terminated + reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if len(reset_env_ids) > 0: + self._reset_idx(reset_env_ids) + # increment the number of steps + self.episode_length_buf += 1 + # perform the stepping of simulation + self._step_impl(actions) + # check if the simulation timeline is stopped, do not update buffers + if not self.sim.is_stopped(): + self._last_obs_buf = self._get_observations() + else: + carb.log_warn("Simulation is stopped. Please exit the simulator...") + # return observations, rewards, resets and extras + return self._last_obs_buf, self.reward_buf, self.reset_buf, self.extras + + def render(self): + """Run rendering without stepping through the physics.""" + self.sim.render() + + def close(self): + """Cleanup for the environment.""" + if not self._is_closed: + # stop physics simulation (precautionary) + self.sim.stop() + # cleanup the scene and callbacks + self.sim.clear_all_callbacks() + self.sim.clear() + # fix warnings at stage close + omni.usd.get_context().get_stage().GetRootLayer().Clear() + # update closing status + self._is_closed = True + + """ + Implementation specifics. + """ + + @abc.abstractmethod + def _design_scene(self) -> Optional[List[str]]: + """Creates the template environment scene. + + All prims under the *template namespace* will be duplicated across the + stage and collisions between the duplicates will be filtered out. In case, + there are any prims which need to be a common collider across all the + environments, they should be returned as a list of prim paths. These could + be prims like the ground plane, walls, etc. + + Returns: + Optional[List[str]]: List of prim paths which are common across all the + environments and need to be considered for common collision filtering. + """ + raise NotImplementedError + + @abc.abstractmethod + def _reset_idx(self, env_ids: VecEnvIndices): + """Resets the MDP for given environment instances. + + Args: + env_ids (VecEnvIndices): Indices of environment instances to reset. + """ + raise NotImplementedError + + @abc.abstractmethod + def _step_impl(self, actions: torch.Tensor): + """Apply actions on the environment and computes MDP signals. + + This function sets the simulation buffers for actions to apply, perform + stepping of the simulation, and compute the MDP signals for reward and + termination. + + Note: + The environment specific implementation of this function is responsible for also + stepping the simulation. To have a clean exit when the timeline is stopped + through the UI, the implementation should check the simulation status + after stepping the simulator and return if the simulation is stopped. + + .. code-block:: python + + # simulate + self.sim.step(render=self.enable_render) + # check that simulation is playing + if self.sim.is_stopped(): + return + + Args: + actions (torch.Tensor): Actions to apply on the environment. + """ + raise NotImplementedError + + @abc.abstractmethod + def _get_observations(self) -> VecEnvObs: + """Grabs observations from the environment. + + The observations are stored in a dictionary. The keys are the group to which the observations belong. + This is useful for various learning setups beyond vanilla reinforcement learning, such as asymmetric + actor-critic, multi-agent, or hierarchical reinforcement learning. + + By default, most learning frameworks deal with default and privileged observations in different ways. + This handling must be taken care of by the wrapper around the :class:`IsaacEnv` instance. + + Note: + For included frameworks (RSL-RL, RL-Games), the observations must have the key "policy". In case, + the key "critic" is also present, then the critic observations are taken from the "critic" group. + Otherwise, they are the same as the "policy" group. + + Returns: + VecEnvObs: Observations from the environment. + """ + raise NotImplementedError + + """ + Helper functions - MDP. + """ + + def _cache_common_quantities(self) -> None: + """Cache common quantities from simulator used for computing MDP signals. + + Implementing this function is optional. It is mostly useful in scenarios where + shared quantities between observations, rewards and termination signals need to be + computed only once. + + Note: + The function should be called after performing simulation stepping and before + computing any MDP signals. + """ + pass + + """ + Helper functions - Simulation. + """ + + def _configure_simulation_flags(self, sim_params: dict = None): + """Configure the various flags for performance. + + This function enables flat-cache for speeding up GPU pipeline, enables hydra scene-graph + instancing for visualizing multiple instances when flatcache is enabled, and disables the + viewport if running in headless mode. + """ + # enable flat-cache for speeding up GPU pipeline + if self.sim.get_physics_context().use_gpu_pipeline: + self.sim.get_physics_context().enable_flatcache(True) + # enable hydra scene-graph instancing + # Think: Create your own carb-settings instance? + set_carb_setting(self.sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + # check viewport settings + if sim_params and "enable_viewport" in sim_params: + # if viewport is disabled, then don't create a window (minor speedups) + if not sim_params["enable_viewport"]: + disable_extension("omni.kit.viewport.window") diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/isaac_env_cfg.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/isaac_env_cfg.py new file mode 100644 index 0000000000..7f55188922 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/isaac_env_cfg.py @@ -0,0 +1,195 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base configuration of the environment. + +This module defines the general configuration of the environment. It includes parameters for +configuring the environment instances, viewer settings, and simulation parameters. +""" + +from dataclasses import MISSING +from typing import Tuple + +from omni.isaac.orbit.utils import configclass + +__all__ = ["IsaacEnvCfg", "EnvCfg", "ViewerCfg", "PhysxCfg", "SimCfg"] + + +## +# General environment configuration +## + + +@configclass +class EnvCfg: + """Configuration of the common environment information.""" + + num_envs: int = MISSING + """Number of environment instances to create.""" + env_spacing: float = MISSING + """Spacing between cloned environments.""" + episode_length_s: float = None + """Duration of an episode (in seconds). Default is None (no limit).""" + + +@configclass +class ViewerCfg: + """Configuration of the scene viewport camera.""" + + debug_vis: bool = False + """Whether to enable/disable debug visualization in the scene.""" + eye: Tuple[float, float, float] = (7.5, 7.5, 7.5) + """Initial camera position (in m). Default is (7.5, 7.5, 7.5).""" + lookat: Tuple[float, float, float] = (0.0, 0.0, 0.0) + """Initial camera target position (in m). Default is (0.0, 0.0, 0.0).""" + + +## +# Simulation settings +## + + +@configclass +class PhysxCfg: + """PhysX solver parameters. + + These parameters are used to configure the PhysX solver. For more information, see the PhysX 5 SDK + documentation. + + PhysX 5 supports GPU-accelerated physics simulation. This is enabled by default, but can be disabled + through the flag `use_gpu`. Unlike CPU PhysX, the GPU simulation feature is not able to dynamically + grow all the buffers. Therefore, it is necessary to provide a reasonable estimate of the buffer sizes + for GPU features. If insufficient buffer sizes are provided, the simulation will fail with errors and + lead to adverse behaviors. The buffer sizes can be adjusted through the `gpu_*` parameters. + + References: + * PhysX 5 documentation: https://nvidia-omniverse.github.io/PhysX/ + """ + + use_gpu: bool = True + """Enable/disable GPU accelerated dynamics simulation. Default is True. + + This enables GPU-accelerated implementations for broad-phase collision checks, contact generation, + shape and body management, and constrained solver. + """ + + solver_type: int = 1 + """The type of solver to use.Default is 1 (TGS). + + Available solvers: + + * :obj:`0`: PGS (Projective Gauss-Seidel) + * :obj:`1`: TGS (Truncated Gauss-Seidel) + """ + + enable_stabilization: bool = True + """Enable/disable additional stabilization pass in solver. Default is True.""" + + bounce_threshold_velocity: float = 0.5 + """Relative velocity threshold for contacts to bounce (in m/s). Default is 0.5 m/s.""" + + friction_offset_threshold: float = 0.04 + """Threshold for contact point to experience friction force (in m). Default is 0.04 m.""" + + friction_correlation_distance: float = 0.025 + """Distance threshold for merging contacts into a single friction anchor point (in m). Default is 0.025 m.""" + + gpu_max_rigid_contact_count: int = 1024 * 1024 + """Size of rigid contact stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + + gpu_max_rigid_patch_count: int = 80 * 1024 * 2 + """Size of the rigid contact patch stream buffer allocated in pinned host memory. Default is 80 * 2 ** 11.""" + + gpu_found_lost_pairs_capacity: int = 1024 * 2 + """Capacity of found and lost buffers allocated in GPU global memory. Default is 2 ** 11. + + This is used for the found/lost pair reports in the BP. + """ + + gpu_found_lost_aggregate_pairs_capacity: int = 1024 * 1024 * 32 + """Capacity of found and lost buffers in aggregate system allocated in GPU global memory. + Default is 2 ** 21. + + This is used for the found/lost pair reports in AABB manager. + """ + + gpu_total_aggregate_pairs_capacity: int = 1024 * 1024 * 2 + """Capacity of total number of aggregate pairs allocated in GPU global memory. Default is 2 ** 21.""" + + gpu_heap_capacity: int = 64 * 1024 * 1024 + """Initial capacity of the GPU and pinned host memory heaps. Additional memory will be allocated + if more memory is required. Default is 2 ** 26.""" + + gpu_temp_buffer_capacity: int = 16 * 1024 * 1024 + """Capacity of temp buffer allocated in pinned host memory. Default is 2 ** 24.""" + + gpu_max_num_partitions: int = 8 + """Limitation for the partitions in the GPU dynamics pipeline. Default is 8. + + This variable must be power of 2. A value greater than 32 is currently not supported. Range: (1, 32) + """ + + gpu_max_soft_body_contacts: int = 1024 * 1024 + """Size of soft body contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + + gpu_max_particle_contacts: int = 1024 * 1024 + """Size of particle contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + + +@configclass +class SimCfg: + """Configuration for simulation physics.""" + + dt: float = 1.0 / 60.0 + """The physics simulation time-step (in seconds). Default is 0.0167 seconds.""" + + substeps: int = 1 + """The number of physics simulation steps per rendering step. Default is 1.""" + + gravity: Tuple[float, float, float] = (0.0, 0.0, -9.81) + """The gravity vector (in m/s^2). Default is (0.0, 0.0, -9.81).""" + + enable_scene_query_support: bool = True + """Enable/disable scene query support when using instanced assets. Default is True. + + If this is set to False, the geometries of instances assets will appear stationary. However, this + can also provide some performance speed-up. + """ + + use_flatcache: bool = True # output from simulation to flat cache + """Enable/disable reading of physics buffers directly. Default is True. + + If this is set to False, the physics buffers will be read from USD, which leads to overhead with + massive parallelization. + """ + + use_gpu_pipeline: bool = True + """Enable/disable GPU pipeline. Default is True. + + If this is set to False, the physics data will be read as CPU buffers. + """ + + device: str = "cuda:0" + """The device for running the simulation/environment. Default is "cuda:0".""" + + physx: PhysxCfg = PhysxCfg() + """PhysX solver settings. Default is PhysxCfg().""" + + +## +# Environment configuration +## + + +@configclass +class IsaacEnvCfg: + """Base configuration of the environment.""" + + env: EnvCfg = MISSING + """General environment configuration.""" + viewer: ViewerCfg = ViewerCfg() + """Viewer configuration. Default is ViewerCfg().""" + sim: SimCfg = SimCfg() + """Physics simulation configuration. Default is SimCfg().""" diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/__init__.py new file mode 100644 index 0000000000..6bd2dc7b64 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Locomotion environments for legged robots. + +These environments are based on the `legged_gym` environments provided by Rudin et al. + +Reference: + https://github.com/leggedrobotics/legged_gym +""" diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/__init__.py new file mode 100644 index 0000000000..9a4a7c4921 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Velocity-based locomotion environments for legged robots.""" + +from .velocity_cfg import VelocityEnvCfg +from .velocity_env import VelocityEnv + +__all__ = ["VelocityEnv", "VelocityEnvCfg"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/velocity_cfg.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/velocity_cfg.py new file mode 100644 index 0000000000..23054d55b4 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/velocity_cfg.py @@ -0,0 +1,199 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from typing import Tuple + +from omni.isaac.orbit.robots.config.anymal import ANYMAL_C_CFG +from omni.isaac.orbit.robots.legged_robot import LeggedRobotCfg +from omni.isaac.orbit.utils import configclass +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + +from omni.isaac.orbit_envs.isaac_env_cfg import EnvCfg, IsaacEnvCfg, SimCfg, ViewerCfg + +## +# Scene settings +## + + +@configclass +class TerrainCfg: + """Configuration for terrain to load.""" + + # whether to enable or disable rough terrain + use_default_ground_plane = True + # usd file to import + # usd_path = f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd" + usd_path = f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/flat_plane.usd" + + +@configclass +class MarkerCfg: + """Properties for visualization marker.""" + + # usd file to import + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd" + # scale of the asset at import + scale = [1.0, 0.1, 0.1] # x,y,z + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Configuration for the goals in the environment.""" + + @configclass + class Ranges: + """Ranges for the commands.""" + + lin_vel_x: Tuple[float, float] = (-1.0, 1.0) # min max [m/s] + lin_vel_y: Tuple[float, float] = (-1.0, 1.0) # min max [m/s] + ang_vel_yaw: Tuple[float, float] = (-1.5, 1.5) # min max [rad/s] + heading: Tuple[float, float] = (-3.14, 3.14) # [rad] + + curriculum = False + max_curriculum = 1.0 + num_commands = 3 # default: lin_vel_x, lin_vel_y, ang_vel_yaw + resampling_time = 4.0 # time before commands are changed [s] + heading_command = False # if true: compute ang vel command from heading error + ranges: Ranges = Ranges() + + +@configclass +class RandomizationCfg: + """Randomization of scene at reset.""" + + initial_base_position = {"enabled": False, "xy_range": (-1.0, 1.0)} + """Initial XY position of the base at the time of reset.""" + + initial_base_velocity = {"enabled": True, "vel_range": (-0.5, 0.5)} + """Initial velocity of the base at the time of reset.""" + + push_robot = {"enabled": True, "interval_s": 15.0, "vel_xy_range": (-1.0, 1.0)} + """Pushes the robots at each time interval (in sec) with velocity offset (in m/s).""" + + additive_body_mass = {"enabled": True, "body_name": "base", "range": (-5.0, 5.0)} + """Adds mass to body on the robot in the specified range.""" + + feet_material_properties = { + "enabled": True, + "static_friction_range": (0.5, 1.5), + "dynamic_friction_range": (0.5, 1.5), + "restitution_range": (0.0, 0.1), + "num_buckets": 64, + } + """Direct randomization of material properties.""" + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg: + """Observations for policy group.""" + + # global group settings + enable_corruption: bool = True + # observation terms (order preserved) + base_lin_vel = {"noise": {"name": "uniform", "min": -0.1, "max": 0.1}} + base_ang_vel = {"noise": {"name": "uniform", "min": -0.2, "max": 0.2}} + projected_gravity = {"noise": {"name": "uniform", "min": -0.05, "max": 0.05}} + velocity_commands = {} + dof_pos = {"noise": {"name": "uniform", "min": -0.01, "max": 0.01}} + dof_vel = {"noise": {"name": "uniform", "min": -1.5, "max": 1.5}} + actions = {} + + # global observation settings + return_dict_obs_in_group = False + """Whether to return observations as dictionary or flattened vector within groups.""" + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # global settings + only_positive_rewards: bool = True + + # -- base + lin_vel_xy_exp = {"weight": 1.0, "std": 0.25} + ang_vel_z_exp = {"weight": 0.5, "std": 0.25} + lin_vel_z_l2 = {"weight": -2.0} + ang_vel_xy_l2 = {"weight": -0.05} + flat_orientation_l2 = {"weight": -2.0} + # base_height_l2 = {"weight": -0.5, "target_height": 0.57} + + # -- dof undesirable + # dof_pos_limits = {"weight": 1e-2} + # dof_vel_limits = {"weight": 1e-2} + dof_torques_l2 = {"weight": -0.000025} + dof_acc_l2 = {"weight": -2.5e-7} + # dof_vel_l2 = {"weight": 0.0} + + # -- command undesirable + action_rate_l2 = {"weight": -0.01} + # applied_torque_limits = {"weight": 1e-2} + + # -- cosmetic + # Note: This reward is useless till we have a proper contact sensor. + # feet_air_time = {"weight": 2.0, "time_threshold": 0.5} + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + episode_timeout = {"enabled": True} + """Reset when episode length ended.""" + base_height_fall = {"enabled": True, "min_height": 0.4} + """Reset when base falls below certain height.""" + + +@configclass +class ControlCfg: + """Processing of MDP actions.""" + + # decimation: Number of control action updates @ sim dt per policy dt + decimation = 4 + # scaling of input actions + action_scale = 0.5 + # clipping of input actions + action_clipping = 100.0 + + +## +# Environment configuration +## + + +@configclass +class VelocityEnvCfg(IsaacEnvCfg): + + # General Settings + env: EnvCfg = EnvCfg(num_envs=4096, env_spacing=2.5, episode_length_s=20.0) + viewer: ViewerCfg = ViewerCfg() + # Physics settings + sim: SimCfg = SimCfg(dt=0.005, substeps=4) + + # Scene Settings + terrain: TerrainCfg = TerrainCfg() + robot: LeggedRobotCfg = ANYMAL_C_CFG + marker: MarkerCfg = MarkerCfg() + + # MDP settings + commands: CommandsCfg = CommandsCfg() + randomization: RandomizationCfg = RandomizationCfg() + observations: ObservationsCfg = ObservationsCfg() + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + # Controller settings + control: ControlCfg = ControlCfg() diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/velocity_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/velocity_env.py new file mode 100644 index 0000000000..ddc63e4195 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/velocity_env.py @@ -0,0 +1,529 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import copy +import gym.spaces +import math +import torch +from typing import List, Sequence, Tuple + +import omni.isaac.core.utils.prims as prim_utils +import omni.replicator.core as rep +import omni.replicator.isaac as rep_dr +from omni.isaac.core.utils.types import DynamicsViewState + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.markers import PointMarker, StaticMarker +from omni.isaac.orbit.robots.legged_robot import LeggedRobot +from omni.isaac.orbit.utils.dict import class_to_dict +from omni.isaac.orbit.utils.math import quat_apply, quat_from_euler_xyz, sample_uniform, wrap_to_pi +from omni.isaac.orbit.utils.mdp import ObservationManager, RewardManager + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvIndices, VecEnvObs + +from .velocity_cfg import VelocityEnvCfg + + +class VelocityEnv(IsaacEnv): + """Environment for tracking a base SE(2) velocity command for a legged robot.""" + + def __init__(self, cfg: VelocityEnvCfg = None, headless: bool = False): + # copy configuration + self.cfg = cfg + + # create classes + self.robot = LeggedRobot(cfg=self.cfg.robot) + + # initialize the base class to setup the scene. + super().__init__(self.cfg, headless=headless) + # parse the configuration for information + self._process_cfg() + # initialize views for the cloned scenes + self._initialize_views() + # setup randomization in environment + self._setup_randomization() + + # prepare the observation manager + self._observation_manager = LocomotionVelocityObservationManager( + class_to_dict(self.cfg.observations), self, self.device + ) + # prepare the reward manager + self._reward_manager = LocomotionVelocityRewardManager( + class_to_dict(self.cfg.rewards), self, self.num_envs, self.dt, self.device + ) + # print information about MDP + print("[INFO] Observation Manager:", self._observation_manager) + print("[INFO] Reward Manager: ", self._reward_manager) + + # compute the observation space + # TODO: Cleanup the `_group_obs_dim` variable. + num_obs = self._observation_manager._group_obs_dim["policy"][0] + self.observation_space = gym.spaces.Box(low=-math.inf, high=math.inf, shape=(num_obs,)) + # compute the action space + self.action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(self.num_actions,)) + print("[INFO]: Completed setting up the environment...") + + """ + Implementation specifics. + """ + + def _design_scene(self) -> List[str]: + # ground plane + if self.cfg.terrain.use_default_ground_plane: + # use the default ground plane + kit_utils.create_ground_plane( + "/World/defaultGroundPlane", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + improve_patch_friction=True, + combine_mode="max", + ) + else: + prim_utils.create_prim("/World/defaultGroundPlane", usd_path=self.cfg.terrain.usd_path) + + # robot + self.robot.spawn(self.template_env_ns + "/Robot") + + # setup debug visualization + if self.cfg.viewer.debug_vis and self.enable_render: + # create point instancer for foot contact + self._feet_contact_marker = PointMarker( + "/Visuals/feet_contact", self.num_envs * len(self.cfg.robot.feet_info), radius=0.035 + ) + # create point instancer to visualize the goal base velocity + self._base_vel_goal_markers = StaticMarker( + "/Visuals/base_vel_goal", + self.num_envs, + usd_path=self.cfg.marker.usd_path, + scale=self.cfg.marker.scale, + color=(1.0, 0.0, 0.0), + ) + # create marker for viewing current base velocity + self._base_vel_markers = StaticMarker( + "/Visuals/base_vel", + self.num_envs, + usd_path=self.cfg.marker.usd_path, + scale=self.cfg.marker.scale, + color=(0.0, 0.0, 1.0), + ) + # return list of global prims + return ["/World/defaultGroundPlane"] + + def _reset_idx(self, env_ids: VecEnvIndices): + """Reset environments based on specified indices. + + Calls the following functions on reset: + - :func:`_reset_robot_state`: Reset the root state and DOF state of the robot. + - :func:`_resample_commands`: Resample the goal/command for the task. E.x.: desired velocity command. + - :func:`_sim_randomization`: Randomizes simulation properties using replicator. E.x.: friction, body mass. + + Addition to above, the function fills up episode information into extras and resets buffers. + + Args: + env_ids (list[int]): List of environment ids which must be reset + """ + # randomize the MDP + # -- robot state + self._reset_robot_state(env_ids) + # -- robot buffers + self.robot.reset_buffers(env_ids) + # -- resample commands + self._resample_commands(env_ids) + # -- randomize dynamics with replicator + rep_dr.physics_view.step_randomization(env_ids) + + # -- Reward logging + # fill extras with episode information + self.extras["episode"] = dict() + # reset + # -- rewards manager: fills the sums for terminated episodes + self._reward_manager.reset_idx(env_ids, self.extras["episode"]) + # -- obs manager + self._observation_manager.reset_idx(env_ids) + # -- reset history + self.previous_actions[env_ids] = 0 + # -- MDP reset + self.reset_buf[env_ids] = 0 + self.episode_length_buf[env_ids] = 0 + + def _step_impl(self, actions: torch.Tensor): + # pre-step: set actions into buffer + # clip actions and move to env device + self.actions = actions.clone().to(device=self.device) + self.actions = self.actions.clip_(-self.cfg.control.action_clipping, self.cfg.control.action_clipping) + # scaled actions + scaled_actions = self.cfg.control.action_scale * self.actions + # perform physics stepping + for _ in range(self.cfg.control.decimation): + # set actions into buffers + self.robot.apply_action(scaled_actions) + # simulate + self.sim.step(render=self.enable_render) + # check that simulation is playing + if self.sim.is_stopped(): + return + # post-step: + # -- update env counters (used for curriculum generation) + self.common_step_counter += 1 + # -- update robot buffers + self.robot.update_buffers(dt=self.dt) + + # compute MDP signals + # reward + self.reward_buf = self._reward_manager.compute() + # terminations + self._check_termination() + # -- store history + self.previous_actions = self.actions.clone() + + # in-between episode randomization + # -- resample commands in between episodes + env_ids = self.episode_length_buf % self._command_interval == 0 + env_ids = env_ids.nonzero(as_tuple=False).flatten() + self._resample_commands(env_ids) + # -- update heading for new commands and on-going command + self._update_command() + # -- push robots + if self.cfg.randomization.push_robot["enabled"]: + if self.common_step_counter % self._push_interval == 0: + self._push_robots() + + # -- add information to extra if timeout occurred due to episode length + # Note: this is used by algorithms like PPO where time-outs are handled differently + self.extras["time_outs"] = self.episode_length_buf >= self.max_episode_length + # -- update USD visualization + if self.cfg.viewer.debug_vis and self.enable_render: + self._debug_vis() + + def _get_observations(self) -> VecEnvObs: + # compute observations + return self._observation_manager.compute() + + """ + Helper functions - Scene handling. + """ + + def _process_cfg(self) -> None: + """Post processing of configuration parameters.""" + # compute constants for environment + self.dt = self.cfg.control.decimation * self.physics_dt # control-dt + self.max_episode_length = math.ceil(self.cfg.env.episode_length_s / self.dt) + + # store constants about the environment + # randomization + # -- random velocity command + self._command_ranges = copy.deepcopy(self.cfg.commands.ranges) + # -- command sampling + self._command_interval = math.ceil(self.cfg.commands.resampling_time / self.dt) + # -- push robots + self._push_interval = math.ceil(self.cfg.randomization.push_robot["interval_s"] / self.dt) + + def _initialize_views(self) -> None: + """Creates views and extract useful quantities from them""" + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + self.sim.reset() + + # define views over instances + self.robot.initialize(self.env_ns + "/.*/Robot") + # define action space + self.num_actions = self.robot.num_actions + + # initialize some data used later on + # -- counter for curriculum + self.common_step_counter = 0 + # -- history + self.actions = torch.zeros((self.num_envs, self.num_actions), device=self.device) + self.previous_actions = torch.zeros((self.num_envs, self.num_actions), device=self.device) + # -- command: x vel, y vel, yaw vel, heading + self.commands = torch.zeros(self.num_envs, self.cfg.commands.num_commands, device=self.device) + self.heading_target = torch.zeros(self.num_envs, device=self.device) + + def _setup_randomization(self): + """Randomize properties of scene at start.""" + # randomize body masses + # TODO: Make configurable via class! + if self.cfg.randomization.additive_body_mass["enabled"]: + # read configuration for randomization + body_name = self.cfg.randomization.additive_body_mass["body_name"] + mass_range = self.cfg.randomization.additive_body_mass["range"] + # set properties into robot + body_index = self.robot.body_names.index(body_name) + body_masses = self.robot.articulations.get_body_masses(body_indices=[body_index]) + body_masses += sample_uniform(mass_range[0], mass_range[1], size=(self.robot.count, 1), device=self.device) + self.robot.articulations.set_body_masses(body_masses, body_indices=[body_index]) + + # register views with replicator + for body in self.robot.feet_bodies.values(): + # FIXME: Hacking default state because Isaac core doesn't handle non-root link well! + body._dynamics_default_state = DynamicsViewState( + positions=torch.zeros(body.count, 3, device=body._device), + orientations=torch.zeros(body.count, 4, device=body._device), + linear_velocities=torch.zeros(body.count, 3, device=body._device), + angular_velocities=torch.zeros(body.count, 3, device=body._device), + ) + # register with replicator + rep_dr.physics_view.register_rigid_prim_view(rigid_prim_view=body) + + # create graph using replicator + with rep_dr.trigger.on_rl_frame(num_envs=self.num_envs): + with rep_dr.gate.on_env_reset(): + # -- feet + if self.cfg.randomization.feet_material_properties["enabled"]: + # read configuration for randomization + sf = self.cfg.randomization.feet_material_properties["static_friction_range"] + df = self.cfg.randomization.feet_material_properties["dynamic_friction_range"] + res = self.cfg.randomization.feet_material_properties["restitution_range"] + # set properties into robot + for body in self.robot.feet_bodies.values(): + rep_dr.physics_view.randomize_rigid_prim_view( + view_name=body.name, + operation="direct", + material_properties=rep.distribution.uniform( + [sf[0], df[0], res[0]] * body.num_shapes, [sf[1], df[1], res[1]] * body.num_shapes + ), + num_buckets=self.cfg.randomization.feet_material_properties["num_buckets"], + ) + # prepares/executes the action graph for randomization + rep.orchestrator.run() + + def _debug_vis(self): + # get marker location + # -- base state + base_pos_w = self.robot.data.root_pos_w.clone() + base_pos_w[:, 2] += 0.5 + # -- command + vel_des_arrow_scale, vel_des_arrow_quat = self._resolve_xy_velocity_to_arrow(self.commands[:, :2]) + vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_lin_vel_w[:, :2]) + # -- feet state + feet_pose_w = self.robot.data.feet_state_w[..., :7].clone().view(-1, 7) + feet_status = torch.where(self.robot.data.feet_air_time.view(-1) > 0.0, 1, 2) + # apply to instance manager + # -- feet marker + self._feet_contact_marker.set_world_poses(feet_pose_w[:, :3], feet_pose_w[:, 3:7]) + self._feet_contact_marker.set_status(feet_status) + # -- goal + self._base_vel_goal_markers.set_scales(vel_des_arrow_scale) + self._base_vel_goal_markers.set_world_poses(base_pos_w, vel_des_arrow_quat) + # -- base velocity + self._base_vel_markers.set_scales(vel_arrow_scale) + self._base_vel_markers.set_world_poses(base_pos_w, vel_arrow_quat) + + def _resolve_xy_velocity_to_arrow(self, xy_velocity: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """Converts the XY base velocity command to arrow direction rotation.""" + # arrow-scale + arrow_scale = torch.tensor(self.cfg.marker.scale, device=self.device).repeat(xy_velocity.shape[0], 1) + arrow_scale[:, 0] *= torch.linalg.norm(xy_velocity, dim=1) + # arrow-direction + heading_angle = torch.atan2(xy_velocity[:, 1], xy_velocity[:, 0]) + zeros = torch.zeros_like(heading_angle) + arrow_quat = quat_from_euler_xyz(zeros, zeros, heading_angle) + + return arrow_scale, arrow_quat + + """ + Helper functions - MDP. + """ + + def _reset_robot_state(self, env_ids): + """Resets root and dof states of robots in selected environments.""" + # handle trivial case + if len(env_ids) == 0: + return + # -- dof state (handled by the robot) + dof_pos, dof_vel = self.robot.get_random_dof_state(env_ids) + self.robot.set_dof_state(dof_pos, dof_vel, env_ids=env_ids) + # -- root state (custom) + root_state = self.robot.get_default_root_state(env_ids) + root_state[:, :3] += self.envs_positions[env_ids] + # xy position + if self.cfg.randomization.initial_base_position["enabled"]: + xy_range = self.cfg.randomization.initial_base_position["xy_range"] + root_state[:, :2] += sample_uniform(xy_range[0], xy_range[1], (len(env_ids), 2), device=self.device) + # base velocities: [7:10]: lin vel, [10:13]: ang vel + if self.cfg.randomization.initial_base_velocity["enabled"]: + vel_range = self.cfg.randomization.initial_base_velocity["vel_range"] + root_state[:, 7:13] = sample_uniform(vel_range[0], vel_range[1], (len(env_ids), 6), device=self.device) + else: + root_state[:, 7:13] = 0.0 + # set into robot + self.robot.set_root_state(root_state, env_ids=env_ids) + + def _resample_commands(self, env_ids: Sequence[int]): + """Randomly select commands of some environments.""" + # handle trivial case + if len(env_ids) == 0: + return + # linear velocity - x direction + _min, _max = self._command_ranges.lin_vel_x + self.commands[env_ids, 0] = sample_uniform(_min, _max, len(env_ids), device=self.device) + # linear velocity - y direction + _min, _max = self._command_ranges.lin_vel_y + self.commands[env_ids, 1] = sample_uniform(_min, _max, len(env_ids), device=self.device) + # ang vel yaw or heading target + if self.cfg.commands.heading_command: + _min, _max = self._command_ranges.heading + self.heading_target[env_ids] = sample_uniform(_min, _max, len(env_ids), device=self.device) + else: + _min, _max = self._command_ranges.ang_vel_yaw + self.commands[env_ids, 2] = sample_uniform(_min, _max, len(env_ids), device=self.device) + # set small commands to zero + self.commands[env_ids, :2] *= (torch.norm(self.commands[env_ids, :2], dim=1) > 0.2).unsqueeze(1).float() + + def _update_command(self): + """Recompute heading command based on current orientation of the robot.""" + if self.cfg.commands.heading_command: + # convert current motion direction to world frame + forward = quat_apply(self.robot.data.root_quat_w, self.robot._FORWARD_VEC_B) + # convert direction to heading + heading = torch.atan2(forward[:, 1], forward[:, 0]) + # adjust command to provide corrected heading based on target + self.commands[:, 2] = torch.clip( + 0.5 * wrap_to_pi(self.heading_target - heading), + self._command_ranges.ang_vel_yaw[0], + self._command_ranges.ang_vel_yaw[1], + ) + + def _push_robots(self): + """Random pushes the robots. Emulates an impulse by setting a randomized base velocity.""" + min_vel, max_vel = self.cfg.randomization.push_robot["vel_xy_range"] + # get current root state + root_state_w = self.robot.data.root_state_w + # add random XY velocity to the base + root_state_w[:, 7:9] += sample_uniform(min_vel, max_vel, (self.num_envs, 2), device=self.device) + # set the root state + self.robot.set_root_state(root_state_w) + + def _check_termination(self) -> None: + # extract values from buffer + # compute resets + self.reset_buf[:] = 0 + # -- episode length + if self.cfg.terminations.episode_timeout["enabled"]: + self.reset_buf = torch.where(self.episode_length_buf >= self.max_episode_length, 1, self.reset_buf) + # -- base height fall + if self.cfg.terminations.base_height_fall["enabled"]: + base_target_height = self.cfg.terminations.base_height_fall["min_height"] + self.reset_buf = torch.where(self.robot.data.root_pos_w[:, 2] <= base_target_height, 1, self.reset_buf) + + +class LocomotionVelocityObservationManager(ObservationManager): + """Observation manager for locomotion velocity-tracking environment.""" + + def base_lin_vel(self, env: VelocityEnv): + """Base linear velocity in base frame.""" + return env.robot.data.root_lin_vel_b + + def base_ang_vel(self, env: VelocityEnv): + """Base angular velocity in base frame.""" + return env.robot.data.root_ang_vel_b + + def projected_gravity(self, env: VelocityEnv): + """Gravity projection on base frame.""" + return env.robot.data.projected_gravity_b + + def velocity_commands(self, env: VelocityEnv): + """Desired base velocity for the robot.""" + return env.commands + + def dof_pos(self, env: VelocityEnv): + """DOF positions for legs offset by the drive default positions.""" + return env.robot.data.dof_pos - env.robot.data.actuator_pos_offset + + def dof_vel(self, env: VelocityEnv): + """DOF velocity of the legs.""" + return env.robot.data.dof_vel - env.robot.data.actuator_vel_offset + + def actions(self, env: VelocityEnv): + """Last actions provided to env.""" + return env.actions + + +class LocomotionVelocityRewardManager(RewardManager): + """Reward manager for locomotion velocity-tracking environment.""" + + def lin_vel_z_l2(self, env: VelocityEnv): + """Penalize z-axis base linear velocity using L2-kernel.""" + return torch.square(env.robot.data.root_lin_vel_w[:, 2]) + + def ang_vel_xy_l2(self, env: VelocityEnv): + """Penalize xy-axii base angular velocity using L2-kernel.""" + return torch.sum(torch.square(env.robot.data.root_ang_vel_w[:, :2]), dim=1) + + def flat_orientation_l2(self, env: VelocityEnv): + """Penalize non-float base orientation.""" + return torch.sum(torch.square(env.robot.data.projected_gravity_b[:, :2]), dim=1) + + def dof_torques_l2(self, env: VelocityEnv): + """Penalize torques applied on the robot.""" + return torch.sum(torch.square(env.robot.data.applied_torques), dim=1) + + def dof_vel_l2(self, env: VelocityEnv): + """Penalize dof velocities on the robot.""" + return torch.sum(torch.square(env.robot.data.dof_vel), dim=1) + + def dof_acc_l2(self, env: VelocityEnv): + """Penalize dof accelerations on the robot.""" + return torch.sum(torch.square(env.robot.data.dof_acc), dim=1) + + def dof_pos_limits(self, env: VelocityEnv): + """Penalize dof positions too close to the limit.""" + out_of_limits = -(env.robot.data.dof_pos - env.robot.data.soft_dof_pos_limits[..., 0]).clip(max=0.0) + out_of_limits += (env.robot.data.dof_pos - env.robot.data.soft_dof_pos_limits[..., 1]).clip(min=0.0) + return torch.sum(out_of_limits, dim=1) + + def dof_vel_limits(self, env: VelocityEnv, soft_ratio: float): + """Penalize dof velocities too close to the limit + + Args: + soft_ratio (float): Defines the soft limit as a percentage of the hard limit. + """ + out_of_limits = torch.abs(env.robot.data.dof_vel) - env.robot.data.soft_dof_vel_limits * soft_ratio + # clip to max error = 1 rad/s per joint to avoid huge penalties + out_of_limits = out_of_limits.clip_(min=0.0, max=1.0) + return torch.sum(out_of_limits, dim=1) + + def action_rate_l2(self, env: VelocityEnv): + """Penalize changes in actions.""" + return torch.sum(torch.square(env.previous_actions - env.actions), dim=1) + + def applied_torque_limits(self, env: VelocityEnv): + """Penalize applied torques that are too close to the actuator limits.""" + out_of_limits = torch.abs(env.robot.data.applied_torques - env.robot.data.computed_torques) + return torch.sum(out_of_limits, dim=1) + + def base_height_l2(self, env: VelocityEnv, target_height: float): + """Penalize base height from its target.""" + # TODO: Fix this for rough-terrain. + base_height = env.robot.data.root_pos_w[:, 2] + return torch.square(base_height - target_height) + + def lin_vel_xy_exp(self, env: VelocityEnv, std: float): + """Tracking of linear velocity commands (xy axes). + + Args: + std (float): Defines the width of the bell-curve. + """ + lin_vel_error = torch.sum(torch.square(env.commands[:, :2] - env.robot.data.root_lin_vel_b[:, :2]), dim=1) + return torch.exp(-lin_vel_error / std) + + def ang_vel_z_exp(self, env: VelocityEnv, std): + """Tracking of angular velocity commands (yaw). + + Args: + std (float): Defines the width of the bell-curve. + """ + ang_vel_error = torch.square(env.commands[:, 2] - env.robot.data.root_ang_vel_b[:, 2]) + return torch.exp(-ang_vel_error / std) + + def feet_air_time(self, env: VelocityEnv, time_threshold: float): + """Reward long steps taken by the feet.""" + first_contact = env.robot.data.feet_air_time > 0.0 + reward = torch.sum((env.robot.data.feet_air_time - time_threshold) * first_contact, dim=1) + # no reward for zero command + reward *= torch.norm(env.commands[:, :2], dim=1) > 0.1 + return reward diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/__init__.py new file mode 100644 index 0000000000..a54776c8b6 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Manipulation environments for fixed-arm robots.""" diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/__init__.py new file mode 100644 index 0000000000..c7e8646084 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Environment for lifting objects with fixed-arm robots.""" + +from .lift_cfg import LiftEnvCfg +from .lift_env import LiftEnv + +__all__ = ["LiftEnv", "LiftEnvCfg"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py new file mode 100644 index 0000000000..ae2dc68d3b --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py @@ -0,0 +1,217 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.orbit.controllers.differential_inverse_kinematics import DifferentialInverseKinematicsCfg +from omni.isaac.orbit.objects import RigidObjectCfg +from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG +from omni.isaac.orbit.robots.single_arm import SingleArmManipulatorCfg +from omni.isaac.orbit.utils import configclass +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + +from omni.isaac.orbit_envs.isaac_env_cfg import EnvCfg, IsaacEnvCfg, PhysxCfg, SimCfg, ViewerCfg + +## +# Scene settings +## + + +@configclass +class TableCfg: + """Properties for the table.""" + + # note: we use instanceable asset since it consumes less memory + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" + + +@configclass +class ManipulationObjectCfg(RigidObjectCfg): + """Properties for the object to manipulate in the scene.""" + + meta_info = RigidObjectCfg.MetaInfoCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + geom_prim_rel_path="/collisions", + scale=(0.8, 0.8, 0.8), + ) + init_state = RigidObjectCfg.InitialStateCfg( + pos=(0.4, 0.0, 0.075), rot=(1.0, 0.0, 0.0, 0.0), lin_vel=(0.0, 0.0, 0.0), ang_vel=(0.0, 0.0, 0.0) + ) + rigid_props = RigidObjectCfg.RigidBodyPropertiesCfg( + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=10.0, + disable_gravity=False, + ) + material_props = RigidObjectCfg.PhysicsMaterialPropertiesCfg( + static_friction=0.5, dynamic_friction=0.5, restitution=0.0, material_path="/physics_material" + ) + + +@configclass +class GoalMarkerCfg: + """Properties for visualization marker.""" + + # usd file to import + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd" + # scale of the asset at import + scale = [0.8, 0.8, 0.8] # x,y,z + + +@configclass +class FrameMarkerCfg: + """Properties for visualization marker.""" + + # usd file to import + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd" + # scale of the asset at import + scale = [0.1, 0.1, 0.1] # x,y,z + + +## +# MDP settings +## + + +@configclass +class RandomizationCfg: + """Randomization of scene at reset.""" + + @configclass + class ObjectInitialPoseCfg: + """Randomization of object initial pose.""" + + # category + position_cat: str = "default" # randomize position: "default", "uniform" + orientation_cat: str = "default" # randomize position: "default", "uniform" + # randomize position + position_uniform_min = [0.25, -0.25, 0.25] # position (x,y,z) + position_uniform_max = [0.5, 0.25, 0.5] # position (x,y,z) + + @configclass + class ObjectDesiredPoseCfg: + """Randomization of object desired pose.""" + + # category + position_cat: str = "default" # randomize position: "default", "uniform" + orientation_cat: str = "default" # randomize position: "default", "uniform" + # randomize position + position_default = [0.5, 0.0, 0.5] # position default (x,y,z) + position_uniform_min = [0.25, -0.25, 0.25] # position (x,y,z) + position_uniform_max = [0.5, 0.25, 0.5] # position (x,y,z) + # randomize orientation + orientation_default = [1.0, 0.0, 0.0, 0.0] # orientation default + + # initialize + object_initial_pose: ObjectInitialPoseCfg = ObjectInitialPoseCfg() + object_desired_pose: ObjectDesiredPoseCfg = ObjectDesiredPoseCfg() + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg: + """Observations for policy group.""" + + # global group settings + enable_corruption: bool = True + # observation terms + arm_dof_pos_scaled = {"scale": 1.0, "noise": {"name": "uniform", "min": -0.01, "max": 0.01}} + tool_dof_pos_scaled = {"scale": 1.0} + arm_dof_vel = {"scale": 0.5, "noise": {"name": "uniform", "min": -0.1, "max": 0.1}} + tool_positions = {} + object_positions = {} + object_desired_positions = {} + actions = {} + + # global observation settings + return_dict_obs_in_group = False + """Whether to return observations as dictionary or flattened vector within groups.""" + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # robot-centric + reaching_object_position_l2 = {"weight": 0.0} + reaching_object_position_exp = {"weight": 2.5, "sigma": 0.25} + penalizing_robot_dof_velocity_l2 = {"weight": 1e-4} + penalizing_robot_dof_acceleration_l2 = {"weight": 1e-7} + penalizing_action_rate_l2 = {"weight": 1e-2} + # object-centric + tracking_object_position_exp = {"weight": 2.5, "sigma": 0.5} + lifting_object_success = {"weight": 0.0, "threshold": 1e-3} + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + episode_timeout = True # reset when episode length ended + object_falling = True # reset when object falls off the table + is_success = False # reset when object is lifted + + +@configclass +class ControlCfg: + """Processing of MDP actions.""" + + # action space + control_type = "default" # "default", "inverse_kinematics" + # decimation: Number of control action updates @ sim dt per policy dt + decimation = 2 + + # configuration loaded when control_type == "inverse_kinematics" + inverse_kinematics: DifferentialInverseKinematicsCfg = DifferentialInverseKinematicsCfg( + command_type="pose_rel", + ik_method="dls", + position_command_scale=(0.1, 0.1, 0.1), + rotation_command_scale=(0.1, 0.1, 0.1), + ) + + +## +# Environment configuration +## + + +@configclass +class LiftEnvCfg(IsaacEnvCfg): + + # General Settings + env: EnvCfg = EnvCfg(num_envs=1024, env_spacing=2.5, episode_length_s=4.0) + viewer: ViewerCfg = ViewerCfg(debug_vis=True, eye=(7.5, 7.5, 7.5), lookat=(0.0, 0.0, 0.0)) + # Physics settings + sim: SimCfg = SimCfg( + dt=1.0 / 60.0, + substeps=1, + physx=PhysxCfg( + gpu_found_lost_aggregate_pairs_capacity=512 * 1024, + gpu_total_aggregate_pairs_capacity=6 * 1024, + ), + ) + + # Scene Settings + # -- robot + robot: SingleArmManipulatorCfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG + # -- object + object: ManipulationObjectCfg = ManipulationObjectCfg() + # -- table + table: TableCfg = TableCfg() + # -- visualization marker + goal_marker: GoalMarkerCfg = GoalMarkerCfg() + frame_marker: FrameMarkerCfg = FrameMarkerCfg() + + # MDP settings + randomization: RandomizationCfg = RandomizationCfg() + observations: ObservationsCfg = ObservationsCfg() + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + # Controller settings + control: ControlCfg = ControlCfg() diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py new file mode 100644 index 0000000000..2985890d25 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py @@ -0,0 +1,425 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gym.spaces +import math +import torch +from typing import List + +import omni.isaac.core.utils.prims as prim_utils + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.controllers.differential_inverse_kinematics import DifferentialInverseKinematics +from omni.isaac.orbit.markers import StaticMarker +from omni.isaac.orbit.objects import RigidObject +from omni.isaac.orbit.robots.single_arm import SingleArmManipulator +from omni.isaac.orbit.utils.dict import class_to_dict +from omni.isaac.orbit.utils.math import random_orientation, sample_uniform, scale_transform +from omni.isaac.orbit.utils.mdp import ObservationManager, RewardManager + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvIndices, VecEnvObs + +from .lift_cfg import LiftEnvCfg, RandomizationCfg + + +class LiftEnv(IsaacEnv): + """Environment for lifting an object off a table with a single-arm manipulator.""" + + def __init__(self, cfg: LiftEnvCfg = None, headless: bool = False): + # copy configuration + self.cfg = cfg + # parse the configuration for controller configuration + # note: controller decides the robot control mode + self._pre_process_cfg() + # create classes (these are called by the function :meth:`_design_scene`) + self.robot = SingleArmManipulator(cfg=self.cfg.robot) + self.object = RigidObject(cfg=self.cfg.object) + + # initialize the base class to setup the scene. + super().__init__(self.cfg, headless=headless) + # parse the configuration for information + self._process_cfg() + # initialize views for the cloned scenes + self._initialize_views() + + # prepare the observation manager + self._observation_manager = LiftObservationManager(class_to_dict(self.cfg.observations), self, self.device) + # prepare the reward manager + self._reward_manager = LiftRewardManager( + class_to_dict(self.cfg.rewards), self, self.num_envs, self.dt, self.device + ) + # print information about MDP + print("[INFO] Observation Manager:", self._observation_manager) + print("[INFO] Reward Manager: ", self._reward_manager) + + # compute the observation space: arm joint state + ee-position + goal-position + actions + num_obs = self._observation_manager.group_obs_dim["policy"][0] + self.observation_space = gym.spaces.Box(low=-math.inf, high=math.inf, shape=(num_obs,)) + # compute the action space + self.action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(self.num_actions,)) + print("[INFO]: Completed setting up the environment...") + # Take an initial step to initialize the scene. + self.sim.step() + # -- fill up buffers + self.object.update_buffers(self.dt) + self.robot.update_buffers(self.dt) + + """ + Implementation specifics. + """ + + def _design_scene(self) -> List[str]: + # ground plane + kit_utils.create_ground_plane("/World/defaultGroundPlane", z_position=-1.05) + # table + prim_utils.create_prim(self.template_env_ns + "/Table", usd_path=self.cfg.table.usd_path) + # robot + self.robot.spawn(self.template_env_ns + "/Robot") + # object + self.object.spawn(self.template_env_ns + "/Object") + + # setup debug visualization + if self.cfg.viewer.debug_vis and self.enable_render: + # create point instancer to visualize the goal points + self._goal_markers = StaticMarker( + "/Visuals/object_goal", + self.num_envs, + usd_path=self.cfg.goal_marker.usd_path, + scale=self.cfg.goal_marker.scale, + ) + # create marker for viewing end-effector pose + self._ee_markers = StaticMarker( + "/Visuals/ee_current", + self.num_envs, + usd_path=self.cfg.frame_marker.usd_path, + scale=self.cfg.frame_marker.scale, + ) + # create marker for viewing command (if task-space controller is used) + if self.cfg.control.control_type == "inverse_kinematics": + self._cmd_markers = StaticMarker( + "/Visuals/ik_command", + self.num_envs, + usd_path=self.cfg.frame_marker.usd_path, + scale=self.cfg.frame_marker.scale, + ) + # return list of global prims + return ["/World/defaultGroundPlane"] + + def _reset_idx(self, env_ids: VecEnvIndices): + # randomize the MDP + # -- robot DOF state + dof_pos, dof_vel = self.robot.get_default_dof_state(env_ids=env_ids) + self.robot.set_dof_state(dof_pos, dof_vel, env_ids=env_ids) + # -- object pose + self._randomize_object_initial_pose(env_ids=env_ids, cfg=self.cfg.randomization.object_initial_pose) + # -- goal pose + self._randomize_object_desired_pose(env_ids=env_ids, cfg=self.cfg.randomization.object_desired_pose) + + # -- Reward logging + # fill extras with episode information + self.extras["episode"] = dict() + # reset + # -- rewards manager: fills the sums for terminated episodes + self._reward_manager.reset_idx(env_ids, self.extras["episode"]) + # -- obs manager + self._observation_manager.reset_idx(env_ids) + # -- reset history + self.previous_actions[env_ids] = 0 + # -- MDP reset + self.reset_buf[env_ids] = 0 + self.episode_length_buf[env_ids] = 0 + # controller reset + if self.cfg.control.control_type == "inverse_kinematics": + self._ik_controller.reset_idx(env_ids) + + def _step_impl(self, actions: torch.Tensor): + # pre-step: set actions into buffer + self.actions = actions.clone().to(device=self.device) + # transform actions based on controller + if self.cfg.control.control_type == "inverse_kinematics": + # set the controller commands + self._ik_controller.set_command(self.actions[:, :-1]) + # use IK to convert to joint-space commands + self.robot_actions[:, : self.robot.arm_num_dof] = self._ik_controller.compute( + self.robot.data.ee_state_w[:, 0:3] - self.envs_positions, + self.robot.data.ee_state_w[:, 3:7], + self.robot.data.ee_jacobian, + self.robot.data.arm_dof_pos, + ) + # offset actuator command with position offsets + dof_pos_offset = self.robot.data.actuator_pos_offset + self.robot_actions[:, : self.robot.arm_num_dof] -= dof_pos_offset[:, : self.robot.arm_num_dof] + # we assume last command is gripper action so don't change that + self.robot_actions[:, -1] = self.actions[:, -1] + elif self.cfg.control.control_type == "default": + self.robot_actions[:] = self.actions + # perform physics stepping + for _ in range(self.cfg.control.decimation): + # set actions into buffers + self.robot.apply_action(self.robot_actions) + # simulate + self.sim.step(render=self.enable_render) + # check that simulation is playing + if self.sim.is_stopped(): + return + # post-step: + # -- compute common buffers + self.robot.update_buffers(self.dt) + self.object.update_buffers(self.dt) + # -- compute MDP signals + # reward + self.reward_buf = self._reward_manager.compute() + # terminations + self._check_termination() + # -- store history + self.previous_actions = self.actions.clone() + + # -- add information to extra if timeout occurred due to episode length + # Note: this is used by algorithms like PPO where time-outs are handled differently + self.extras["time_outs"] = self.episode_length_buf >= self.max_episode_length + # -- add information to extra if task completed + object_position_error = torch.norm(self.object.data.root_pos_w - self.object_des_pose_w[:, 0:3], dim=1) + self.extras["is_success"] = torch.where(object_position_error < 0.002, 1, self.reset_buf) + # -- update USD visualization + if self.cfg.viewer.debug_vis and self.enable_render: + self._debug_vis() + + def _get_observations(self) -> VecEnvObs: + # compute observations + return self._observation_manager.compute() + + """ + Helper functions - Scene handling. + """ + + def _pre_process_cfg(self) -> None: + """Pre-processing of configuration parameters.""" + # set configuration for task-space controller + if self.cfg.control.control_type == "inverse_kinematics": + print("Using inverse kinematics controller...") + # enable jacobian computation + self.cfg.robot.data_info.enable_jacobian = True + # enable gravity compensation + self.cfg.robot.rigid_props.disable_gravity = True + # set the end-effector offsets + self.cfg.control.inverse_kinematics.position_offset = self.cfg.robot.ee_info.pos_offset + self.cfg.control.inverse_kinematics.rotation_offset = self.cfg.robot.ee_info.rot_offset + else: + print("Using default joint controller...") + + def _process_cfg(self) -> None: + """Post processing of configuration parameters.""" + # compute constants for environment + self.dt = self.cfg.control.decimation * self.physics_dt # control-dt + self.max_episode_length = math.ceil(self.cfg.env.episode_length_s / self.dt) + + # convert configuration parameters to torchee + # randomization + # -- initial pose + config = self.cfg.randomization.object_initial_pose + for attr in ["position_uniform_min", "position_uniform_max"]: + setattr(config, attr, torch.tensor(getattr(config, attr), device=self.device, requires_grad=False)) + # -- desired pose + config = self.cfg.randomization.object_desired_pose + for attr in ["position_uniform_min", "position_uniform_max", "position_default", "orientation_default"]: + setattr(config, attr, torch.tensor(getattr(config, attr), device=self.device, requires_grad=False)) + + def _initialize_views(self) -> None: + """Creates views and extract useful quantities from them""" + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + self.sim.reset() + + # define views over instances + self.robot.initialize(self.env_ns + "/.*/Robot") + self.object.initialize(self.env_ns + "/.*/Object") + + # create controller + if self.cfg.control.control_type == "inverse_kinematics": + self._ik_controller = DifferentialInverseKinematics( + self.cfg.control.inverse_kinematics, self.robot.count, self.device + ) + self.num_actions = self._ik_controller.num_actions + 1 + elif self.cfg.control.control_type == "default": + self.num_actions = self.robot.num_actions + + # history + self.actions = torch.zeros((self.num_envs, self.num_actions), device=self.device) + self.previous_actions = torch.zeros((self.num_envs, self.num_actions), device=self.device) + # robot joint actions + self.robot_actions = torch.zeros((self.num_envs, self.robot.num_actions), device=self.device) + # commands + self.object_des_pose_w = torch.zeros((self.num_envs, 7), device=self.device) + # time-step = 0 + self.object_init_pose_w = torch.zeros((self.num_envs, 7), device=self.device) + + def _debug_vis(self): + """Visualize the environment in debug mode.""" + # apply to instance manager + # -- goal + self._goal_markers.set_world_poses(self.object_des_pose_w[:, 0:3], self.object_des_pose_w[:, 3:7]) + # -- end-effector + self._ee_markers.set_world_poses(self.robot.data.ee_state_w[:, 0:3], self.robot.data.ee_state_w[:, 3:7]) + # -- task-space commands + if self.cfg.control.control_type == "inverse_kinematics": + # convert to world frame + ee_positions = self._ik_controller.desired_ee_pos + self.envs_positions + ee_orientations = self._ik_controller.desired_ee_rot + # set poses + self._cmd_markers.set_world_poses(ee_positions, ee_orientations) + + """ + Helper functions - MDP. + """ + + def _check_termination(self) -> None: + # access buffers from simulator + object_pos = self.object.data.root_pos_w - self.envs_positions + # extract values from buffer + self.reset_buf[:] = 0 + # compute resets + # -- when task is successful + if self.cfg.terminations.is_success: + object_position_error = torch.norm(self.object.data.root_pos_w - self.object_des_pose_w[:, 0:3], dim=1) + self.reset_buf = torch.where(object_position_error < 0.002, 1, self.reset_buf) + # -- object fell off the table (table at height: 0.0 m) + if self.cfg.terminations.object_falling: + self.reset_buf = torch.where(object_pos[:, 2] < -0.05, 1, self.reset_buf) + # -- episode length + if self.cfg.terminations.episode_timeout: + self.reset_buf = torch.where(self.episode_length_buf >= self.max_episode_length, 1, self.reset_buf) + + def _randomize_object_initial_pose(self, env_ids: torch.Tensor, cfg: RandomizationCfg.ObjectInitialPoseCfg): + """Randomize the initial pose of the object.""" + # get the default root state + root_state = self.object.get_default_root_state(env_ids) + # -- object root position + if cfg.position_cat == "default": + pass + elif cfg.position_cat == "uniform": + # sample uniformly from box + # note: this should be within in the workspace of the robot + root_state[:, 0:3] = sample_uniform( + cfg.position_uniform_min, cfg.position_uniform_max, (len(env_ids), 3), device=self.device + ) + else: + raise ValueError(f"Invalid category for randomizing the object positions '{cfg.position_cat}'.") + # -- object root orientation + if cfg.orientation_cat == "default": + pass + elif cfg.orientation_cat == "uniform": + # sample uniformly in SO(3) + root_state[:, 3:7] = random_orientation(len(env_ids), self.device) + else: + raise ValueError(f"Invalid category for randomizing the object orientation '{cfg.orientation_cat}'.") + # transform command from local env to world + root_state[:, 0:3] += self.envs_positions[env_ids] + # update object init pose + self.object_init_pose_w[env_ids] = root_state[:, 0:7] + # set the root state + self.object.set_root_state(root_state, env_ids=env_ids) + + def _randomize_object_desired_pose(self, env_ids: torch.Tensor, cfg: RandomizationCfg.ObjectDesiredPoseCfg): + """Randomize the desired pose of the object.""" + # -- desired object root position + if cfg.position_cat == "default": + # constant command for position + self.object_des_pose_w[env_ids, 0:3] = cfg.position_default + elif cfg.position_cat == "uniform": + # sample uniformly from box + # note: this should be within in the workspace of the robot + self.object_des_pose_w[env_ids, 0:3] = sample_uniform( + cfg.position_uniform_min, cfg.position_uniform_max, (len(env_ids), 3), device=self.device + ) + else: + raise ValueError(f"Invalid category for randomizing the desired object positions '{cfg.position_cat}'.") + # -- desired object root orientation + if cfg.orientation_cat == "default": + # constant position of the object + self.object_des_pose_w[env_ids, 3:7] = cfg.orientation_default + elif cfg.orientation_cat == "uniform": + self.object_des_pose_w[env_ids, 3:7] = random_orientation(len(env_ids), self.device) + else: + raise ValueError( + f"Invalid category for randomizing the desired object orientation '{cfg.orientation_cat}'." + ) + # transform command from local env to world + self.object_des_pose_w[env_ids, 0:3] += self.envs_positions[env_ids] + + +class LiftObservationManager(ObservationManager): + """Reward manager for single-arm reaching environment.""" + + def arm_dof_pos_scaled(self, env: LiftEnv): + """DOF positions for the arm normalized to its max and min ranges.""" + return scale_transform( + env.robot.data.arm_dof_pos, + env.robot.data.soft_dof_pos_limits[:, : env.robot.arm_num_dof, 0], + env.robot.data.soft_dof_pos_limits[:, : env.robot.arm_num_dof, 1], + ) + + def arm_dof_vel(self, env: LiftEnv): + """DOF velocity of the arm.""" + return env.robot.data.arm_dof_vel + + def tool_dof_pos_scaled(self, env: LiftEnv): + """DOF positions of the tool normalized to its max and min ranges.""" + return scale_transform( + env.robot.data.tool_dof_pos, + env.robot.data.soft_dof_pos_limits[:, env.robot.arm_num_dof :, 0], + env.robot.data.soft_dof_pos_limits[:, env.robot.arm_num_dof :, 1], + ) + + def tool_positions(self, env: LiftEnv): + """Current end-effector position of the arm.""" + return env.robot.data.ee_state_w[:, :3] - env.envs_positions + + def object_positions(self, env: LiftEnv): + """Current object position.""" + return env.object.data.root_pos_w - env.envs_positions + + def object_desired_positions(self, env: LiftEnv): + """Desired object position.""" + return env.object_des_pose_w[:, 0:3] - env.envs_positions + + def actions(self, env: LiftEnv): + """Last actions provided to env.""" + return env.actions + + +class LiftRewardManager(RewardManager): + """Reward manager for single-arm object lifting environment.""" + + def reaching_object_position_l2(self, env: LiftEnv): + """Penalize end-effector tracking position error using L2-kernel.""" + return torch.sum(torch.square(env.robot.data.ee_state_w[:, 0:3] - env.object.data.root_pos_w), dim=1) + + def reaching_object_position_exp(self, env: LiftEnv, sigma: float): + """Penalize end-effector tracking position error using exp-kernel.""" + return torch.sum(torch.square(env.robot.data.ee_state_w[:, 0:3] - env.object.data.root_pos_w), dim=1) + + def penalizing_robot_dof_velocity_l2(self, env: LiftEnv): + """Penalize large movements of the robot arm.""" + return -torch.sum(torch.square(env.robot.data.arm_dof_vel), dim=1) + + def penalizing_robot_dof_acceleration_l2(self, env: LiftEnv): + """Penalize fast movements of the robot arm.""" + return -torch.sum(torch.square(env.robot.data.dof_acc), dim=1) + + def penalizing_action_rate_l2(self, env: LiftEnv): + """Penalize large variations in action commands.""" + return -torch.sum(torch.square(env.actions - env.previous_actions), dim=1) + + def tracking_object_position_exp(self, env: LiftEnv, sigma: float): + """Penalize tracking object position error using exp-kernel.""" + initial_error = torch.sum(torch.square(env.object_des_pose_w[:, 0:3] - env.object_init_pose_w[:, 0:3]), dim=1) + current_error = torch.sum(torch.square(env.object_des_pose_w[:, 0:3] - env.object.data.root_pos_w), dim=1) + return torch.exp(-current_error / sigma) - torch.exp(-initial_error / sigma) + + def lifting_object_success(self, env: LiftEnv, threshold: float): + """Sparse reward if object is lifted successfully.""" + error = torch.sum(torch.square(env.object.data.root_pos_w - env.object_des_pose_w[:, 0:3]), dim=1) + return torch.where(error < threshold, 1.0, 0.0) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/__init__.py new file mode 100644 index 0000000000..408540183c --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Environment for end-effector pose tracking task for fixed-arm robots.""" + +from .reach_env import ReachEnv, ReachEnvCfg + +__all__ = ["ReachEnv", "ReachEnvCfg"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/reach_cfg.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/reach_cfg.py new file mode 100644 index 0000000000..032902b61f --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/reach_cfg.py @@ -0,0 +1,150 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.orbit.controllers.differential_inverse_kinematics import DifferentialInverseKinematicsCfg +from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG +from omni.isaac.orbit.robots.single_arm import SingleArmManipulatorCfg +from omni.isaac.orbit.utils import configclass +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + +from omni.isaac.orbit_envs.isaac_env_cfg import EnvCfg, IsaacEnvCfg, SimCfg, ViewerCfg + +## +# Scene settings +## + + +@configclass +class TableCfg: + """Properties for the table.""" + + # note: we use instanceable asset since it consumes less memory + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" + + +@configclass +class MarkerCfg: + """Properties for visualization marker.""" + + # usd file to import + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd" + # scale of the asset at import + scale = [0.1, 0.1, 0.1] # x,y,z + + +## +# MDP settings +## + + +@configclass +class RandomizationCfg: + """Randomization of scene at reset.""" + + @configclass + class EndEffectorDesiredPoseCfg: + """Randomization of end-effector pose command.""" + + # category + position_cat: str = "default" # randomize position: "default", "uniform" + orientation_cat: str = "default" # randomize position: "default", "uniform" + # randomize position + position_default = [0.5, 0.0, 0.5] # position default (x,y,z) + position_uniform_min = [0.25, -0.25, 0.25] # position (x,y,z) + position_uniform_max = [0.5, 0.25, 0.5] # position (x,y,z) + # randomize orientation + orientation_default = [1.0, 0.0, 0.0, 0.0] # orientation default + + # initialize + ee_desired_pose: EndEffectorDesiredPoseCfg = EndEffectorDesiredPoseCfg() + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg: + """Observations for policy group.""" + + # global group settings + enable_corruption: bool = True + # observation terms + arm_dof_pos_normalized = {"scale": 1.0, "noise": {"name": "uniform", "min": -0.01, "max": 0.01}} + arm_dof_vel = {"scale": 0.5, "noise": {"name": "uniform", "min": -0.1, "max": 0.1}} + ee_position = {} + ee_position_command = {} + actions = {} + + # global observation settings + return_dict_obs_in_group = False + """Whether to return observations as dictionary or flattened vector within groups.""" + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + tracking_robot_position_l2 = {"weight": 0.0} + tracking_robot_position_exp = {"weight": 2.5, "sigma": 0.05} # 0.25 + penalizing_robot_dof_velocity_l2 = {"weight": -0.02} # -1e-4 + penalizing_robot_dof_acceleration_l2 = {"weight": -1e-5} + penalizing_action_rate_l2 = {"weight": -0.1} + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + episode_timeout = True # reset when episode length ended + + +@configclass +class ControlCfg: + """Processing of MDP actions.""" + + # action space + control_type = "default" # "default", "inverse_kinematics" + # decimation: Number of control action updates @ sim dt per policy dt + decimation = 2 + + # configuration loaded when control_type == "inverse_kinematics" + inverse_kinematics: DifferentialInverseKinematicsCfg = DifferentialInverseKinematicsCfg( + command_type="pose_rel", + ik_method="dls", + position_command_scale=(0.1, 0.1, 0.1), + rotation_command_scale=(0.1, 0.1, 0.1), + ) + + +## +# Environment configuration +## + + +@configclass +class ReachEnvCfg(IsaacEnvCfg): + + # General Settings + env: EnvCfg = EnvCfg(num_envs=2048, env_spacing=2.5, episode_length_s=4.0) + viewer: ViewerCfg = ViewerCfg(debug_vis=True, eye=(7.5, 7.5, 7.5), lookat=(0.0, 0.0, 0.0)) + # Physics settings + sim: SimCfg = SimCfg(dt=1.0 / 60.0, substeps=1) + + # Scene Settings + robot: SingleArmManipulatorCfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG + table: TableCfg = TableCfg() + marker: MarkerCfg = MarkerCfg() + + # MDP settings + randomization: RandomizationCfg = RandomizationCfg() + observations: ObservationsCfg = ObservationsCfg() + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + # Controller settings + control: ControlCfg = ControlCfg() diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/reach_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/reach_env.py new file mode 100644 index 0000000000..1cca1b0847 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/reach_env.py @@ -0,0 +1,342 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gym.spaces +import math +import torch + +import omni.isaac.core.utils.prims as prim_utils + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.controllers.differential_inverse_kinematics import DifferentialInverseKinematics +from omni.isaac.orbit.markers import PointMarker, StaticMarker +from omni.isaac.orbit.robots.single_arm import SingleArmManipulator +from omni.isaac.orbit.utils.dict import class_to_dict +from omni.isaac.orbit.utils.math import random_orientation, sample_uniform, scale_transform +from omni.isaac.orbit.utils.mdp import ObservationManager, RewardManager + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvIndices, VecEnvObs + +from .reach_cfg import RandomizationCfg, ReachEnvCfg + + +class ReachEnv(IsaacEnv): + """Environment for reaching to desired pose for a single-arm manipulator.""" + + def __init__(self, cfg: ReachEnvCfg = None, headless: bool = False): + # copy configuration + self.cfg = cfg + # parse the configuration for controller configuration + # note: controller decides the robot control mode + self._pre_process_cfg() + # create classes (these are called by the function :meth:`_design_scene` + self.robot = SingleArmManipulator(cfg=self.cfg.robot) + + # initialize the base class to setup the scene. + super().__init__(self.cfg, headless=headless) + # parse the configuration for information + self._process_cfg() + # initialize views for the cloned scenes + self._initialize_views() + + # prepare the observation manager + self._observation_manager = ReachObservationManager(class_to_dict(self.cfg.observations), self, self.device) + # prepare the reward manager + self._reward_manager = ReachRewardManager( + class_to_dict(self.cfg.rewards), self, self.num_envs, self.dt, self.device + ) + # print information about MDP + print("[INFO] Observation Manager:", self._observation_manager) + print("[INFO] Reward Manager: ", self._reward_manager) + + # compute the observation space + num_obs = self._observation_manager._group_obs_dim["policy"][0] + self.observation_space = gym.spaces.Box(low=-math.inf, high=math.inf, shape=(num_obs,)) + # compute the action space + self.action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(self.num_actions,)) + print("[INFO]: Completed setting up the environment...") + # Take an initial step to initialize the scene. + self.sim.step() + # -- fill up buffers + self.robot.update_buffers(self.dt) + + """ + Implementation specifics. + """ + + def _design_scene(self): + # ground plane + kit_utils.create_ground_plane("/World/defaultGroundPlane", z_position=-1.05) + # table + prim_utils.create_prim(self.template_env_ns + "/Table", usd_path=self.cfg.table.usd_path) + # robot + self.robot.spawn(self.template_env_ns + "/Robot") + + # setup debug visualization + if self.cfg.viewer.debug_vis and self.enable_render: + # create point instancer to visualize the goal points + self._goal_markers = PointMarker("/Visuals/ee_goal", self.num_envs, radius=0.025) + # create marker for viewing end-effector pose + self._ee_markers = StaticMarker( + "/Visuals/ee_current", self.num_envs, usd_path=self.cfg.marker.usd_path, scale=self.cfg.marker.scale + ) + # create marker for viewing command (if task-space controller is used) + if self.cfg.control.control_type == "inverse_kinematics": + self._cmd_markers = StaticMarker( + "/Visuals/ik_command", self.num_envs, usd_path=self.cfg.marker.usd_path, scale=self.cfg.marker.scale + ) + # return list of global prims + return ["/World/defaultGroundPlane"] + + def _reset_idx(self, env_ids: VecEnvIndices): + # randomize the MDP + # -- robot DOF state + dof_pos, dof_vel = self.robot.get_default_dof_state(env_ids=env_ids) + self.robot.set_dof_state(dof_pos, dof_vel, env_ids=env_ids) + # -- desired end-effector pose + self._randomize_ee_desired_pose(env_ids, cfg=self.cfg.randomization.ee_desired_pose) + + # -- Reward logging + # fill extras with episode information + self.extras["episode"] = dict() + # reset + # -- rewards manager: fills the sums for terminated episodes + self._reward_manager.reset_idx(env_ids, self.extras["episode"]) + # -- obs manager + self._observation_manager.reset_idx(env_ids) + # -- reset history + self.previous_actions[env_ids] = 0 + # -- MDP reset + self.reset_buf[env_ids] = 0 + self.episode_length_buf[env_ids] = 0 + # controller reset + if self.cfg.control.control_type == "inverse_kinematics": + self._ik_controller.reset_idx(env_ids) + + def _step_impl(self, actions: torch.Tensor): + # pre-step: set actions into buffer + self.actions = actions.clone().to(device=self.device) + # transform actions based on controller + if self.cfg.control.control_type == "inverse_kinematics": + # set the controller commands + self._ik_controller.set_command(self.actions) + # compute the joint commands + self.robot_actions[:, : self.robot.arm_num_dof] = self._ik_controller.compute( + self.robot.data.ee_state_w[:, 0:3] - self.envs_positions, + self.robot.data.ee_state_w[:, 3:7], + self.robot.data.ee_jacobian, + self.robot.data.arm_dof_pos, + ) + # offset actuator command with position offsets + self.robot_actions[:, : self.robot.arm_num_dof] -= self.robot.data.actuator_pos_offset[ + :, : self.robot.arm_num_dof + ] + elif self.cfg.control.control_type == "default": + self.robot_actions[:, : self.robot.arm_num_dof] = self.actions + # perform physics stepping + for _ in range(self.cfg.control.decimation): + # set actions into buffers + self.robot.apply_action(self.robot_actions) + # simulate + self.sim.step(render=self.enable_render) + # check that simulation is playing + if self.sim.is_stopped(): + return + # post-step: + # -- compute common buffers + self.robot.update_buffers(self.dt) + # -- compute MDP signals + # reward + self.reward_buf = self._reward_manager.compute() + # terminations + self._check_termination() + # -- store history + self.previous_actions = self.actions.clone() + + # -- add information to extra if timeout occurred due to episode length + # Note: this is used by algorithms like PPO where time-outs are handled differently + self.extras["time_outs"] = self.episode_length_buf >= self.max_episode_length + # -- update USD visualization + if self.cfg.viewer.debug_vis and self.enable_render: + self._debug_vis() + + def _get_observations(self) -> VecEnvObs: + # compute observations + return self._observation_manager.compute() + + """ + Helper functions - Scene handling. + """ + + def _pre_process_cfg(self) -> None: + """Pre processing of configuration parameters.""" + # set configuration for task-space controller + if self.cfg.control.control_type == "inverse_kinematics": + print("Using inverse kinematics controller...") + # enable jacobian computation + self.cfg.robot.data_info.enable_jacobian = True + # enable gravity compensation + self.cfg.robot.rigid_props.disable_gravity = True + # set the end-effector offsets + self.cfg.control.inverse_kinematics.position_offset = self.cfg.robot.ee_info.pos_offset + self.cfg.control.inverse_kinematics.rotation_offset = self.cfg.robot.ee_info.rot_offset + else: + print("Using default joint controller...") + + def _process_cfg(self) -> None: + """Post processing of configuration parameters.""" + # compute constants for environment + self.dt = self.cfg.control.decimation * self.physics_dt # control-dt + self.max_episode_length = math.ceil(self.cfg.env.episode_length_s / self.dt) + + # convert configuration parameters to torch + # randomization + # -- desired pose + config = self.cfg.randomization.ee_desired_pose + for attr in ["position_uniform_min", "position_uniform_max", "position_default", "orientation_default"]: + setattr(config, attr, torch.tensor(getattr(config, attr), device=self.device, requires_grad=False)) + + def _initialize_views(self) -> None: + """Creates views and extract useful quantities from them""" + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + self.sim.reset() + + # define views over instances + self.robot.initialize(self.env_ns + "/.*/Robot") + + # create controller + if self.cfg.control.control_type == "inverse_kinematics": + self._ik_controller = DifferentialInverseKinematics( + self.cfg.control.inverse_kinematics, self.robot.count, self.device + ) + # note: we exclude gripper from actions in this env + self.num_actions = self._ik_controller.num_actions + elif self.cfg.control.control_type == "default": + # note: we exclude gripper from actions in this env + self.num_actions = self.robot.arm_num_dof + + # history + self.actions = torch.zeros((self.num_envs, self.num_actions), device=self.device) + self.previous_actions = torch.zeros((self.num_envs, self.num_actions), device=self.device) + # robot joint actions + self.robot_actions = torch.zeros((self.num_envs, self.robot.num_actions), device=self.device) + # commands + self.ee_des_pose_w = torch.zeros((self.num_envs, 7), device=self.device) + + def _debug_vis(self): + # compute error between end-effector and command + error = torch.sum(torch.square(self.ee_des_pose_w[:, :3] - self.robot.data.ee_state_w[:, 0:3]), dim=1) + # set indices of the prim based on error threshold + goal_indices = torch.where(error < 0.002, 1, 0) + # apply to instance manager + # -- goal + self._goal_markers.set_world_poses(self.ee_des_pose_w[:, :3], self.ee_des_pose_w[:, 3:7]) + self._goal_markers.set_status(goal_indices) + # -- end-effector + self._ee_markers.set_world_poses(self.robot.data.ee_state_w[:, 0:3], self.robot.data.ee_state_w[:, 3:7]) + # -- task-space commands + if self.cfg.control.control_type == "inverse_kinematics": + # convert to world frame + ee_positions = self._ik_controller.desired_ee_pos + self.envs_positions + ee_orientations = self._ik_controller.desired_ee_rot + # set poses + self._cmd_markers.set_world_poses(ee_positions, ee_orientations) + + """ + Helper functions - MDP. + """ + + def _check_termination(self) -> None: + # extract values from buffer + # compute resets + self.reset_buf[:] = 0 + # -- episode length + if self.cfg.terminations.episode_timeout: + self.reset_buf = torch.where(self.episode_length_buf >= self.max_episode_length, 1, self.reset_buf) + + def _randomize_ee_desired_pose(self, env_ids: torch.Tensor, cfg: RandomizationCfg.EndEffectorDesiredPoseCfg): + """Randomize the desired pose of the end-effector.""" + # -- desired object root position + if cfg.position_cat == "default": + # constant command for position + self.ee_des_pose_w[env_ids, 0:3] = cfg.position_default + elif cfg.position_cat == "uniform": + # sample uniformly from box + # note: this should be within in the workspace of the robot + self.ee_des_pose_w[env_ids, 0:3] = sample_uniform( + cfg.position_uniform_min, cfg.position_uniform_max, (len(env_ids), 3), device=self.device + ) + else: + raise ValueError(f"Invalid category for randomizing the desired object positions '{cfg.position_cat}'.") + # -- desired object root orientation + if cfg.orientation_cat == "default": + # constant position of the object + self.ee_des_pose_w[env_ids, 3:7] = cfg.orientation_default + elif cfg.orientation_cat == "uniform": + self.ee_des_pose_w[env_ids, 3:7] = random_orientation(len(env_ids), self.device) + else: + raise ValueError( + f"Invalid category for randomizing the desired object orientation '{cfg.orientation_cat}'." + ) + # transform command from local env to world + self.ee_des_pose_w[env_ids, 0:3] += self.envs_positions[env_ids] + + +class ReachObservationManager(ObservationManager): + """Reward manager for single-arm reaching environment.""" + + def arm_dof_pos_normalized(self, env: ReachEnv): + """DOF positions for the arm normalized to its max and min ranges.""" + return scale_transform( + env.robot.data.arm_dof_pos, + env.robot.data.soft_dof_pos_limits[:, :7, 0], + env.robot.data.soft_dof_pos_limits[:, :7, 1], + ) + + def arm_dof_vel(self, env: ReachEnv): + """DOF velocity of the arm.""" + return env.robot.data.arm_dof_vel + + def ee_position(self, env: ReachEnv): + """Current end-effector position of the arm.""" + return env.robot.data.ee_state_w[:, :3] - env.envs_positions + + def ee_position_command(self, env: ReachEnv): + """Desired end-effector position of the arm.""" + return env.ee_des_pose_w[:, :3] - env.envs_positions + + def actions(self, env: ReachEnv): + """Last actions provided to env.""" + return env.actions + + +class ReachRewardManager(RewardManager): + """Reward manager for single-arm reaching environment.""" + + def tracking_robot_position_l2(self, env: ReachEnv): + """Penalize tracking position error using L2-kernel.""" + # compute error + error = torch.sum(torch.square(env.ee_des_pose_w[:, :3] - env.robot.data.ee_state_w[:, 0:3]), dim=1) + return error + + def tracking_robot_position_exp(self, env: ReachEnv, sigma: float): + """Penalize tracking position error using exp-kernel.""" + # compute error + error = torch.sum(torch.square(env.ee_des_pose_w[:, :3] - env.robot.data.ee_state_w[:, 0:3]), dim=1) + return torch.exp(-error / sigma) + + def penalizing_robot_dof_velocity_l2(self, env: ReachEnv): + """Penalize large movements of the robot arm.""" + return torch.sum(torch.square(env.robot.data.arm_dof_vel), dim=1) + + def penalizing_robot_dof_acceleration_l2(self, env: ReachEnv): + """Penalize fast movements of the robot arm.""" + return torch.sum(torch.square(env.robot.data.dof_acc), dim=1) + + def penalizing_action_rate_l2(self, env: ReachEnv): + """Penalize large variations in action commands.""" + return torch.sum(torch.square(env.actions - env.previous_actions), dim=1) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/__init__.py new file mode 100644 index 0000000000..02880b653b --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/__init__.py @@ -0,0 +1,65 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for environments.""" + + +import os + +# submodules +from .parse_cfg import load_default_env_cfg, parse_env_cfg + +__all__ = ["load_default_env_cfg", "parse_env_cfg", "get_checkpoint_path"] + + +def get_checkpoint_path(log_path: str, run_dir: str = "*", checkpoint: str = None) -> str: + """Get path to the model checkpoint in input directory. + + The checkpoint file is resolved as: //. + + Args: + log_path (str): The log directory path to find models in. + run_dir (int, optional): The name of the directory containing the run. Defaults to the most + recent directory created inside :obj:`log_dir`. + checkpoint (str, optional): The model checkpoint file or directory name. Defaults to the most recent + recent torch-model saved in the :obj:`run_dir` directory. + + Raises: + ValueError: When no runs are found in the input directory. + + Returns: + str: The path to the model checkpoint. + + Reference: + https://github.com/leggedrobotics/legged_gym/blob/master/legged_gym/utils/helpers.py#L103 + """ + # check if runs present in directory + try: + # find all runs in the directory + runs = [os.path.join(log_path, run) for run in os.scandir(log_path)] + # sort by date to handle change of month + runs = sorted(runs, key=os.path.getmtime) + # create last run file path + last_run_path = runs[-1] + except IndexError: + raise ValueError(f"No runs present in the directory: {log_path}") + # path to the directory containing the run + if run_dir.startswith("*"): + # check if there are other paths. Example: "*/nn" + run_path = run_dir.replace("*", last_run_path) + else: + run_path = os.path.join(log_path, run_dir) + # name of model checkpoint + if checkpoint is None: + # list all model checkpoints in the directory + model_checkpoints = [f for f in os.listdir(run_path) if ".pt" in f] + # sort by date + model_checkpoints.sort(key=lambda m: f"{m:0>15}") + # get latest model checkpoint file + checkpoint_file = model_checkpoints[-1] + else: + checkpoint_file = checkpoint + + return os.path.join(run_path, checkpoint_file) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/data_collector/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/data_collector/__init__.py new file mode 100644 index 0000000000..b5714b6719 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/data_collector/__init__.py @@ -0,0 +1,8 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .robomimic_data_collector import RobomimicDataCollector + +__all__ = ["RobomimicDataCollector"] diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/data_collector/robomimic_data_collector.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/data_collector/robomimic_data_collector.py new file mode 100644 index 0000000000..145e752d9b --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/data_collector/robomimic_data_collector.py @@ -0,0 +1,270 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Interface to collect and store data from the environment using format from `robomimic`.""" + + +import h5py +import json +import numpy as np +import os +import torch +from typing import Iterable, Union + +import carb + + +class RobomimicDataCollector: + """Data collection interface for robomimic. + + This class implements a data collector interface for saving simulation states to disk. + The data is stored in `HDF5`_ binary data format. The class is useful for collecting + demonstrations. The collected data follows the `structure`_ from robomimic. + + All datasets in `robomimic` require the observations and next observations obtained + from before and after the environment step. These are stored as a dictionary of + observations in the keys "obs" and "next_obs" respectively. + + For certain agents in `robomimic`, the episode data should have the following + additional keys: "actions", "rewards", "dones". This behavior can be altered by changing + the dataset keys required in the training configuration for the respective learning agent. + + For reference on datasets, please check the robomimic `documentation`. + + .. _HDF5: https://www.h5py.org/ + .. _structure: https://robomimic.github.io/docs/datasets/overview.html#dataset-structure + .. _documentation: https://github.com/ARISE-Initiative/robomimic/blob/master/robomimic/config/base_config.py#L167-L173 + """ + + def __init__( + self, + env_name: str, + directory_path: str, + filename: str = "test", + num_demos: int = 1, + flush_freq: int = 1, + env_config: dict = None, + ): + """Initializes the data collection wrapper. + + Args: + env_name (str): The name of the environment. + directory_path (str): The path to store collected data. + filename (str, optional): The basename of the saved file. Defaults to "test". + num_demos (int, optional): Number of demonstrations to record until stopping. Defaults to 1. + flush_freq (int, optional): Frequency to dump data to disk. Defaults to 1. + env_config (dict): The configuration for the environment. Defaults to None. + """ + # save input arguments + self._env_name = env_name + self._env_config = env_config + self._directory = os.path.abspath(directory_path) + self._filename = filename + self._num_demos = num_demos + self._flush_freq = flush_freq + # print info + print(self.__str__()) + + # create directory it doesn't exist + if not os.path.isdir(self._directory): + os.makedirs(self._directory) + + # placeholder for current hdf5 file object + self._h5_file_stream = None + self._h5_data_group = None + self._h5_episode_group = None + + # store count of demos within episode + self._demo_count = 0 + # flags for setting up + self._is_first_interaction = True + self._is_stop = False + # create buffers to store data + self._dataset = dict() + + def __del__(self): + """Destructor for data collector.""" + if not self._is_stop: + self.close() + + def __str__(self) -> str: + """Represents the data collector as a string.""" + msg = "Dataset collector object" + msg += f"\tStoring trajectories in directory: {self._directory}\n" + msg += f"\tNumber of demos for collection : {self._num_demos}\n" + msg += f"\tFrequency for saving data to disk: {self._flush_freq}\n" + + return msg + + """ + Properties + """ + + @property + def demo_count(self) -> int: + """The number of demos collected so far.""" + return self._demo_count + + """ + Operations. + """ + + def is_stopped(self) -> bool: + """Whether data collection is stopped or not. + + Returns: + bool: True if data collection has stopped. + """ + return self._is_stop + + def reset(self): + """Reset the internals of data logger.""" + # setup the file to store data in + if self._is_first_interaction: + self._demo_count = 0 + self._create_new_file(self._filename) + self._is_first_interaction = False + # clear out existing buffers + self._dataset = dict() + + def add(self, key: str, value: Union[np.ndarray, torch.Tensor]): + """Add a key-value pair to the dataset. + + The key can be nested by using the "/" character. For example: + "obs/joint_pos". Currently only two-level nesting is supported. + + Args: + key (str): The key name. + value (Union[np.ndarray, torch.Tensor]): The corresponding value + of shape (N, ...), where `N` is number of environments. + + Raises: + ValueError: When provided key has sub-keys more than 2. Example: "obs/joints/pos", instead + of "obs/joint_pos". + """ + # check if data should be recorded + if self._is_first_interaction: + carb.log_warn("Please call reset before adding new data. Calling reset...") + self.reset() + if self._is_stop: + carb.log_warn(f"Desired number of demonstrations collected: {self._demo_count} >= {self._num_demos}.") + return + # check datatype + if isinstance(value, torch.Tensor): + value = value.cpu().numpy() + else: + value = np.asarray(value) + # check if there are sub-keys + sub_keys = key.split("/") + num_sub_keys = len(sub_keys) + if len(sub_keys) > 2: + raise ValueError(f"Input key '{key}' has elements {num_sub_keys} which is more than two.") + # add key to dictionary if it doesn't exist + for i in range(value.shape[0]): + # demo index + if f"env_{i}" not in self._dataset: + self._dataset[f"env_{i}"] = dict() + # key index + if num_sub_keys == 2: + # create keys + if sub_keys[0] not in self._dataset[f"env_{i}"]: + self._dataset[f"env_{i}"][sub_keys[0]] = dict() + if sub_keys[1] not in self._dataset[f"env_{i}"][sub_keys[0]]: + self._dataset[f"env_{i}"][sub_keys[0]][sub_keys[1]] = list() + # add data to key + self._dataset[f"env_{i}"][sub_keys[0]][sub_keys[1]].append(value[i]) + else: + # create keys + if sub_keys[0] not in self._dataset[f"env_{i}"]: + self._dataset[f"env_{i}"][sub_keys[0]] = list() + # add data to key + self._dataset[f"env_{i}"][sub_keys[0]].append(value[i]) + + def flush(self, env_ids: Iterable[int] = (0)): + """Flush the episode data based on environment indices. + + Args: + env_ids (Iterable[int], optional): Environment indices to write data for. Defaults to (0). + """ + # check that data is being recorded + if self._h5_file_stream is None or self._h5_data_group is None: + carb.log_error("No file stream has been opened. Please call reset before flushing data.") + return + # iterate over each environment and add their data + for index in env_ids: + # data corresponding to demo + env_dataset = self._dataset[f"env_{index}"] + # create episode group based on demo count + h5_episode_group = self._h5_data_group.create_group(f"demo_{self._demo_count}") + # store number of steps taken + h5_episode_group.attrs["num_samples"] = len(env_dataset["actions"]) + # store other data from dictionary + for key, value in env_dataset.items(): + if isinstance(value, dict): + # create group + key_group = h5_episode_group.create_group(key) + # add sub-keys values + for sub_key, sub_value in value.items(): + key_group.create_dataset(sub_key, data=np.array(sub_value)) + else: + h5_episode_group.create_dataset(key, data=np.array(value)) + # increment total step counts + self._h5_data_group.attrs["total"] += h5_episode_group.attrs["num_samples"] + # increment total demo counts + self._demo_count += 1 + # reset buffer for environment + self._dataset[f"env_{index}"] = dict() + # dump at desired frequency + if self._demo_count % self._flush_freq == 0: + self._h5_file_stream.flush() + print(f">>> Flushing data to disk. Collected demos: {self._demo_count} / {self._num_demos}") + # if demos collected then stop + if self._demo_count >= self._num_demos: + self.close() + + def close(self): + """Stop recording and save the file at its current state.""" + if not self._is_stop: + print(f">>> Closing recording of data. Collected demos: {self._demo_count} / {self._num_demos}") + # close the file safely + if self._h5_file_stream is not None: + self._h5_file_stream.close() + # mark that data collection is stopped + self._is_stop = True + + """ + Helper functions. + """ + + def _create_new_file(self, fname: str): + """Create a new HDF5 file for writing episode info into. + + Reference: + https://robomimic.github.io/docs/datasets/overview.html + + Args: + fname (str): The base name of the file. + """ + if not fname.endswith(".hdf5"): + fname += ".hdf5" + # define path to file + hdf5_path = os.path.join(self._directory, fname) + # construct the stream object + self._h5_file_stream = h5py.File(hdf5_path, "w") + # create group to store data + self._h5_data_group = self._h5_file_stream.create_group("data") + # stores total number of samples accumulated across demonstrations + self._h5_data_group.attrs["total"] = 0 + # store the environment meta-info + # -- we use gym environment type + # Ref: https://github.com/ARISE-Initiative/robomimic/blob/master/robomimic/envs/env_base.py#L15 + env_type = 2 + # -- check if env config provided + if self._env_config is None: + self._env_config = dict() + # -- add info + self._h5_data_group.attrs["env_args"] = json.dumps( + {"env_name": self._env_name, "type": env_type, "env_kwargs": self._env_config} + ) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/parse_cfg.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/parse_cfg.py new file mode 100644 index 0000000000..da759a6d2f --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/parse_cfg.py @@ -0,0 +1,113 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for parsing and loading environments configurations.""" + + +import gym +import importlib +import inspect +import os +import yaml +from typing import Any, Union + +from omni.isaac.orbit.utils import update_class_from_dict, update_dict + + +def load_default_env_cfg(task_name: str) -> Union[dict, Any]: + """Load default configuration file for an environment from Gym registry. + + This function resolves the configuration file for environment based on the file type. + It supports both YAML and Python configuration files. + + Args: + task_name (str): The name of the environment. + + Returns: + Union[dict, Any]: The parsed configuration object. + """ + # retrieve the configuration file to load + cfg_entry_point: str = gym.spec(task_name)._kwargs.pop("cfg_entry_point") + + # parse the default config file + if cfg_entry_point.endswith(".yaml"): + if os.path.exists(cfg_entry_point): + # absolute path for the config file + config_file = cfg_entry_point + else: + # resolve path to the module location + mod_name, file_name = cfg_entry_point.split(":") + mod_path = os.path.dirname(importlib.import_module(mod_name).__file__) + # obtain the configuration file path + config_file = os.path.join(mod_path, file_name) + # load the configuration + print(f"[INFO]: Parsing default environment configuration from: {config_file}") + with open(config_file) as f: + cfg = yaml.full_load(f) + else: + if callable(cfg_entry_point): + # resolve path to the module location + mod_path = inspect.getfile(cfg_entry_point) + # load the configuration + cfg_cls = cfg_entry_point() + else: + # resolve path to the module location + mod_name, attr_name = cfg_entry_point.split(":") + mod = importlib.import_module(mod_name) + cfg_cls = getattr(mod, attr_name) + # load the configuration + print(f"[INFO]: Parsing default environment configuration from: {inspect.getfile(cfg_cls)}") + cfg = cfg_cls() + + return cfg + + +def parse_env_cfg(task_name: str, use_gpu: bool = True, num_envs: int = None, **kwargs) -> Union[dict, Any]: + """Parse configuration file for an environment and override based on inputs. + + Args: + task_name (str): The name of the environment. + use_gpu (bool, optional): Whether to use GPU/CPU pipeline. Defaults to True. + num_envs (int, optional): Number of environments to create. Defaults to True. + + Returns: + Union[dict, Any]: The parsed configuration object. + """ + # create a dictionary to update from + args_cfg = {"sim": {"physx": dict()}, "env": dict()} + # resolve pipeline to use (based on input) + if not use_gpu: + args_cfg["sim"]["use_gpu_pipeline"] = False + args_cfg["sim"]["physx"]["use_gpu"] = False + args_cfg["sim"]["device"] = "cpu" + else: + args_cfg["sim"]["use_gpu_pipeline"] = True + args_cfg["sim"]["physx"]["use_gpu"] = True + args_cfg["sim"]["device"] = "cuda:0" + # FIXME (15.05.2022): if environment name has soft, then GPU pipeline is not supported yet + if "soft" in task_name.lower(): + args_cfg["sim"]["use_gpu_pipeline"] = False + args_cfg["sim"]["physx"]["use_gpu"] = True + args_cfg["sim"]["device"] = "cpu" + args_cfg["sim"]["use_flatcache"] = False + + # number of environments + if num_envs is not None: + args_cfg["env"]["num_envs"] = num_envs + print(f"[Config]: Overriding number of environments to: {num_envs}") + + # load the configuration + cfg = load_default_env_cfg(task_name) + # update the main configuration + if isinstance(cfg, dict): + cfg = update_dict(cfg, args_cfg) + else: + update_class_from_dict(cfg, args_cfg) + + # print information about pipeline + print("[INFO]: Simulation pipeline: ", "GPU" if args_cfg["sim"]["use_gpu_pipeline"] else "CPU") + print("[INFO]: Simulation device : ", "GPU" if args_cfg["sim"]["physx"]["use_gpu"] else "CPU") + + return cfg diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/__init__.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/__init__.py new file mode 100644 index 0000000000..eab8252221 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module includes wrappers for various RL frameworks. + +Currently the following are supported: + +* Stable-Baselines3: https://github.com/DLR-RM/stable-baselines3 +* RL-Games: https://github.com/Denys88/rl_games +* RSL-RL: https://github.com/leggedrobotics/rsl_rl + +""" diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/rl_games.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/rl_games.py new file mode 100644 index 0000000000..e7671f8ee9 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/rl_games.py @@ -0,0 +1,227 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapper to configure an :class:`IsaacEnv` instance to RL-Games vectorized environment. + +The following example shows how to wrap an environment for RL-Games and register the environment construction +for RL-Games :class:`Runner` class: + +.. code-block:: python + + from rl_games.common import env_configurations, vecenv + + from omni.isaac.orbit_envs.utils.wrappers.rl_games import RlGamesGpuEnv, RlGamesVecEnvWrapper + + # configuration parameters + rl_device = "cuda:0" + clip_obs = 10.0 + clip_actions = 1.0 + + # wrap around environment for rl-games + env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions) + + # register the environment to rl-games registry + # note: in agents configuration: environment name must be "rlgpu" + vecenv.register( + "IsaacRlgWrapper", lambda config_name, num_actors, **kwargs: RlGamesGpuEnv(config_name, num_actors, **kwargs) + ) + env_configurations.register("rlgpu", {"vecenv_type": "IsaacRlgWrapper", "env_creator": lambda **kwargs: env}) + +""" + + +import gym +import torch +from typing import Dict, Union + +from rl_games.common import env_configurations +from rl_games.common.vecenv import IVecEnv + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvObs + +__all__ = ["RlGamesVecEnvWrapper", "RlGamesGpuEnv"] + + +""" +Vectorized environment wrapper. +""" + + +class RlGamesVecEnvWrapper(gym.Wrapper): + """Wraps around IsaacSim environment for RL-Games. + + This class wraps around the IsaacSim environment. Since RL-Games works directly on + GPU buffers, the wrapper handles moving of buffers from the simulation environment + to the same device as the learning agent. Additionally, it performs clipping of + observations and actions. + + For algorithms like asymmetric actor-critic, RL-Games expects a dictionary for + observations. This dictionary contains "obs" and "states" which typically correspond + to the actor and critic observations respectively. + + To use asymmetric actor-critic, the environment instance must have the attributes + :attr:`num_states` (int) and :attr:`state_space` (:obj:`gym.spaces.Box`). These are + used by the learning agent to allocate buffers in the trajectory memory. Additionally, + the method :meth:`_get_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. + + Reference: + https://github.com/Denys88/rl_games/blob/master/rl_games/common/ivecenv.py + https://github.com/NVIDIA-Omniverse/IsaacGymEnvs + """ + + def __init__(self, env: IsaacEnv, rl_device: str, clip_obs: float, clip_actions: float): + """Initializes the wrapper instance. + + Args: + env (IsaacEnv): The environment to wrap around. + rl_device (str): The device on which agent computations are performed. + clip_obs (float): The clipping value for observations. + clip_actions (float): The clipping value for actions. + + Raises: + ValueError: The environment is not inherited from :class:`IsaacEnv`. + """ + # check that input is valid + if not isinstance(env.unwrapped, IsaacEnv): + raise ValueError(f"The environment must be inherited from IsaacEnv. Environment type: {type(env)}") + # initialize gym wrapper + gym.Wrapper.__init__(self, env) + # initialize rl-games vec-env + IVecEnv.__init__(self) + # store provided arguments + self._rl_device = rl_device + self._clip_obs = clip_obs + self._clip_actions = clip_actions + # information about spaces for the wrapper + self.observation_space = self.env.observation_space + self.action_space = self.env.action_space + # information for privileged observations + self.state_space = getattr(self.env, "state_space", None) + self.num_states = getattr(self.env, "num_states", 0) + # print information about wrapper + print("[INFO]: RL-Games Environment Wrapper:") + print(f"\t\t Observations clipping: {clip_obs}") + print(f"\t\t Actions clipping : {clip_actions}") + print(f"\t\t Agent device : {rl_device}") + print(f"\t\t Asymmetric-learning : {self.num_states != 0}") + + """ + Properties + """ + + def get_number_of_agents(self) -> int: + """Returns number of actors in the environment.""" + return getattr(self, "num_agents", 1) + + def get_env_info(self) -> dict: + """Returns the Gym spaces for the environment.""" + # fill the env info dict + env_info = {"observation_space": self.observation_space, "action_space": self.action_space} + # add information about privileged observations space + if self.num_states > 0: + env_info["state_space"] = self.state_space + + return env_info + + """ + Operations - MDP + """ + + def reset(self): + obs_dict = self.env.reset() + # process observations and states + return self._process_obs(obs_dict) + + def step(self, actions): + # clip the actions + actions = torch.clamp(actions.clone(), -self._clip_actions, self._clip_actions) + # perform environment step + obs_dict, rew, dones, extras = self.env.step(actions) + # process observations and states + obs_and_states = self._process_obs(obs_dict) + # move buffers to rl-device + # note: we perform clone to prevent issues when rl-device and sim-device are the same. + rew = rew.to(self._rl_device) + dones = dones.to(self._rl_device) + extras = { + k: v.to(device=self._rl_device, non_blocking=True) if hasattr(v, "to") else v for k, v in extras.items() + } + + return obs_and_states, rew, dones, extras + + """ + Helper functions + """ + + def _process_obs(self, obs_dict: VecEnvObs) -> Union[torch.Tensor, Dict[str, torch.Tensor]]: + """Processing of the observations and states from the environment. + + Note: + States typically refers to privileged observations for the critic function. It is typically used in + asymmetric actor-critic algorithms [1]. + + Args: + obs (VecEnvObs): The current observations from environment. + + Returns: + Union[torch.Tensor, Dict[str, torch.Tensor]]: If environment provides states, then a dictionary + containing the observations and states is returned. Otherwise just the observations tensor + is returned. + + Reference: + 1. Pinto, Lerrel, et al. "Asymmetric actor critic for image-based robot learning." + arXiv preprint arXiv:1710.06542 (2017). + """ + # process policy obs + obs = obs_dict["policy"] + # clip the observations + obs = torch.clamp(obs, -self._clip_obs, self._clip_obs) + # move the buffer to rl-device + obs = obs.to(self._rl_device).clone() + + # check if asymmetric actor-critic or not + if self.num_states > 0: + # acquire states from the environment if it exists + try: + states = obs_dict["critic"] + except AttributeError: + raise NotImplementedError("Environment does not define key `critic` for privileged observations.") + # clip the states + states = torch.clamp(states, -self._clip_obs, self._clip_obs) + # move buffers to rl-device + states = states.to(self._rl_device).clone() + # convert to dictionary + return {"obs": obs, "states": states} + else: + return obs + + +""" +Environment Handler. +""" + + +class RlGamesGpuEnv(IVecEnv): + """Thin wrapper to create instance of the environment to fit RL-Games runner.""" + + # TODO: Adding this for now but do we really need this? + + def __init__(self, config_name: str, num_actors: int, **kwargs): + self.env: RlGamesVecEnvWrapper = env_configurations.configurations[config_name]["env_creator"](**kwargs) + + def step(self, action): + return self.env.step(action) + + def reset(self): + return self.env.reset() + + def get_number_of_agents(self): + return self.env.get_number_of_agents() + + def get_env_info(self): + return self.env.get_env_info() diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/rsl_rl.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/rsl_rl.py new file mode 100644 index 0000000000..e91ad4932e --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/rsl_rl.py @@ -0,0 +1,280 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapper to configure an :class:`IsaacEnv` instance to RSL-RL vectorized environment. + +The following example shows how to wrap an environment for RSL-RL: + +.. code-block:: python + + from omni.isaac.orbit_envs.utils.wrappers.rsl_rl import RslRlVecEnvWrapper + + env = RslRlVecEnvWrapper(env) + +""" + + +import copy +import gym +import gym.spaces +import os +import torch +from typing import Dict, Optional, Tuple + +# rsl-rl +from rsl_rl.env.vec_env import VecEnv + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv + +__all__ = ["RslRlVecEnvWrapper", "export_policy_as_jit", "export_policy_as_onnx"] + + +""" +Vectorized environment wrapper. +""" + +# VecEnvObs is what is returned by the reset() method +# it contains the observation for each env +VecEnvObs = Tuple[torch.Tensor, Optional[torch.Tensor]] +# VecEnvStepReturn is what is returned by the step() method +# it contains the observation (actor and critic), reward, done, info for each env +VecEnvStepReturn = Tuple[VecEnvObs, VecEnvObs, torch.Tensor, torch.Tensor, Dict] + + +class RslRlVecEnvWrapper(gym.Wrapper, VecEnv): + """Wraps around IsaacSim environment for RSL-RL. + + To use asymmetric actor-critic, the environment instance must have the attributes :attr:`num_states` (int) + and :attr:`state_space` (:obj:`gym.spaces.Box`). These are used by the learning agent to allocate buffers in + the trajectory memory. Additionally, the method :meth:`_get_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. + + Reference: + https://github.com/leggedrobotics/rsl_rl/blob/master/rsl_rl/env/vec_env.py + """ + + def __init__(self, env: IsaacEnv): + # check that input is valid + if not isinstance(env.unwrapped, IsaacEnv): + raise ValueError(f"The environment must be inherited from IsaacEnv. Environment type: {type(env)}") + # initialize the wrapper + gym.Wrapper.__init__(self, env) + # check that environment only provides flatted obs + if not isinstance(env.observation_space, gym.spaces.Box): + raise ValueError( + f"RSL-RL only supports flattened observation spaces. Input observation space: {env.observation_space}" + ) + # store information required by wrapper + self.num_envs = self.env.unwrapped.num_envs + self.num_actions = self.env.action_space.shape[0] + self.num_obs = self.env.observation_space.shape[0] + # information for privileged observations + self.privileged_obs_space = getattr(self.env, "state_space", None) + self.num_privileged_obs = getattr(self.env, "num_states", None) + + """ + Properties + """ + + def get_observations(self) -> torch.Tensor: + """Returns the current observations of the environment.""" + return self.env.unwrapped._get_observations()["policy"] + + def get_privileged_observations(self) -> Optional[torch.Tensor]: + """Returns the current privileged observations of the environment (if available).""" + if self.num_privileged_obs is not None: + try: + privileged_obs = self.env.unwrapped._get_observations()["critic"] + except AttributeError: + raise NotImplementedError("Environment does not define the key `critic` for privileged observations.") + else: + privileged_obs = None + + return privileged_obs + + """ + Operations - MDP + """ + + def reset(self) -> VecEnvObs: + # reset the environment + obs_dict = self.env.reset() + # return observations + return self._process_obs(obs_dict) + + def step(self, actions: torch.Tensor) -> VecEnvStepReturn: + # record step information + obs_dict, rew, dones, extras = self.env.step(actions) + # process observations + obs, privileged_obs = self._process_obs(obs_dict) + # return step information + return obs, privileged_obs, rew, dones, extras + + """ + Helper functions + """ + + def _process_obs(self, obs_dict: dict) -> VecEnvObs: + """Processing of the observations from the environment. + + Args: + obs (dict): The current observations from environment. + + Returns: + Tuple[torch.Tensor, Optional[torch.Tensor]]: The observations for actor and critic. If no + privileged observations are available then the critic observations are set to :obj:`None`. + """ + # process policy obs + obs = obs_dict["policy"] + # process critic observations + # note: if None then policy observations are used + if self.num_privileged_obs is not None: + try: + privileged_obs = obs_dict["critic"] + except AttributeError: + raise NotImplementedError("Environment does not define the key `critic` for privileged observations.") + else: + privileged_obs = None + # return observations + return obs, privileged_obs + + +""" +Helper functions. +""" + + +def export_policy_as_jit(actor_critic: object, path: str, filename="policy.pt"): + """Export policy into a Torch JIT file. + + Args: + actor_critic (object): The actor-critic torch module. + path (str): The path to the saving directory. + filename (str, optional): The name of exported JIT file. Defaults to "policy.pt". + + Reference: + https://github.com/leggedrobotics/legged_gym/blob/master/legged_gym/utils/helpers.py#L180 + """ + policy_exporter = _TorchPolicyExporter(actor_critic) + policy_exporter.export(path, filename) + + +def export_policy_as_onnx(actor_critic: object, path: str, filename="policy.onnx"): + """Export policy into a Torch ONNX file. + + Args: + actor_critic (object): The actor-critic torch module. + path (str): The path to the saving directory. + filename (str, optional): The name of exported JIT file. Defaults to "policy.pt". + """ + if not os.path.exists(path): + os.makedirs(path, exist_ok=True) + policy_exporter = _OnnxPolicyExporter(actor_critic) + policy_exporter.export(path, filename) + + +""" +Helper Classes - Private. +""" + + +class _TorchPolicyExporter(torch.nn.Module): + """Exporter of actor-critic into JIT file. + + Reference: + https://github.com/leggedrobotics/legged_gym/blob/master/legged_gym/utils/helpers.py#L193 + """ + + def __init__(self, actor_critic): + super().__init__() + self.actor = copy.deepcopy(actor_critic.actor) + self.is_recurrent = actor_critic.is_recurrent + if self.is_recurrent: + self.rnn = copy.deepcopy(actor_critic.memory_a.rnn) + self.rnn.cpu() + self.register_buffer("hidden_state", torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size)) + self.register_buffer("cell_state", torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size)) + self.forward = self.forward_lstm + self.reset = self.reset_memory + + def forward_lstm(self, x): + x, (h, c) = self.rnn(x.unsqueeze(0), (self.hidden_state, self.cell_state)) + self.hidden_state[:] = h + self.cell_state[:] = c + x = x.squeeze(0) + return self.actor(x) + + def forward(self, x): + return self.actor(x) + + @torch.jit.export + def reset(self): + pass + + def reset_memory(self): + self.hidden_state[:] = 0.0 + self.cell_state[:] = 0.0 + + def export(self, path, filename): + os.makedirs(path, exist_ok=True) + path = os.path.join(path, filename) + self.to("cpu") + traced_script_module = torch.jit.script(self) + traced_script_module.save(path) + + +class _OnnxPolicyExporter(torch.nn.Module): + """Exporter of actor-critic into ONNX file.""" + + def __init__(self, actor_critic): + super().__init__() + self.actor = copy.deepcopy(actor_critic.actor) + self.is_recurrent = actor_critic.is_recurrent + if self.is_recurrent: + self.rnn = copy.deepcopy(actor_critic.memory_a.rnn) + self.rnn.cpu() + self.forward = self.forward_lstm + + def forward_lstm(self, x_in, h_in, c_in): + x, (h, c) = self.rnn(x_in.unsqueeze(0), (h_in, c_in)) + x = x.squeeze(0) + return self.actor(x), h, c + + def forward(self, x): + return self.actor(x) + + def export(self, path, filename): + self.to("cpu") + if self.is_recurrent: + obs = torch.zeros(1, self.rnn.input_size) + h_in = torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size) + c_in = torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size) + actions, h_out, c_out = self(obs, h_in, c_in) + torch.onnx.export( + self, + (obs, h_in, c_in), + os.path.join(path, filename), + export_params=True, + opset_version=11, + verbose=True, + input_names=["obs", "h_in", "c_in"], + output_names=["actions", "h_out", "c_out"], + dynamic_axes={}, + ) + else: + obs = torch.zeros(1, self.actor[0].in_features) + torch.onnx.export( + self, + obs, + os.path.join(path, filename), + export_params=True, + opset_version=11, + verbose=True, + input_names=["obs"], + output_names=["actions"], + dynamic_axes={}, + ) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/sb3.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/sb3.py new file mode 100644 index 0000000000..916824d142 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/utils/wrappers/sb3.py @@ -0,0 +1,217 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapper to configure an :class:`IsaacEnv` instance to Stable-Baselines3 vectorized environment. + +The following example shows how to wrap an environment for Stable-Baselines3: + +.. code-block:: python + + from omni.isaac.orbit_envs.utils.wrappers.sb3 import Sb3VecEnvWrapper + + env = Sb3VecEnvWrapper(env) + +""" + + +import gym +import numpy as np +import torch +from typing import Any, Dict, List + +# stable-baselines3 +from stable_baselines3.common.vec_env.base_vec_env import VecEnv, VecEnvObs, VecEnvStepReturn + +from omni.isaac.orbit_envs.isaac_env import IsaacEnv + +__all__ = ["Sb3VecEnvWrapper"] + + +""" +Vectorized environment wrapper. +""" + + +class Sb3VecEnvWrapper(gym.Wrapper, VecEnv): + """Wraps around IsaacSim environment for Stable Baselines3. + + Isaac Sim internally implements a vectorized environment. However, since it is + still considered a single environment instance, Stable Baselines tries to wrap + around it using the :class:`DummyVecEnv`. This is only done if the environment + is not inheriting from their :class:`VecEnv`. Thus, this class thinly wraps + over the environment from :class:`IsaacEnv`. + + We also add monitoring functionality that computes the un-discounted episode + return and length. This information is added to the info dicts under key `episode`. + + In contrast to Isaac Sim environment, stable-baselines expect the following: + + 1. numpy datatype for MDP signals + 2. a list of info dicts for each sub-environment (instead of a dict) + 3. when environment has terminated, the observations from the environment should correspond + to the one after reset. The "real" final observation is passed using the info dicts + under the key `terminal_observation`. + + Warning: + By the nature of physics stepping in Isaac Sim, it is not possible to forward the + simulation buffers without performing a physics step. Thus, reset is performed only + at the start of :meth:`step()` function before the actual physics step is taken. + Thus, the returned observations for terminated environments is still the final + observation and not the ones after the reset. + + Reference: + https://stable-baselines3.readthedocs.io/en/master/guide/vec_envs.html + https://stable-baselines3.readthedocs.io/en/master/common/monitor.html + """ + + def __init__(self, env: IsaacEnv): + # check that input is valid + if not isinstance(env.unwrapped, IsaacEnv): + raise ValueError(f"The environment must be inherited from IsaacEnv. Environment type: {type(env)}") + # initialize the wrapper + gym.Wrapper.__init__(self, env) + # initialize vec-env + VecEnv.__init__(self, self.env.num_envs, self.env.observation_space, self.env.action_space) + # add buffer for logging episodic information + self._ep_rew_buf = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.env.device) + self._ep_len_buf = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.env.device) + + """ + Properties + """ + + def get_episode_rewards(self) -> List[float]: + """Returns the rewards of all the episodes.""" + return self._ep_rew_buf.cpu().tolist() + + def get_episode_lengths(self) -> List[int]: + """Returns the number of time-steps of all the episodes.""" + return self._ep_len_buf.cpu().tolist() + + """ + Operations - MDP + """ + + def reset(self) -> VecEnvObs: + obs_dict = self.env.reset() + # convert data types to numpy depending on backend + return self._process_obs(obs_dict) + + def step(self, actions: np.ndarray) -> VecEnvStepReturn: + # convert input to numpy array + actions = np.asarray(actions) + # convert to tensor + actions = torch.from_numpy(actions).to(device=self.env.device) + # record step information + obs_dict, rew, dones, extras = self.env.step(actions) + + # update episode un-discounted return and length + self._ep_rew_buf += rew + self._ep_len_buf += 1 + reset_ids = (dones > 0).nonzero(as_tuple=False) + + # convert data types to numpy depending on backend + # Note: IsaacEnv uses torch backend (by default). + obs = self._process_obs(obs_dict) + rew = rew.cpu().numpy() + dones = dones.cpu().numpy() + # convert extra information to list of dicts + infos = self._process_extras(obs, dones, extras, reset_ids) + + # reset info for terminated environments + self._ep_rew_buf[reset_ids] = 0 + self._ep_len_buf[reset_ids] = 0 + + return obs, rew, dones, infos + + """ + Unused methods. + """ + + def step_async(self, actions): + self._async_actions = actions + + def step_wait(self): + return self.step(self._async_actions) + + def get_attr(self, attr_name, indices): + raise NotImplementedError + + def set_attr(self, attr_name, value, indices=None): + raise NotImplementedError + + def env_method(self, method_name: str, *method_args, indices=None, **method_kwargs): + raise NotImplementedError + + def env_is_wrapped(self, wrapper_class, indices=None): + raise NotImplementedError + + def get_images(self): + raise NotImplementedError + + """ + Helper functions. + """ + + def _process_obs(self, obs_dict) -> np.ndarray: + """Convert observations into NumPy data type.""" + # Sb3 doesn't support asymmetric observation spaces, so we only use "policy" + obs = obs_dict["policy"] + # Note: IsaacEnv uses torch backend (by default). + if self.env.sim.backend == "torch": + if isinstance(obs, dict): + for key, value in obs.items(): + obs[key] = value.detach().cpu().numpy() + else: + obs = obs.detach().cpu().numpy() + elif self.env.sim.backend == "numpy": + pass + else: + raise NotImplementedError(f"Unsupported backend for simulation: {self.env.sim.backend}") + return obs + + def _process_extras(self, obs, dones, extras, reset_ids) -> List[Dict[str, Any]]: + """Convert miscellaneous information into dictionary for each sub-environment.""" + # create empty list of dictionaries to fill + infos: List[Dict[str, Any]] = [dict.fromkeys(extras.keys()) for _ in range(self.env.num_envs)] + # fill-in information for each sub-environment + # Note: This loop becomes slow when number of environments is large. + for idx in range(self.env.num_envs): + # fill-in episode monitoring info + if idx in reset_ids: + infos[idx]["episode"] = dict() + infos[idx]["episode"]["r"] = float(self._ep_rew_buf[idx]) + infos[idx]["episode"]["l"] = float(self._ep_len_buf[idx]) + else: + infos[idx]["episode"] = None + # fill-in information from extras + for key, value in extras.items(): + # 1. remap the key for time-outs for what SB3 expects + # 2. remap extra episodes information safely + # 3. for others just store their values + if key == "time_outs": + infos[idx]["TimeLimit.truncated"] = bool(value[idx]) + elif key == "episode": + # only log this data for episodes that are terminated + if infos[idx]["episode"] is not None: + for sub_key, sub_value in value.items(): + infos[idx]["episode"][sub_key] = sub_value + else: + infos[idx][key] = value[idx] + # add information about terminal observation separately + if dones[idx] == 1: + # extract terminal observations + if isinstance(obs, dict): + terminal_obs = dict.fromkeys(obs.keys()) + for key, value in obs.items(): + terminal_obs[key] = value[idx] + else: + terminal_obs = obs[idx] + # add info to dict + infos[idx]["terminal_observation"] = terminal_obs + else: + infos[idx]["terminal_observation"] = None + # return list of dictionaries + return infos diff --git a/source/extensions/omni.isaac.orbit_envs/setup.py b/source/extensions/omni.isaac.orbit_envs/setup.py new file mode 100644 index 0000000000..029a7f5878 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/setup.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Installation script for the 'omni.isaac.orbit_envs' python package.""" + + +import itertools + +from setuptools import setup + +# Minimum dependencies required prior to installation +INSTALL_REQUIRES = [ + # generic + "numpy", + "torch", + "protobuf==3.20.2", + # gym + "gym==0.21.0", + "importlib-metadata~=4.13.0", + # data collection + "h5py", +] + +# Extra dependencies for RL agents +EXTRAS_REQUIRE = { + "sb3": ["stable-baselines3>=1.5.0", "tensorboard"], + "rl_games": ["rl-games==1.5.2"], + "rsl_rl": ["rsl_rl@git+https://github.com/leggedrobotics/rsl_rl.git"], + "robomimic": ["robomimic@git+https://github.com/ARISE-Initiative/robomimic.git"], +} +# cumulation of all extra-requires +EXTRAS_REQUIRE["all"] = list(itertools.chain.from_iterable(EXTRAS_REQUIRE.values())) + + +# Installation operation +setup( + name="omni-isaac-orbit_envs", + author="NVIDIA, ETH Zurich, and University of Toronto", + maintainer="Mayank Mittal", + maintainer_email="mittalma@ethz.ch", + url="https://github.com/NVIDIA-Omniverse/orbit", + version="0.1.0", + description="Python module for robotic environments built using ORBIT.", + keywords=["robotics", "rl"], + include_package_data=True, + python_requires=">=3.7.*", + install_requires=INSTALL_REQUIRES, + extras_require=EXTRAS_REQUIRE, + packages=["omni.isaac.orbit_envs"], + classifiers=["Natural Language :: English", "Programming Language :: Python :: 3.7"], + zip_safe=False, +) diff --git a/source/extensions/omni.isaac.orbit_envs/test/test_data_collector.py b/source/extensions/omni.isaac.orbit_envs/test/test_data_collector.py new file mode 100644 index 0000000000..c5f561fcd0 --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/test/test_data_collector.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import torch +import unittest + +from omni.isaac.orbit_envs.utils.data_collector import RobomimicDataCollector + + +class TestRobomimicDataCollector(unittest.TestCase): + """Test dataset flushing behavior of robomimic data collector.""" + + def test_basic_flushing(self): + """Adds random data into the collector and checks saving of the data.""" + # name of the environment (needed by robomimic) + task_name = "Isaac-Franka-Lift-v0" + # specify directory for logging experiments + test_dir = os.path.dirname(os.path.abspath(__file__)) + log_dir = os.path.join(test_dir, "logs", "demos") + # name of the file to save data + filename = "hdf_dataset.hdf5" + # number of episodes to collect + num_demos = 10 + # number of environments to simulate + num_envs = 4 + + # create data-collector + collector_interface = RobomimicDataCollector(task_name, log_dir, filename, num_demos) + + # reset the collector + collector_interface.reset() + + while not collector_interface.is_stopped(): + # generate random data to store + # -- obs + obs = {"joint_pos": torch.randn(num_envs, 7), "joint_vel": torch.randn(num_envs, 7)} + # -- actions + actions = torch.randn(num_envs, 7) + # -- next obs + next_obs = {"joint_pos": torch.randn(num_envs, 7), "joint_vel": torch.randn(num_envs, 7)} + # -- rewards + rewards = torch.randn(num_envs) + # -- dones + dones = torch.rand(num_envs) > 0.5 + + # store signals + # -- obs + for key, value in obs.items(): + collector_interface.add(f"obs/{key}", value) + # -- actions + collector_interface.add("actions", actions) + # -- next_obs + for key, value in next_obs.items(): + collector_interface.add(f"next_obs/{key}", value.cpu().numpy()) + # -- rewards + collector_interface.add("rewards", rewards) + # -- dones + collector_interface.add("dones", dones) + + # flush data from collector for successful environments + # note: in this case we flush all the time + reset_env_ids = dones.nonzero(as_tuple=False).squeeze(-1) + collector_interface.flush(reset_env_ids) + + # close collector + collector_interface.close() + # TODO: Add inspection of the saved dataset as part of the test. + + +if __name__ == "__main__": + unittest.main() diff --git a/source/extensions/omni.isaac.orbit_envs/test/test_environments.py b/source/extensions/omni.isaac.orbit_envs/test/test_environments.py new file mode 100644 index 0000000000..7ed06ce8fd --- /dev/null +++ b/source/extensions/omni.isaac.orbit_envs/test/test_environments.py @@ -0,0 +1,108 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + + +from omni.isaac.kit import SimulationApp + +# launch the simulator +config = {"headless": True} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import gym.envs +import torch +import unittest +from typing import Dict, Union + +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils.parse_cfg import parse_env_cfg + + +class TestEnvironments(unittest.TestCase): + """Test cases for all registered environments.""" + + @classmethod + def tearDownClass(cls): + """Closes simulator after running all test fixtures.""" + simulation_app.close() + + def setUp(self) -> None: + self.use_gpu = False + self.num_envs = 20 + self.headless = simulation_app.config["headless"] + # acquire all Isaac environments names + self.registered_tasks = list() + for task_spec in gym.envs.registry.all(): + if "Isaac" in task_spec.id: + self.registered_tasks.append(task_spec.id) + # print all existing task names + print(">>> All registered environments:", self.registered_tasks) + + def test_random_actions(self): + """Run random actions and check environments return valid signals.""" + + for task_name in self.registered_tasks: + print(f">>> Running test for environment: {task_name}") + # parse configuration + env_cfg = parse_env_cfg(task_name, use_gpu=self.use_gpu, num_envs=self.num_envs) + # create environment + env = gym.make(task_name, cfg=env_cfg, headless=self.headless) + + # reset environment + obs = env.reset() + # check signal + self.assertTrue(self._check_valid_tensor(obs)) + + # simulate environment for 1000 steps + for _ in range(1000): + # sample actions from -1 to 1 + actions = 2 * torch.rand((env.num_envs, env.action_space.shape[0]), device=env.device) - 1 + # apply actions + obs, rew, dones, info = env.step(actions) + # check signals + self.assertTrue(self._check_valid_tensor(obs)) + self.assertTrue(self._check_valid_tensor(rew)) + self.assertTrue(self._check_valid_tensor(dones)) + self.assertTrue(self._check_valid_tensor(info)) + + # close the environment + print(f">>> Closing environment: {task_name}") + env.close() + + """ + Helper functions. + """ + + @staticmethod + def _check_valid_tensor(data: Union[torch.Tensor, Dict]) -> bool: + """Checks if given data does not have corrupted values. + + Args: + data (Union[torch.Tensor, Dict]): Data buffer. + + Returns: + bool: 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): + return TestEnvironments._check_valid_tensor(value) + else: + valid_tensor = valid_tensor and not torch.any(torch.isnan(value)) + return valid_tensor + else: + raise ValueError(f"Input data of invalid type: {type(data)}.") + + +if __name__ == "__main__": + unittest.main() diff --git a/source/standalone/demo/play_arms.py b/source/standalone/demo/play_arms.py new file mode 100644 index 0000000000..e1138cf48a --- /dev/null +++ b/source/standalone/demo/play_arms.py @@ -0,0 +1,161 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script demonstrates how to use the physics engine to simulate a single-arm manipulator. + +We currently support the following robots: + +* Franka Emika Panda +* Universal Robot UR10 + +From the default configuration file for these robots, zero actions imply a default pose. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import torch + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG +from omni.isaac.orbit.robots.config.universal_robots import UR10_CFG +from omni.isaac.orbit.robots.single_arm import SingleArmManipulator +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + +""" +Main +""" + + +def main(): + """Spawns a single arm manipulator and applies random joint commands.""" + + # Load kit helper + sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01, backend="torch") + # Set main camera + set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + # Spawn things into stage + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane", z_position=-1.05) + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)}, + ) + # Table + table_usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" + prim_utils.create_prim("/World/Table_1", usd_path=table_usd_path, translation=(0.55, -1.0, 0.0)) + prim_utils.create_prim("/World/Table_2", usd_path=table_usd_path, translation=(0.55, 1.0, 0.0)) + # Robots + # -- Resolve robot config from command-line arguments + if args_cli.robot == "franka_panda": + robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG + elif args_cli.robot == "ur10": + robot_cfg = UR10_CFG + else: + raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") + # -- Spawn robot + robot = SingleArmManipulator(cfg=robot_cfg) + robot.spawn("/World/Robot_1", translation=(0.0, -1.0, 0.0)) + robot.spawn("/World/Robot_2", translation=(0.0, 1.0, 0.0)) + + # Play the simulator + sim.reset() + # Acquire handles + # Initialize handles + robot.initialize("/World/Robot.*") + # Reset states + robot.reset_buffers() + + # Now we are ready! + print("[INFO]: Setup complete...") + + # dummy actions + actions = torch.rand(robot.count, robot.num_actions, device=robot.device) + has_gripper = robot.cfg.meta_info.tool_num_dof > 0 + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # episode counter + sim_time = 0.0 + ep_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=not args_cli.headless) + continue + # reset + if ep_step_count % 1000 == 0: + sim_time = 0.0 + ep_step_count = 0 + # reset dof state + dof_pos, dof_vel = robot.get_default_dof_state() + robot.set_dof_state(dof_pos, dof_vel) + robot.reset_buffers() + # reset command + actions = torch.rand(robot.count, robot.num_actions, device=robot.device) + # reset gripper + if has_gripper: + actions[:, -1] = -1 + print("[INFO]: Resetting robots state...") + # change the gripper action + if ep_step_count % 200 and has_gripper: + # flip command for the gripper + actions[:, -1] = -actions[:, -1] + # apply action to the robot + robot.apply_action(actions) + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + ep_step_count += 1 + # note: to deal with timeline events such as stopping, we need to check if the simulation is playing + if sim.is_playing(): + # update buffers + robot.update_buffers(sim_dt) + + +if __name__ == "__main__": + # Run the main function + main() + # Close the simulator + simulation_app.close() diff --git a/source/standalone/demo/play_camera.py b/source/standalone/demo/play_camera.py new file mode 100644 index 0000000000..336e5bad48 --- /dev/null +++ b/source/standalone/demo/play_camera.py @@ -0,0 +1,177 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script shows how to use the camera sensor from the Orbit framework. + +The camera sensor is created and interfaced through the Omniverse Replicator API. However, instead of using +the simulator or OpenGL convention for the camera, we use the robotics or ROS convention. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import numpy as np +import os +import random + +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.debug_draw._debug_draw as omni_debug_draw +import omni.replicator.core as rep +from omni.isaac.core.prims import RigidPrim +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view +from pxr import Gf, UsdGeom + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.sensors.camera import Camera, PinholeCameraCfg +from omni.isaac.orbit.sensors.camera.utils import create_pointcloud_from_rgbd + +""" +Helpers +""" + + +def design_scene(): + """Add prims to the scene.""" + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane") + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)}, + ) + # Random objects + for i in range(8): + # 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_utils.create_prim( + f"/World/Objects/Obj_{i:02d}", + prim_type, + translation=position, + scale=(0.25, 0.25, 0.25), + semantic_label=prim_type, + ) + # add rigid properties + rigid_obj = RigidPrim(f"/World/Objects/Obj_{i:02d}", mass=5.0) + # cast to geom prim + geom_prim = getattr(UsdGeom, prim_type)(rigid_obj.prim) + # set random color + color = Gf.Vec3f(random.random(), random.random(), random.random()) + geom_prim.CreateDisplayColorAttr() + geom_prim.GetDisplayColorAttr().Set([color]) + + +""" +Main +""" + + +def main(): + """Runs a camera sensor from orbit.""" + + # Load kit helper + sim = SimulationContext(stage_units_in_meters=1.0, physics_dt=0.005, rendering_dt=0.005, backend="torch") + # Set main camera + set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + # Acquire draw interface + draw_interface = omni_debug_draw.acquire_debug_draw_interface() + + # Populate scene + design_scene() + # Setup camera sensor + camera_cfg = PinholeCameraCfg( + sensor_tick=0, + height=480, + width=640, + data_types=["rgb", "distance_to_image_plane", "normals", "motion_vectors"], + usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)), + ) + camera = Camera(cfg=camera_cfg, device="cpu") + camera.spawn("/World/CameraSensor") + camera.initialize() + + # Create replicator writer + output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "output", "camera") + rep_writer = rep.BasicWriter(output_dir=output_dir, frame_padding=3) + + # Play simulator + sim.play() + # Set pose + camera.set_world_pose_from_view(eye=[5.0, 5.0, 5.0], target=[0.0, 0.0, 0.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=not args_cli.headless) + continue + # Step simulation + sim.step() + # Update camera data + camera.update(dt=0.0) + print(camera) + print("Received shape of rgb image: ", camera.data.output["rgb"].shape) + print("Received shape of depth image: ", camera.data.output["distance_to_image_plane"].shape) + print("-------------------------------") + + # Save images + rep_writer.write(camera.data.output) + + # Pointcloud in world frame + pointcloud_w, pointcloud_rgb = create_pointcloud_from_rgbd( + camera.data.intrinsic_matrix, + depth=camera.data.output["distance_to_image_plane"], + rgb=camera.data.output["rgb"], + position=camera.data.position, + orientation=camera.data.orientation, + normalize_rgb=True, + num_channels=4, + ) + # visualize the points + num_points = pointcloud_w.shape[0] + points_size = [1.25] * num_points + points_color = pointcloud_rgb + draw_interface.clear_points() + draw_interface.draw_points(pointcloud_w.tolist(), points_color, points_size) + + +if __name__ == "__main__": + # Runs the main function + main() + # Close the simulator + simulation_app.close() diff --git a/source/standalone/demo/play_cloner.py b/source/standalone/demo/play_cloner.py new file mode 100644 index 0000000000..35ed7e71a8 --- /dev/null +++ b/source/standalone/demo/play_cloner.py @@ -0,0 +1,186 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script demonstrates how to use the cloner API from Isaac Sim. + +Reference: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_gym_cloner.html +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.") +parser.add_argument("--num_robots", type=int, default=128, help="Number of robots to spawn.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import torch + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.cloner import GridCloner +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.carb import set_carb_setting +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG +from omni.isaac.orbit.robots.config.universal_robots import UR10_CFG +from omni.isaac.orbit.robots.single_arm import SingleArmManipulator +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + +""" +Main +""" + + +def main(): + """Spawns the Franka robot and clones it using Isaac Gym Cloner API.""" + + # Load kit helper + sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01, backend="torch", device="cuda:0") + # Set main camera + set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + # Enable flatcache 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: + sim.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(sim._settings, "/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") + + # Spawn things into stage + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane", z_position=-1.05) + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)}, + ) + # -- Table + table_usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" + prim_utils.create_prim("/World/envs/env_0/Table", usd_path=table_usd_path) + # -- Robot + # resolve robot config from command-line arguments + if args_cli.robot == "franka_panda": + robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG + elif args_cli.robot == "ur10": + robot_cfg = UR10_CFG + else: + raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") + # spawn robot + robot = SingleArmManipulator(cfg=robot_cfg) + robot.spawn("/World/envs/env_0/Robot", translation=(0.0, 0.0, 0.0)) + + # Clone the scene + num_envs = args_cli.num_robots + cloner.define_base_env("/World/envs") + envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) + envs_positions = cloner.clone( + source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True + ) + # convert environment positions to torch tensor + envs_positions = torch.tensor(envs_positions, dtype=torch.float, device=sim.device) + # filter collisions within each environment instance + physics_scene_path = sim.get_physics_context().prim_path + cloner.filter_collisions( + physics_scene_path, "/World/collisions", envs_prim_paths, global_paths=["/World/defaultGroundPlane"] + ) + + # Play the simulator + sim.reset() + # Acquire handles + # Initialize handles + robot.initialize("/World/envs/env_.*/Robot") + # Reset states + robot.reset_buffers() + + # Now we are ready! + print("[INFO]: Setup complete...") + + # dummy actions + actions = torch.rand(robot.count, robot.num_actions, device=robot.device) + has_gripper = robot.cfg.meta_info.tool_num_dof > 0 + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # episode counter + sim_time = 0.0 + ep_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=not args_cli.headless) + continue + # reset + if ep_step_count % 100 == 0: + sim_time = 0.0 + ep_step_count = 0 + # reset dof state + dof_pos, dof_vel = robot.get_default_dof_state() + robot.set_dof_state(dof_pos, dof_vel) + robot.reset_buffers() + # reset command + actions = torch.rand(robot.count, robot.num_actions, device=robot.device) + # reset gripper + if has_gripper: + actions[:, -1] = -1 + print("[INFO]: Resetting robots state...") + # change the gripper action + if ep_step_count % 200 and has_gripper: + # flip command for the gripper + actions[:, -1] = -actions[:, -1] + # apply actions + robot.apply_action(actions) + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + ep_step_count += 1 + # note: to deal with timeline events such as stopping, we need to check if the simulation is playing + if sim.is_playing(): + # update buffers + robot.update_buffers(sim_dt) + + +if __name__ == "__main__": + # Run the main function + main() + # Close the simulator + simulation_app.close() diff --git a/source/standalone/demo/play_empty.py b/source/standalone/demo/play_empty.py new file mode 100644 index 0000000000..ea463b3155 --- /dev/null +++ b/source/standalone/demo/play_empty.py @@ -0,0 +1,87 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script demonstrates how to create a simple stage in Isaac Sim with lights and a ground plane.""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils + +""" +Main +""" + + +def main(): + """Spawns lights in the stage and sets the camera view.""" + + # Load kit helper + sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01, backend="torch") + # Set main camera + set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + # Spawn things into stage + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane") + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)}, + ) + + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + + # 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=not args_cli.headless) + continue + # perform step + sim.step() + + +if __name__ == "__main__": + # Run empty stage + main() + # Close the simulator + simulation_app.close() diff --git a/source/standalone/demo/play_ik_control.py b/source/standalone/demo/play_ik_control.py new file mode 100644 index 0000000000..aae0657e15 --- /dev/null +++ b/source/standalone/demo/play_ik_control.py @@ -0,0 +1,236 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script demonstrates how to use the inverse kinematics controller with the simulator. + +The differential IK controller can be configured in different modes. It uses the Jacobians computed by +PhysX. This helps perform parallelized computation of the inverse kinematics. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.") +parser.add_argument("--num_envs", type=int, default=128, help="Number of environments to spawn.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import torch + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.cloner import GridCloner +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.carb import set_carb_setting +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.controllers.differential_inverse_kinematics import ( + DifferentialInverseKinematics, + DifferentialInverseKinematicsCfg, +) +from omni.isaac.orbit.markers import StaticMarker +from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG +from omni.isaac.orbit.robots.config.universal_robots import UR10_CFG +from omni.isaac.orbit.robots.single_arm import SingleArmManipulator +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR + +""" +Main +""" + + +def main(): + """Spawns a single-arm manipulator and applies commands through inverse kinematics control.""" + + # Load kit helper + sim = SimulationContext( + stage_units_in_meters=1.0, physics_dt=0.01, rendering_dt=0.01, backend="torch", device="cuda:0" + ) + # Set main camera + set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + # Enable GPU pipeline and flatcache + if sim.get_physics_context().use_gpu_pipeline: + sim.get_physics_context().enable_flatcache(True) + # Enable hydra scene-graph instancing + set_carb_setting(sim._settings, "/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") + + # Spawn things into stage + # Markers + ee_marker = StaticMarker("/Visuals/ee_current", count=args_cli.num_envs, scale=(0.1, 0.1, 0.1)) + goal_marker = StaticMarker("/Visuals/ee_goal", count=args_cli.num_envs, scale=(0.1, 0.1, 0.1)) + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane", z_position=-1.05) + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)}, + ) + # -- Table + table_usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" + prim_utils.create_prim("/World/envs/env_0/Table", usd_path=table_usd_path) + # -- Robot + # resolve robot config from command-line arguments + if args_cli.robot == "franka_panda": + robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG + elif args_cli.robot == "ur10": + robot_cfg = UR10_CFG + else: + raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") + # configure robot settings to use IK controller + robot_cfg.data_info.enable_jacobian = True + robot_cfg.rigid_props.disable_gravity = True + # spawn robot + robot = SingleArmManipulator(cfg=robot_cfg) + robot.spawn("/World/envs/env_0/Robot", translation=(0.0, 0.0, 0.0)) + + # Clone the scene + num_envs = args_cli.num_envs + envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) + envs_positions = cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths) + # convert environment positions to torch tensor + envs_positions = torch.tensor(envs_positions, dtype=torch.float, device=sim.device) + # filter collisions within each environment instance + physics_scene_path = sim.get_physics_context().prim_path + cloner.filter_collisions( + physics_scene_path, "/World/collisions", envs_prim_paths, global_paths=["/World/defaultGroundPlane"] + ) + + # Create controller + # the controller takes as command type: {position/pose}_{abs/rel} + ik_control_cfg = DifferentialInverseKinematicsCfg( + command_type="pose_abs", + ik_method="dls", + position_offset=robot.cfg.ee_info.pos_offset, + rotation_offset=robot.cfg.ee_info.rot_offset, + ) + ik_controller = DifferentialInverseKinematics(ik_control_cfg, num_envs, sim.device) + + # Play the simulator + sim.reset() + # Acquire handles + # Initialize handles + robot.initialize("/World/envs/env_.*/Robot") + # Reset states + robot.reset_buffers() + ik_controller.reset_idx() + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Create buffers to store actions + ik_commands = torch.zeros(robot.count, ik_controller.num_actions, device=robot.device) + robot_actions = torch.ones(robot.count, robot.num_actions, device=robot.device) + + # Set end effector goals + # Define goals for the arm + ee_goals = [ + [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_goals = torch.tensor(ee_goals, device=sim.device) + # Track the given command + current_goal_idx = 0 + ik_commands[:] = ee_goals[current_goal_idx] + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # episode counter + sim_time = 0.0 + count = 0 + # Note: We need to update buffers before the first step for the controller. + robot.update_buffers(sim_dt) + + # Simulate physics + # 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=not args_cli.headless) + continue + # reset + if count % 150 == 0: + # reset time + count = 0 + sim_time = 0.0 + # reset dof state + dof_pos, dof_vel = robot.get_default_dof_state() + robot.set_dof_state(dof_pos, dof_vel) + robot.reset_buffers() + # reset controller + ik_controller.reset_idx() + # reset actions + ik_commands[:] = ee_goals[current_goal_idx] + # change goal + current_goal_idx = (current_goal_idx + 1) % len(ee_goals) + # set the controller commands + ik_controller.set_command(ik_commands) + # compute the joint commands + robot_actions[:, : robot.arm_num_dof] = ik_controller.compute( + robot.data.ee_state_w[:, 0:3] - envs_positions, + robot.data.ee_state_w[:, 3:7], + robot.data.ee_jacobian, + robot.data.arm_dof_pos, + ) + # in some cases the zero action correspond to offset in actuators + # so we need to subtract these over here so that they can be added later on + arm_command_offset = robot.data.actuator_pos_offset[:, : robot.arm_num_dof] + # offset actuator command with position offsets + # note: valid only when doing position control of the robot + robot_actions[:, : robot.arm_num_dof] -= arm_command_offset + # apply actions + robot.apply_action(robot_actions) + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + count += 1 + # note: to deal with timeline events such as stopping, we need to check if the simulation is playing + if sim.is_playing(): + # update buffers + robot.update_buffers(sim_dt) + # update marker positions + ee_marker.set_world_poses(robot.data.ee_state_w[:, 0:3], robot.data.ee_state_w[:, 3:7]) + goal_marker.set_world_poses(ik_commands[:, 0:3] + envs_positions, ik_commands[:, 3:7]) + + +if __name__ == "__main__": + # Run IK example with Manipulator + main() + # Close the simulator + simulation_app.close() diff --git a/source/standalone/demo/play_quadrupeds.py b/source/standalone/demo/play_quadrupeds.py new file mode 100644 index 0000000000..587a96eba6 --- /dev/null +++ b/source/standalone/demo/play_quadrupeds.py @@ -0,0 +1,202 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script demonstrates how to use the physics engine to simulate a legged robot. + +We currently support the following robots: + +* ANYmal-B (from ANYbotics) +* ANYmal-C (from ANYbotics) +* A1 (from Unitree Robotics) + +From the default configuration file for these robots, zero actions imply a standing pose. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import torch +from typing import List + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.markers import PointMarker, StaticMarker +from omni.isaac.orbit.robots.config.anymal import ANYMAL_B_CFG, ANYMAL_C_CFG +from omni.isaac.orbit.robots.config.unitree import UNITREE_A1_CFG +from omni.isaac.orbit.robots.legged_robot import LeggedRobot + +""" +Helpers +""" + + +def design_scene(): + """Add prims to the scene.""" + # Ground-plane + kit_utils.create_ground_plane( + "/World/defaultGroundPlane", + static_friction=0.5, + dynamic_friction=0.5, + restitution=0.8, + improve_patch_friction=True, + ) + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)}, + ) + + +""" +Main +""" + + +def main(): + """Imports all legged robots supported in Orbit and applies zero actions.""" + + # Load kit helper + sim = SimulationContext(stage_units_in_meters=1.0, physics_dt=0.005, rendering_dt=0.005, backend="torch") + # Set main camera + set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + + # Spawn things into stage + # -- anymal-b + robot_b = LeggedRobot(cfg=ANYMAL_B_CFG) + robot_b.spawn("/World/Anymal_b/Robot_1", translation=(0.0, -1.5, 0.65)) + robot_b.spawn("/World/Anymal_b/Robot_2", translation=(0.0, -0.5, 0.65)) + # -- anymal-c + robot_c = LeggedRobot(cfg=ANYMAL_C_CFG) + robot_c.spawn("/World/Anymal_c/Robot_1", translation=(1.5, -1.5, 0.65)) + robot_c.spawn("/World/Anymal_c/Robot_2", translation=(1.5, -0.5, 0.65)) + # -- unitree a1 + robot_a = LeggedRobot(cfg=UNITREE_A1_CFG) + robot_a.spawn("/World/Unitree_A1/Robot_1", translation=(1.5, 0.5, 0.42)) + robot_a.spawn("/World/Unitree_A1/Robot_2", translation=(1.5, 1.5, 0.42)) + # design props + design_scene() + + # Play the simulator + sim.reset() + # Acquire handles + # Initialize handles + robot_b.initialize("/World/Anymal_b/Robot.*") + robot_c.initialize("/World/Anymal_c/Robot.*") + robot_a.initialize("/World/Unitree_A1/Robot.*") + # Reset states + robot_b.reset_buffers() + robot_c.reset_buffers() + robot_a.reset_buffers() + + # Debug visualization markers. + # -- feet markers + feet_markers: List[StaticMarker] = list() + feet_contact_markers: List[PointMarker] = list() + # iterate over robots + for robot_name in ["Anymal_b", "Anymal_c", "Unitree_A1"]: + # foot + marker = StaticMarker(f"/World/Visuals/{robot_name}/feet", 4 * robot_c.count, scale=(0.1, 0.1, 0.1)) + feet_markers.append(marker) + # contact + marker = PointMarker(f"/World/Visuals/{robot_name}/feet_contact", 4 * robot_c.count, radius=0.035) + feet_contact_markers.append(marker) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # dummy action + actions = torch.zeros(robot_a.count, robot_a.num_actions, device=robot_a.device) + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + sim_time = 0.0 + 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=not args_cli.headless) + continue + # reset + if count % 1000 == 0: + # reset counters + sim_time = 0.0 + count = 0 + # reset dof state + for robot in [robot_a, robot_b, robot_c]: + dof_pos, dof_vel = robot.get_default_dof_state() + robot.set_dof_state(dof_pos, dof_vel) + robot.reset_buffers() + # reset command + actions = torch.zeros(robot_a.count, robot_a.num_actions, device=robot_a.device) + print(">>>>>>>> Reset!") + # apply actions + robot_b.apply_action(actions) + robot_c.apply_action(actions) + robot_a.apply_action(actions) + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + count += 1 + # note: to deal with timeline events such as stopping, we need to check if the simulation is playing + if sim.is_playing(): + # update buffers + robot_b.update_buffers(sim_dt) + robot_c.update_buffers(sim_dt) + robot_a.update_buffers(sim_dt) + # update marker positions + for foot_marker, contact_marker, robot in zip( + feet_markers, feet_contact_markers, [robot_b, robot_c, robot_a] + ): + # feet + foot_marker.set_world_poses( + robot.data.feet_state_w[..., 0:3].view(-1, 3), robot.data.feet_state_w[..., 3:7].view(-1, 4) + ) + # contact sensors + contact_marker.set_world_poses( + robot.data.feet_state_w[..., 0:3].view(-1, 3), robot.data.feet_state_w[..., 3:7].view(-1, 4) + ) + contact_marker.set_status(torch.where(robot.data.feet_air_time.view(-1) > 0.0, 1, 2)) + + +if __name__ == "__main__": + # Run the main function + main() + # Close the simulator + simulation_app.close() diff --git a/source/standalone/demo/play_ridgeback_franka.py b/source/standalone/demo/play_ridgeback_franka.py new file mode 100644 index 0000000000..8ef37116fb --- /dev/null +++ b/source/standalone/demo/play_ridgeback_franka.py @@ -0,0 +1,184 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script demonstrates how to use the physics engine to simulate a mobile manipulator. + +We currently support the following robots: + +* Franka Emika Panda on a Clearpath Ridgeback Omni-drive Base + +From the default configuration file for these robots, zero actions imply a default pose. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import torch + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.viewports import set_camera_view + +import omni.isaac.orbit.utils.kit as kit_utils +from omni.isaac.orbit.robots.config.ridgeback_franka import RIDGEBACK_FRANKA_PANDA_CFG +from omni.isaac.orbit.robots.mobile_manipulator import MobileManipulator + +""" +Helpers +""" + + +def design_scene(): + """Add prims to the scene.""" + # Ground-plane + kit_utils.create_ground_plane("/World/defaultGroundPlane") + # Lights-1 + prim_utils.create_prim( + "/World/Light/GreySphere", + "SphereLight", + translation=(4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)}, + ) + # Lights-2 + prim_utils.create_prim( + "/World/Light/WhiteSphere", + "SphereLight", + translation=(-4.5, 3.5, 10.0), + attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)}, + ) + + +""" +Main +""" + + +def main(): + """Spawns a mobile manipulator and applies random joint position commands.""" + + # Load kit helper + sim = SimulationContext(stage_units_in_meters=1.0, physics_dt=0.01, rendering_dt=0.01, backend="torch") + # Set main camera + set_camera_view([1.5, 1.5, 1.5], [0.0, 0.0, 0.0]) + # Spawn things into stage + robot = MobileManipulator(cfg=RIDGEBACK_FRANKA_PANDA_CFG) + robot.spawn("/World/Robot_1", translation=(0.0, -1.0, 0.0)) + robot.spawn("/World/Robot_2", translation=(0.0, 1.0, 0.0)) + design_scene() + # Play the simulator + sim.reset() + # Acquire handles + # Initialize handles + robot.initialize("/World/Robot.*") + # Reset states + robot.reset_buffers() + + # Now we are ready! + print("[INFO]: Setup complete...") + + # dummy action + actions = torch.rand(robot.count, robot.num_actions, device=robot.device) + actions[:, 0 : robot.base_num_dof] = 0.0 + actions[:, -1] = -1 + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + # episode counter + sim_time = 0.0 + ep_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=not args_cli.headless) + continue + # reset + if ep_step_count % 1000 == 0: + sim_time = 0.0 + ep_step_count = 0 + # reset dof state + dof_pos, dof_vel = robot.get_default_dof_state() + robot.set_dof_state(dof_pos, dof_vel) + robot.reset_buffers() + # reset command + actions = torch.rand(robot.count, robot.num_actions, device=robot.device) + actions[:, 0 : robot.base_num_dof] = 0.0 + actions[:, -1] = 1 + print(">>>>>>>> Reset! Opening gripper.") + # change the gripper action + if ep_step_count % 200: + # flip command + actions[:, -1] = -actions[:, -1] + # change the base action + if ep_step_count == 200: + actions[:, : robot.base_num_dof] = 0.0 + actions[:, 0] = 1.0 + if ep_step_count == 300: + actions[:, : robot.base_num_dof] = 0.0 + actions[:, 0] = -1.0 + if ep_step_count == 400: + actions[:, : robot.base_num_dof] = 0.0 + actions[:, 1] = 1.0 + if ep_step_count == 500: + actions[:, : robot.base_num_dof] = 0.0 + actions[:, 1] = -1.0 + if ep_step_count == 600: + actions[:, : robot.base_num_dof] = 0.0 + actions[:, 2] = 1.0 + if ep_step_count == 700: + actions[:, : robot.base_num_dof] = 0.0 + actions[:, 2] = -1.0 + if ep_step_count == 900: + actions[:, : robot.base_num_dof] = 0.0 + actions[:, 2] = 1.0 + # change the arm action + if ep_step_count % 100: + actions[:, robot.base_num_dof : -1] = torch.rand(robot.count, robot.arm_num_dof, device=robot.device) + # apply action + robot.apply_action(actions) + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + ep_step_count += 1 + # note: to deal with timeline events such as stopping, we need to check if the simulation is playing + if sim.is_playing(): + # update buffers + robot.update_buffers(sim_dt) + # read buffers + if ep_step_count % 20 == 0: + if robot.data.tool_dof_pos[0, -1] > 0.01: + print("Opened gripper.") + else: + print("Closed gripper.") + + +if __name__ == "__main__": + # Run the main function + main() + # Close the simulator + simulation_app.close() diff --git a/source/standalone/environments/list_envs.py b/source/standalone/environments/list_envs.py new file mode 100644 index 0000000000..a35ecdd151 --- /dev/null +++ b/source/standalone/environments/list_envs.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to print all the available environments in ORBIT. + +The script iterates over all registered environments and stores the details in a table. +It prints the name of the environment, the entry point and the config file. + +All the environments are registered in the `omni.isaac.orbit_envs` extension. They start +with `Isaac` in their name. +""" + +import gym +from prettytable import PrettyTable + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 + + +def main(): + """Print all environments registered in `omni.isaac.orbit_envs` extension.""" + + # print all the available environments + table = PrettyTable(["S. No.", "Task Name", "Entry Point", "Config"]) + table.title = "Available Environments in ORBIT" + # set alignment of table columns + table.align["Task Name"] = "l" + table.align["Entry Point"] = "l" + table.align["Config"] = "l" + + # count of environments + index = 0 + # acquire all Isaac environments names + for task_spec in gym.envs.registry.all(): + if "Isaac" in task_spec.id: + # add details to table + table.add_row([index + 1, task_spec.id, task_spec.entry_point, task_spec._kwargs["cfg_entry_point"]]) + # increment count + index += 1 + + print(table) + + +if __name__ == "__main__": + main() diff --git a/source/standalone/environments/random_agent.py b/source/standalone/environments/random_agent.py new file mode 100644 index 0000000000..c8a4711a61 --- /dev/null +++ b/source/standalone/environments/random_agent.py @@ -0,0 +1,66 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to an environment with random action agent. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import torch + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import parse_env_cfg + + +def main(): + """Random actions agent with Isaac Sim environment.""" + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + # create environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + + # reset environment + env.reset() + # simulate environment + while simulation_app.is_running(): + # sample actions from -1 to 1 + actions = 2 * torch.rand((env.num_envs, env.action_space.shape[0]), device=env.device) - 1 + # apply actions + _, _, _, _ = env.step(actions) + # check if simulator is stopped + if env.unwrapped.sim.is_stopped(): + break + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/environments/teleoperation/teleop_se3_agent.py b/source/standalone/environments/teleoperation/teleop_se3_agent.py new file mode 100644 index 0000000000..d96c172c0c --- /dev/null +++ b/source/standalone/environments/teleoperation/teleop_se3_agent.py @@ -0,0 +1,118 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to run a keyboard teleoperation with Orbit manipulation environments. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") +parser.add_argument("--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.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import torch + +import carb + +from omni.isaac.orbit.devices import Se3Keyboard, Se3SpaceMouse + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import parse_env_cfg + + +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) + + +def main(): + """Running keyboard teleoperation with Orbit manipulation environment.""" + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + # modify configuration + env_cfg.control.control_type = "inverse_kinematics" + env_cfg.control.inverse_kinematics.command_type = "pose_rel" + env_cfg.terminations.episode_timeout = False + # create environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + # check environment name (for reach , we don't allow the gripper) + if "Reach" in args_cli.task: + carb.log_warn( + f"The environment '{args_cli.task}' does not support gripper control. The device command will be ignored." + ) + + # create controller + if args_cli.device.lower() == "keyboard": + teleop_interface = Se3Keyboard( + pos_sensitivity=0.005 * args_cli.sensitivity, rot_sensitivity=0.005 * args_cli.sensitivity + ) + elif args_cli.device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse( + pos_sensitivity=0.05 * args_cli.sensitivity, rot_sensitivity=0.005 * args_cli.sensitivity + ) + else: + raise ValueError(f"Invalid device interface '{args_cli.device}'. Supported: 'keyboard', 'spacemouse'.") + # add teleoperation key for env reset + teleop_interface.add_callback("L", env.reset) + # print helper for keyboard + print(teleop_interface) + + # reset environment + env.reset() + teleop_interface.reset() + + # simulate environment + while simulation_app.is_running(): + # 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) + # pre-process actions + actions = pre_process_actions(delta_pose, gripper_command) + # apply actions + _, _, _, _ = env.step(actions) + # check if simulator is stopped + if env.unwrapped.sim.is_stopped(): + break + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/environments/zero_agent.py b/source/standalone/environments/zero_agent.py new file mode 100644 index 0000000000..90ec068a87 --- /dev/null +++ b/source/standalone/environments/zero_agent.py @@ -0,0 +1,66 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to run an environment with zero action agent. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import torch + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import parse_env_cfg + + +def main(): + """Zero actions agent with Isaac Sim environment.""" + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + # create environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + + # reset environment + env.reset() + # simulate environment + while simulation_app.is_running(): + # compute zero actions + actions = torch.zeros((env.num_envs, env.action_space.shape[0]), device=env.device) + # apply actions + _, _, _, _ = env.step(actions) + # check if simulator is stopped + if env.unwrapped.sim.is_stopped(): + break + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/rl_games/config.py b/source/standalone/workflows/rl_games/config.py new file mode 100644 index 0000000000..62bf739e60 --- /dev/null +++ b/source/standalone/workflows/rl_games/config.py @@ -0,0 +1,45 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import yaml + +from omni.isaac.orbit_envs import ORBIT_ENVS_DATA_DIR + +__all__ = ["RLG_PPO_CONFIG_FILE", "parse_rlg_cfg"] + + +RLG_PPO_CONFIG_FILE = { + # classic + "Isaac-Cartpole-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rl_games/cartpole_ppo.yaml"), + "Isaac-Ant-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rl_games/ant_ppo.yaml"), + "Isaac-Humanoid-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rl_games/humanoid_ppo.yaml"), + # manipulation + "Isaac-Lift-Franka-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rl_games/lift_ppo.yaml"), + "Isaac-Reach-Franka-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rl_games/reach_ppo.yaml"), +} +"""Mapping from environment names to PPO agent files.""" + + +def parse_rlg_cfg(task_name) -> dict: + """Parse configuration based on command line arguments. + + Args: + task_name (str): The name of the environment. + + Returns: + dict: A dictionary containing the parsed configuration. + """ + # retrieve the default environment config file + try: + config_file = RLG_PPO_CONFIG_FILE[task_name] + except KeyError: + raise ValueError(f"Task not found: {task_name}") + + # parse agent configuration + with open(config_file) as f: + cfg = yaml.load(f, Loader=yaml.Loader) + + return cfg diff --git a/source/standalone/workflows/rl_games/play.py b/source/standalone/workflows/rl_games/play.py new file mode 100644 index 0000000000..50bd8e15ec --- /dev/null +++ b/source/standalone/workflows/rl_games/play.py @@ -0,0 +1,146 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to play a checkpoint if an RL agent from RL-Games. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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("--checkpoint", type=str, default=None, help="Path to model checkpoint.") +parser.add_argument( + "--use_last_checkpoint", + action="store_true", + help="When no checkpoint provided, use the last saved model. Otherwise use the best saved model.", +) +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import math +import os + +from rl_games.common import env_configurations, vecenv +from rl_games.common.player import BasePlayer +from rl_games.torch_runner import Runner + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import get_checkpoint_path, parse_env_cfg +from omni.isaac.orbit_envs.utils.wrappers.rl_games import RlGamesGpuEnv, RlGamesVecEnvWrapper + +from config import parse_rlg_cfg + + +def main(): + """Play with RL-Games agent.""" + + # parse env configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + agent_cfg = parse_rlg_cfg(args_cli.task) + + # 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) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + # wrap around environment for rl-games + env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions) + + # register the environment to rl-games registry + # note: in agents configuration: environment name must be "rlgpu" + vecenv.register( + "IsaacRlgWrapper", lambda config_name, num_actors, **kwargs: RlGamesGpuEnv(config_name, num_actors, **kwargs) + ) + env_configurations.register("rlgpu", {"vecenv_type": "IsaacRlgWrapper", "env_creator": lambda **kwargs: env}) + + # 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) + print(f"[INFO] Loading experiment from directory: {log_root_path}") + # find checkpoint + if args_cli.checkpoint is None: + # specify directory for logging runs + if "full_experiment_name" not in agent_cfg["params"]["config"]: + run_dir = os.path.join("*", "nn") + else: + run_dir = os.path.join(agent_cfg["params"]["config"]["full_experiment_name"], "nn") + # specify name of checkpoint + if args_cli.use_last_checkpoint: + checkpoint_file = None + else: + checkpoint_file = f"{agent_cfg['params']['config']['name']}.pth" + # get path to previous checkpoint + resume_path = get_checkpoint_path(log_root_path, run_dir, checkpoint_file) + else: + resume_path = os.path.abspath(args_cli.checkpoint) + # load previously trained model + agent_cfg["params"]["load_checkpoint"] = True + agent_cfg["params"]["load_path"] = resume_path + print(f"[INFO]: Loading model checkpoint from: {agent_cfg['params']['load_path']}") + + # set number of actors into agent config + agent_cfg["params"]["config"]["num_actors"] = env.unwrapped.num_envs + # create runner from rl-games + runner = Runner() + runner.load(agent_cfg) + # obtain the agent from the runner + agent: BasePlayer = runner.create_player() + agent.restore(resume_path) + agent.reset() + # flag for inference + is_deterministic = True + + # reset environment + obs = env.reset() + # required: enables the flag for batched observations + _ = agent.get_batch_size(obs, 1) + # simulate environment + # note: We simplified the logic in rl-games player.py (:func:`BasePlayer.run()`) function in an + # attempt to have complete control over environment stepping. However, this removes other + # operations such as masking that is used for multi-agent learning by RL-Games. + while simulation_app.is_running(): + # convert obs to agent format + obs = agent.obs_to_torch(obs) + # agent stepping + actions = agent.get_action(obs, is_deterministic) + # env stepping + obs, _, dones, _ = env.step(actions) + # check if simulator is stopped + if env.unwrapped.sim.is_stopped(): + break + # perform operations for terminated episodes + if len(dones) > 0: + # reset rnn state for terminated episodes + if agent.is_rnn and agent.states is not None: + for s in agent.states: + s[:, dones, :] = 0.0 + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/rl_games/train.py b/source/standalone/workflows/rl_games/train.py new file mode 100644 index 0000000000..e53a0a8521 --- /dev/null +++ b/source/standalone/workflows/rl_games/train.py @@ -0,0 +1,127 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to train RL agent with RL-Games. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +import os + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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("--seed", type=int, default=None, help="Seed used for the environment") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +# load cheaper kit config in headless +if args_cli.headless: + app_experience = f"{os.environ['EXP_PATH']}/omni.isaac.sim.python.gym.headless.kit" +else: + app_experience = f"{os.environ['EXP_PATH']}/omni.isaac.sim.python.kit" +# launch the simulator +simulation_app = SimulationApp(config, experience=app_experience) + +"""Rest everything follows.""" + + +import gym +import math +import os +from datetime import datetime + +from rl_games.common import env_configurations, vecenv +from rl_games.common.algo_observer import IsaacAlgoObserver +from rl_games.torch_runner import Runner + +from omni.isaac.orbit.utils.io import dump_pickle, dump_yaml + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import parse_env_cfg +from omni.isaac.orbit_envs.utils.wrappers.rl_games import RlGamesGpuEnv, RlGamesVecEnvWrapper + +from config import parse_rlg_cfg + + +def main(): + """Train with RL-Games agent.""" + + args_cli_seed = args_cli.seed + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + agent_cfg = parse_rlg_cfg(args_cli.task) + # override from command line + if args_cli_seed is not None: + agent_cfg["params"]["seed"] = args_cli_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) + print(f"[INFO] Logging experiment in directory: {log_root_path}") + # specify directory for logging runs + if "full_experiment_name" not in agent_cfg["params"]["config"]: + log_dir = datetime.now().strftime("%b%d_%H-%M-%S") + else: + log_dir = agent_cfg["params"]["config"]["full_experiment_name"] + # set directory into agent config + # logging directory path: / + agent_cfg["params"]["config"]["train_dir"] = log_root_path + agent_cfg["params"]["config"]["full_experiment_name"] = log_dir + + # 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"] + clip_obs = agent_cfg["params"]["env"].get("clip_observations", math.inf) + clip_actions = agent_cfg["params"]["env"].get("clip_actions", math.inf) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + # wrap around environment for rl-games + env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions) + + # register the environment to rl-games registry + # note: in agents configuration: environment name must be "rlgpu" + vecenv.register( + "IsaacRlgWrapper", lambda config_name, num_actors, **kwargs: RlGamesGpuEnv(config_name, num_actors, **kwargs) + ) + env_configurations.register("rlgpu", {"vecenv_type": "IsaacRlgWrapper", "env_creator": lambda **kwargs: env}) + + # 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()) + runner.load(agent_cfg) + + # set seed of the env + env.seed(agent_cfg["params"]["seed"]) + # reset the agent and env + runner.reset() + # train the agent + runner.run({"train": True, "play": False, "sigma": None}) + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/robomimic/collect_demonstrations.py b/source/standalone/workflows/robomimic/collect_demonstrations.py new file mode 100644 index 0000000000..bc33e29f2d --- /dev/null +++ b/source/standalone/workflows/robomimic/collect_demonstrations.py @@ -0,0 +1,164 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to collect demonstrations with IsaacSim environments.""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--device", type=str, default="keyboard", help="Device for interacting with environment") +parser.add_argument("--num_demos", type=int, default=1, help="Number of episodes to store in the dataset.") +parser.add_argument("--filename", type=str, default="hdf_dataset", help="Basename of output file.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import os +import torch + +from omni.isaac.orbit.devices import Se3Keyboard, Se3SpaceMouse +from omni.isaac.orbit.utils.io import dump_pickle, dump_yaml + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils.data_collector import RobomimicDataCollector +from omni.isaac.orbit_envs.utils.parse_cfg import parse_env_cfg + + +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 main(): + """Collect demonstrations from the environment using teleop interfaces.""" + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + # modify configuration + env_cfg.control.control_type = "inverse_kinematics" + env_cfg.control.inverse_kinematics.command_type = "pose_rel" + env_cfg.terminations.episode_timeout = False + env_cfg.terminations.is_success = True + env_cfg.observations.return_dict_obs_in_group = True + + # create environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + + # create controller + if args_cli.device.lower() == "keyboard": + teleop_interface = Se3Keyboard(pos_sensitivity=0.4, rot_sensitivity=0.8) + elif args_cli.device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse(pos_sensitivity=0.05, rot_sensitivity=0.005) + else: + raise ValueError(f"Invalid device interface '{args_cli.device}'. Supported: 'keyboard', 'spacemouse'.") + # add teleoperation key for env reset + teleop_interface.add_callback("L", env.reset) + # print helper + print(teleop_interface) + + # specify directory for logging experiments + log_dir = os.path.join("./logs/robomimic", args_cli.task) + # dump the configuration into log-directory + dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) + dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) + + # create data-collector + collector_interface = RobomimicDataCollector( + env_name=args_cli.task, + directory_path=log_dir, + filename=args_cli.filename, + num_demos=args_cli.num_demos, + flush_freq=env.num_envs, + env_config={"device": args_cli.device}, + ) + + # reset environment + obs_dict = env.reset() + # robomimic only cares about policy observations + obs = obs_dict["policy"] + # reset interfaces + teleop_interface.reset() + collector_interface.reset() + + # simulate environment + try: + while not collector_interface.is_stopped(): + # 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) + + # TODO: Deal with the case when reset is triggered by teleoperation device. + # The observations need to be recollected. + # store signals before stepping + # -- obs + for key, value in obs.items(): + collector_interface.add(f"obs/{key}", value) + # -- actions + collector_interface.add("actions", actions) + # perform action on environment + obs_dict, rewards, dones, info = env.step(actions) + # check that simulation is stopped or not + if env.unwrapped.sim.is_stopped(): + break + # robomimic only cares about policy observations + obs = obs_dict["policy"] + # store signals from the environment + # -- next_obs + for key, value in obs.items(): + collector_interface.add(f"next_obs/{key}", value.cpu().numpy()) + # -- rewards + collector_interface.add("rewards", rewards) + # -- dones + collector_interface.add("dones", dones) + # -- is-success label + try: + collector_interface.add("success", info["is_success"]) + except KeyError: + raise RuntimeError( + f"Only goal-conditioned environment supported. No attribute named 'is_success' found in {list(info.keys())}." + ) + # flush data from collector for successful environments + reset_env_ids = dones.nonzero(as_tuple=False).squeeze(-1) + collector_interface.flush(reset_env_ids) + except KeyboardInterrupt: + pass + + # close the simulator + collector_interface.close() + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/robomimic/config.py b/source/standalone/workflows/robomimic/config.py new file mode 100644 index 0000000000..9b3101674b --- /dev/null +++ b/source/standalone/workflows/robomimic/config.py @@ -0,0 +1,18 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os + +from omni.isaac.orbit_envs import ORBIT_ENVS_DATA_DIR + +ROBOMIMIC_CONFIG_FILES_DICT = { + "Isaac-Lift-Franka-v0": { + "bc": os.path.join(ORBIT_ENVS_DATA_DIR, "robomimic/lift_bc.json"), + "bcq": os.path.join(ORBIT_ENVS_DATA_DIR, "robomimic/lift_bcq.json"), + } +} +"""Mapping from environment names to imitation learning config files.""" + +__all__ = ["ROBOMIMIC_CONFIG_FILES_DICT"] diff --git a/source/standalone/workflows/robomimic/play.py b/source/standalone/workflows/robomimic/play.py new file mode 100644 index 0000000000..f2686cdc35 --- /dev/null +++ b/source/standalone/workflows/robomimic/play.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to run a trained policy from robomimic. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--checkpoint", type=str, default=None, help="Pytorch model checkpoint to load.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import math +import torch + +import robomimic # noqa: F401 +import robomimic.utils.file_utils as FileUtils +import robomimic.utils.torch_utils as TorchUtils + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import parse_env_cfg + + +def main(): + """Run a trained policy from robomimic with Isaac Sim environment.""" + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=1) + # modify configuration + env_cfg.control.control_type = "inverse_kinematics" + env_cfg.control.inverse_kinematics.command_type = "pose_rel" + env_cfg.terminations.episode_timeout = False + env_cfg.terminations.is_success = True + env_cfg.observations.return_dict_obs_in_group = True + # FIXME: Teleop works Spawn the Franka s.t. it is axis aligned with world frame. + env_cfg.robot.init_state.dof_pos = { + "panda_joint1": 0.0, + "panda_joint2": math.pi / 16.0, + "panda_joint3": 0.0, + "panda_joint4": -math.pi / 2.0 - math.pi / 3.0, + "panda_joint5": 0.0, + "panda_joint6": math.pi - 0.2, + "panda_joint7": math.pi / 4, + "panda_finger_joint*": 0.035, + } + # create environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + + # acquire device + device = TorchUtils.get_torch_device(try_to_use_cuda=True) + # restore policy + policy, _ = FileUtils.policy_from_checkpoint(ckpt_path=args_cli.checkpoint, device=device, verbose=True) + + # reset environment + obs_dict = env.reset() + # robomimic only cares about policy observations + obs = obs_dict["policy"] + # simulate environment + while simulation_app.is_running(): + # compute actions + actions = policy(obs) + actions = torch.from_numpy(actions).to(device=device).view(1, env.action_space.shape[0]) + # apply actions + obs_dict, _, _, _ = env.step(actions) + # check if simulator is stopped + if env.unwrapped.sim.is_stopped(): + break + # robomimic only cares about policy observations + obs = obs_dict["policy"] + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/robomimic/tools/episode_merging.py b/source/standalone/workflows/robomimic/tools/episode_merging.py new file mode 100644 index 0000000000..2131ce1711 --- /dev/null +++ b/source/standalone/workflows/robomimic/tools/episode_merging.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Tool to merge multiple episodes with single trajectory into one episode with multiple trajectories. +""" + + +import argparse +import h5py +import json +import os + +if __name__ == "__main__": + # parse arguments + parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") + parser.add_argument( + "--dir", type=str, default=None, help="PATH to directory that contains all single episode hdf5 files" + ) + parser.add_argument("--out", type=str, default="merged_dataset.hdf5", help="output hdf5 file") + args_cli = parser.parse_args() + + parent_dir = args_cli.dir + merged_dataset_name = args_cli.out + + # get hdf5 entries from specified directory + entries = [i for i in os.listdir(parent_dir) if i.endswith(".hdf5")] + + # create new hdf5 file for merging episodes + fp = h5py.File(parent_dir + merged_dataset_name, "a") + + # init + f_grp = fp.create_group("data") + f_grp.attrs["num_samples"] = 0 + count = 0 + + for entry in entries: + fc = h5py.File(parent_dir + entry, "r") + + # find total number of samples in all demos + f_grp.attrs["num_samples"] = f_grp.attrs["num_samples"] + fc["data"]["demo_0"].attrs["num_samples"] + + fc.copy("data/demo_0", fp["data"], "demo_" + str(count)) + count += 1 + + # This is needed to run env in robomimic + fp["data"].attrs["env_args"] = json.dumps({"env_name": "Isaac-Lift-v0", "type": 2, "env_kwargs": {}}) + + fp.close() + + print("merged") diff --git a/source/standalone/workflows/robomimic/tools/inspect_demonstrations.py b/source/standalone/workflows/robomimic/tools/inspect_demonstrations.py new file mode 100644 index 0000000000..41575e30c6 --- /dev/null +++ b/source/standalone/workflows/robomimic/tools/inspect_demonstrations.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Tool to check structure of hdf5 files. +""" + + +import argparse +import h5py + + +def check_group(f, num: int): + """Print the data from different keys in stored dictionary.""" + # print name of the group first + for subs in f: + if type(subs) == str: + print("\t" * num, subs, ":", type(f[subs])) + check_group(f[subs], num + 1) + # print attributes of the group + print("\t" * num, "attributes", ":") + for attr in f.attrs: + print("\t" * (num + 1), attr, ":", type(f.attrs[attr]), ":", f.attrs[attr]) + + +if __name__ == "__main__": + # parse arguments + parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") + parser.add_argument("file", type=str, default=None, help="The path to HDF5 file to analyze.") + args_cli = parser.parse_args() + + # open specified file + with h5py.File(args_cli.file, "r") as f: + # print name of the file first + print(f) + # print contents of file + check_group(f["data"], 1) diff --git a/source/standalone/workflows/robomimic/tools/split_train_val.py b/source/standalone/workflows/robomimic/tools/split_train_val.py new file mode 100644 index 0000000000..e876bc4d46 --- /dev/null +++ b/source/standalone/workflows/robomimic/tools/split_train_val.py @@ -0,0 +1,124 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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. +# + +""" +Script for splitting a dataset hdf5 file into training and validation trajectories. + +Args: + dataset (str): path to hdf5 dataset + + filter_key (str): if provided, split the subset of trajectories + in the file that correspond to this filter key into a training + and validation set of trajectories, instead of splitting the + full set of trajectories + + ratio (float): validation ratio, in (0, 1). Defaults to 0.1, which is 10%. + +Example usage: + python split_train_val.py --dataset /path/to/demo.hdf5 --ratio 0.1 +""" + +import argparse +import h5py +import numpy as np + +from robomimic.utils.file_utils import create_hdf5_filter_key + + +def split_train_val_from_hdf5(hdf5_path, val_ratio=0.1, filter_key=None): + """ + Splits data into training set and validation set from HDF5 file. + + Args: + hdf5_path (str): path to the hdf5 file + to load the transitions from + + val_ratio (float): ratio of validation demonstrations to all demonstrations + + filter_key (str): if provided, split the subset of demonstration keys stored + under mask/@filter_key instead of the full set of demonstrations + """ + + # retrieve number of demos + f = h5py.File(hdf5_path, "r") + if filter_key is not None: + print(f"Using filter key: {filter_key}") + demos = sorted(elem.decode("utf-8") for elem in np.array(f[f"mask/{filter_key}"])) + else: + demos = sorted(list(f["data"].keys())) + num_demos = len(demos) + f.close() + + # get random split + num_demos = len(demos) + num_val = int(val_ratio * num_demos) + mask = np.zeros(num_demos) + mask[:num_val] = 1.0 + np.random.shuffle(mask) + mask = mask.astype(int) + train_inds = (1 - mask).nonzero()[0] + valid_inds = mask.nonzero()[0] + train_keys = [demos[i] for i in train_inds] + valid_keys = [demos[i] for i in valid_inds] + print(f"{num_val} validation demonstrations out of {num_demos} total demonstrations.") + + # pass mask to generate split + name_1 = "train" + name_2 = "valid" + if filter_key is not None: + name_1 = f"{filter_key}_{name_1}" + name_2 = f"{filter_key}_{name_2}" + + train_lengths = create_hdf5_filter_key(hdf5_path=hdf5_path, demo_keys=train_keys, key_name=name_1) + valid_lengths = create_hdf5_filter_key(hdf5_path=hdf5_path, demo_keys=valid_keys, key_name=name_2) + + print(f"Total number of train samples: {np.sum(train_lengths)}") + print(f"Average number of train samples {np.mean(train_lengths)}") + + print(f"Total number of valid samples: {np.sum(valid_lengths)}") + print(f"Average number of valid samples {np.mean(valid_lengths)}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("dataset", type=str, help="path to hdf5 dataset") + parser.add_argument( + "--filter_key", + type=str, + default=None, + help="if provided, split the subset of trajectories in the file that correspond to\ + this filter key into a training and validation set of trajectories, instead of\ + splitting the full set of trajectories", + ) + parser.add_argument("--ratio", type=float, default=0.1, help="validation ratio, in (0, 1)") + args = parser.parse_args() + + # seed to make sure results are consistent + np.random.seed(0) + + split_train_val_from_hdf5(args.dataset, val_ratio=args.ratio, filter_key=args.filter_key) diff --git a/source/standalone/workflows/robomimic/train.py b/source/standalone/workflows/robomimic/train.py new file mode 100644 index 0000000000..acf2e9f824 --- /dev/null +++ b/source/standalone/workflows/robomimic/train.py @@ -0,0 +1,390 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# 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. + + +""" +The main entry point for training policies from pre-collected data. + +Args: + algo (str): name of the algorithm to run. + task (str): name of the environment. + name (str): if provided, override the experiment name defined in the config + dataset (str): if provided, override the dataset path defined in the config +""" + + +import argparse +import json +import numpy as np +import os +import sys +import time +import torch +import traceback +from collections import OrderedDict +from torch.utils.data import DataLoader + +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 +from robomimic.algo import RolloutPolicy, algo_factory +from robomimic.config import config_factory +from robomimic.utils.log_utils import DataLogger, PrintLogger + +from config import ROBOMIMIC_CONFIG_FILES_DICT + + +def train(config, device): + """ + Train a model using the algorithm. + """ + + # first set seeds + np.random.seed(config.train.seed) + torch.manual_seed(config.train.seed) + + 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}") + print(f">>> Saving videos into directory: {video_dir}") + + if config.experiment.logging.terminal_output_to_txt: + # log stdout and stderr to a text file + logger = PrintLogger(os.path.join(log_dir, "log.txt")) + sys.stdout = logger + sys.stderr = logger + + # read config to set up metadata for observation modalities (e.g. detecting rgb observations) + ObsUtils.initialize_obs_utils_with_config(config) + + # make sure the dataset exists + dataset_path = os.path.expanduser(config.train.data) + if not os.path.exists(dataset_path): + raise FileNotFoundError(f"Dataset at provided path {dataset_path} not found!") + + # load basic metadata from training file + print("\n============= Loaded Environment Metadata =============") + env_meta = FileUtils.get_env_metadata_from_dataset(dataset_path=config.train.data) + shape_meta = FileUtils.get_shape_metadata_from_dataset( + dataset_path=config.train.data, all_obs_keys=config.all_obs_keys, verbose=True + ) + + if config.experiment.env is not None: + env_meta["env_name"] = config.experiment.env + print("=" * 30 + "\n" + "Replacing Env to {}\n".format(env_meta["env_name"]) + "=" * 30) + + # create environment + envs = OrderedDict() + if config.experiment.rollout.enabled: + # create environments for validation runs + env_names = [env_meta["env_name"]] + + if config.experiment.additional_envs is not None: + for name in config.experiment.additional_envs: + env_names.append(name) + + for env_name in env_names: + env = EnvUtils.create_env_from_metadata( + env_meta=env_meta, + env_name=env_name, + render=False, + render_offscreen=config.experiment.render_video, + use_image_obs=shape_meta["use_images"], + ) + envs[env.name] = env + print(envs[env.name]) + + print("") + + # setup for a new training run + data_logger = DataLogger(log_dir, log_tb=config.experiment.logging.log_tb) + model = algo_factory( + algo_name=config.algo_name, + config=config, + obs_key_shapes=shape_meta["all_shapes"], + ac_dim=shape_meta["ac_dim"], + device=device, + ) + + # save the config as a json file + with open(os.path.join(log_dir, "..", "config.json"), "w") as outfile: + json.dump(config, outfile, indent=4) + + print("\n============= Model Summary =============") + print(model) # print model summary + print("") + + # load training data + trainset, validset = TrainUtils.load_data_for_training(config, obs_keys=shape_meta["all_obs_keys"]) + train_sampler = trainset.get_dataset_sampler() + print("\n============= Training Dataset =============") + print(trainset) + print("") + + # maybe retrieve statistics for normalizing observations + obs_normalization_stats = None + if config.train.hdf5_normalize_obs: + obs_normalization_stats = trainset.get_obs_normalization_stats() + + # initialize data loaders + train_loader = DataLoader( + dataset=trainset, + sampler=train_sampler, + batch_size=config.train.batch_size, + shuffle=(train_sampler is None), + num_workers=config.train.num_data_workers, + drop_last=True, + ) + + if config.experiment.validate: + # cap num workers for validation dataset at 1 + num_workers = min(config.train.num_data_workers, 1) + valid_sampler = validset.get_dataset_sampler() + valid_loader = DataLoader( + dataset=validset, + sampler=valid_sampler, + batch_size=config.train.batch_size, + shuffle=(valid_sampler is None), + num_workers=num_workers, + drop_last=True, + ) + else: + valid_loader = None + + # main training loop + best_valid_loss = None + best_return = {k: -np.inf for k in envs} if config.experiment.rollout.enabled else None + best_success_rate = {k: -1.0 for k in envs} if config.experiment.rollout.enabled else None + last_ckpt_time = time.time() + + # number of learning steps per epoch (defaults to a full dataset pass) + train_num_steps = config.experiment.epoch_every_n_steps + valid_num_steps = config.experiment.validation_epoch_every_n_steps + + for epoch in range(1, config.train.num_epochs + 1): # epoch numbers start at 1 + step_log = TrainUtils.run_epoch(model=model, data_loader=train_loader, epoch=epoch, num_steps=train_num_steps) + model.on_epoch_end(epoch) + + # setup checkpoint path + epoch_ckpt_name = f"model_epoch_{epoch}" + + # check for recurring checkpoint saving conditions + should_save_ckpt = False + if config.experiment.save.enabled: + time_check = (config.experiment.save.every_n_seconds is not None) and ( + time.time() - last_ckpt_time > config.experiment.save.every_n_seconds + ) + epoch_check = ( + (config.experiment.save.every_n_epochs is not None) + and (epoch > 0) + 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 + ckpt_reason = None + if should_save_ckpt: + last_ckpt_time = time.time() + ckpt_reason = "time" + + print(f"Train Epoch {epoch}") + print(json.dumps(step_log, sort_keys=True, indent=4)) + for k, v in step_log.items(): + if k.startswith("Time_"): + data_logger.record(f"Timing_Stats/Train_{k[5:]}", v, epoch) + else: + data_logger.record(f"Train/{k}", v, epoch) + + # Evaluate the model on validation set + if config.experiment.validate: + with torch.no_grad(): + step_log = TrainUtils.run_epoch( + model=model, data_loader=valid_loader, epoch=epoch, validate=True, num_steps=valid_num_steps + ) + for k, v in step_log.items(): + if k.startswith("Time_"): + data_logger.record(f"Timing_Stats/Valid_{k[5:]}", v, epoch) + else: + data_logger.record(f"Valid/{k}", v, epoch) + + print(f"Validation Epoch {epoch}") + print(json.dumps(step_log, sort_keys=True, indent=4)) + + # save checkpoint if achieve new best validation loss + valid_check = "Loss" in step_log + if valid_check and (best_valid_loss is None or (step_log["Loss"] <= best_valid_loss)): + best_valid_loss = step_log["Loss"] + if config.experiment.save.enabled and config.experiment.save.on_best_validation: + epoch_ckpt_name += f"_best_validation_{best_valid_loss}" + should_save_ckpt = True + ckpt_reason = "valid" if ckpt_reason is None else ckpt_reason + + # Evaluate the model by by running rollouts + + # do rollouts at fixed rate or if it's time to save a new ckpt + video_paths = None + rollout_check = (epoch % config.experiment.rollout.rate == 0) or (should_save_ckpt and ckpt_reason == "time") + if config.experiment.rollout.enabled and (epoch > config.experiment.rollout.warmstart) and rollout_check: + + # wrap model as a RolloutPolicy to prepare for rollouts + rollout_model = RolloutPolicy(model, obs_normalization_stats=obs_normalization_stats) + + num_episodes = config.experiment.rollout.n + all_rollout_logs, video_paths = TrainUtils.rollout_with_stats( + policy=rollout_model, + envs=envs, + horizon=config.experiment.rollout.horizon, + use_goals=config.use_goals, + num_episodes=num_episodes, + render=False, + video_dir=video_dir if config.experiment.render_video else None, + epoch=epoch, + video_skip=config.experiment.get("video_skip", 5), + terminate_on_success=config.experiment.rollout.terminate_on_success, + ) + + # summarize results from rollouts to tensorboard and terminal + for env_name in all_rollout_logs: + rollout_logs = all_rollout_logs[env_name] + for k, v in rollout_logs.items(): + if k.startswith("Time_"): + data_logger.record(f"Timing_Stats/Rollout_{env_name}_{k[5:]}", v, epoch) + else: + data_logger.record(f"Rollout/{k}/{env_name}", v, epoch, log_stats=True) + + print("\nEpoch {} Rollouts took {}s (avg) with results:".format(epoch, rollout_logs["time"])) + print(f"Env: {env_name}") + print(json.dumps(rollout_logs, sort_keys=True, indent=4)) + + # checkpoint and video saving logic + updated_stats = TrainUtils.should_save_from_rollout_logs( + all_rollout_logs=all_rollout_logs, + best_return=best_return, + best_success_rate=best_success_rate, + epoch_ckpt_name=epoch_ckpt_name, + save_on_best_rollout_return=config.experiment.save.on_best_rollout_return, + save_on_best_rollout_success_rate=config.experiment.save.on_best_rollout_success_rate, + ) + best_return = updated_stats["best_return"] + best_success_rate = updated_stats["best_success_rate"] + epoch_ckpt_name = updated_stats["epoch_ckpt_name"] + should_save_ckpt = ( + config.experiment.save.enabled and updated_stats["should_save_ckpt"] + ) or should_save_ckpt + if updated_stats["ckpt_reason"] is not None: + ckpt_reason = updated_stats["ckpt_reason"] + + # Only keep saved videos if the ckpt should be saved (but not because of validation score) + should_save_video = (should_save_ckpt and (ckpt_reason != "valid")) or config.experiment.keep_all_videos + if video_paths is not None and not should_save_video: + for env_name in video_paths: + os.remove(video_paths[env_name]) + + # Save model checkpoints based on conditions (success rate, validation loss, etc) + if should_save_ckpt: + TrainUtils.save_model( + model=model, + config=config, + env_meta=env_meta, + shape_meta=shape_meta, + ckpt_path=os.path.join(ckpt_dir, epoch_ckpt_name + ".pth"), + obs_normalization_stats=obs_normalization_stats, + ) + + # Finally, log memory usage in MB + process = psutil.Process(os.getpid()) + mem_usage = int(process.memory_info().rss / 1000000) + data_logger.record("System/RAM Usage (MB)", mem_usage, epoch) + print(f"\nEpoch {epoch} Memory Usage: {mem_usage} MB\n") + + # terminate logging + data_logger.close() + + +def main(args): + + if args.task is not None: + ext_cfg = json.load(open(ROBOMIMIC_CONFIG_FILES_DICT[args.task][args.algo])) + config = config_factory(ext_cfg["algo_name"]) + # update config with external json - this will throw errors if + # the external config has keys not present in the base algo config + with config.values_unlocked(): + config.update(ext_cfg) + else: + raise ValueError("Please provide a task name through CLI arguments.") + + if args.dataset is not None: + config.train.data = args.dataset + + if args.name is not None: + config.experiment.name = args.name + + # change location of experiment directory + config.train.output_dir = os.path.abspath(os.path.join("./logs/robomimic", args.task)) + # get torch device + device = TorchUtils.get_torch_device(try_to_use_cuda=config.train.cuda) + + config.lock() + + # catch error during training and print it + res_str = "finished run successfully!" + try: + train(config, device=device) + except Exception as e: + res_str = f"run failed with error:\n{e}\n\n{traceback.format_exc()}" + print(res_str) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + + # Experiment Name (for tensorboard, saving models, etc.) + parser.add_argument( + "--name", + type=str, + default=None, + help="(optional) if provided, override the experiment name defined in the config", + ) + + # Dataset path, to override the one in the config + parser.add_argument( + "--dataset", + type=str, + default=None, + help="(optional) if provided, override the dataset path defined in the config", + ) + + 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.") + + args = parser.parse_args() + main(args) diff --git a/source/standalone/workflows/rsl_rl/config.py b/source/standalone/workflows/rsl_rl/config.py new file mode 100644 index 0000000000..7e3e2bf184 --- /dev/null +++ b/source/standalone/workflows/rsl_rl/config.py @@ -0,0 +1,45 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import yaml + +from omni.isaac.orbit_envs import ORBIT_ENVS_DATA_DIR + +__all__ = ["RSLRL_PPO_CONFIG_FILE", "parse_rslrl_cfg"] + + +RSLRL_PPO_CONFIG_FILE = { + # classic + "Isaac-Cartpole-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rsl_rl/cartpole_ppo.yaml"), + # manipulation + "Isaac-Lift-Franka-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rsl_rl/lift_ppo.yaml"), + "Isaac-Reach-Franka-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rsl_rl/reach_ppo.yaml"), + # locomotion + "Isaac-Velocity-Anymal-C-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "rsl_rl/anymal_ppo.yaml"), +} +"""Mapping from environment names to PPO agent files.""" + + +def parse_rslrl_cfg(task_name) -> dict: + """Parse configuration for RSL-RL agent based on inputs. + + Args: + task_name (str): The name of the environment. + + Returns: + dict: A dictionary containing the parsed configuration. + """ + # retrieve the default environment config file + try: + config_file = RSLRL_PPO_CONFIG_FILE[task_name] + except KeyError: + raise ValueError(f"Task not found: {task_name}") + + # parse agent configuration + with open(config_file) as f: + cfg = yaml.load(f, Loader=yaml.FullLoader) + + return cfg diff --git a/source/standalone/workflows/rsl_rl/play.py b/source/standalone/workflows/rsl_rl/play.py new file mode 100644 index 0000000000..e37ce6e248 --- /dev/null +++ b/source/standalone/workflows/rsl_rl/play.py @@ -0,0 +1,107 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to play a checkpoint if an RL agent from RSL-RL. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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("--checkpoint", type=str, default=None, help="Path to model checkpoint.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym +import os + +from rsl_rl.runners import OnPolicyRunner + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import get_checkpoint_path, parse_env_cfg +from omni.isaac.orbit_envs.utils.wrappers.rsl_rl import RslRlVecEnvWrapper, export_policy_as_onnx + +from config import parse_rslrl_cfg + + +def main(): + """Play with RSL-RL agent.""" + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + agent_cfg = parse_rslrl_cfg(args_cli.task) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + # wrap around environment for rsl-rl + env = RslRlVecEnvWrapper(env) + + # specify directory for logging experiments + log_root_path = os.path.join("logs", "rsl_rl", agent_cfg["runner"]["experiment_name"]) + log_root_path = os.path.abspath(log_root_path) + print(f"[INFO] Loading experiment from directory: {log_root_path}") + # get path to previous checkpoint + if args_cli.checkpoint is None: + # specify directory for logging runs + run_dir = "*" + if agent_cfg["runner"]["load_run"] != -1: + run_dir = agent_cfg["runner"]["load_run"] + # specify name of checkpoint + checkpoint_file = None + if agent_cfg["runner"]["checkpoint"] != -1: + checkpoint_file = f"model_{agent_cfg['runner']['checkpoint']}.pt" + # get path to previous checkpoint + resume_path = get_checkpoint_path(log_root_path, run_dir, checkpoint_file) + else: + resume_path = os.path.abspath(args_cli.checkpoint) + # load previously trained model + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + + # create runner from rsl-rl + ppo_runner = OnPolicyRunner(env, agent_cfg, log_dir=None, device=agent_cfg["device"]) + ppo_runner.load(resume_path) + # obtain the trained policy for inference + policy = ppo_runner.get_inference_policy(device=env.unwrapped.device) + + # export policy to onnx + export_model_dir = os.path.join(os.path.dirname(resume_path), "exported") + export_policy_as_onnx(ppo_runner.alg.actor_critic, export_model_dir, filename="policy.onnx") + + # reset environment + obs, _ = env.reset() + # simulate environment + while simulation_app.is_running(): + # agent stepping + actions = policy(obs) + # env stepping + obs, _, _, _, _ = env.step(actions) + # check if simulator is stopped + if env.unwrapped.sim.is_stopped(): + break + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/rsl_rl/train.py b/source/standalone/workflows/rsl_rl/train.py new file mode 100644 index 0000000000..e81463138b --- /dev/null +++ b/source/standalone/workflows/rsl_rl/train.py @@ -0,0 +1,115 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to train RL agent with RSL-RL. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +import os + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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("--seed", type=int, default=None, help="Seed used for the environment") +args_cli = parser.parse_args() + +config = {"headless": args_cli.headless} +# load cheaper kit config in headless +if args_cli.headless: + app_experience = f"{os.environ['EXP_PATH']}/omni.isaac.sim.python.gym.headless.kit" +else: + app_experience = f"{os.environ['EXP_PATH']}/omni.isaac.sim.python.kit" +# launch the simulator +simulation_app = SimulationApp(config, experience=app_experience) + +"""Rest everything follows.""" + + +import gym +import os +from datetime import datetime + +from rsl_rl.runners import OnPolicyRunner + +from omni.isaac.orbit.utils.io import dump_pickle, dump_yaml + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import get_checkpoint_path, parse_env_cfg +from omni.isaac.orbit_envs.utils.wrappers.rsl_rl import RslRlVecEnvWrapper + +from config import parse_rslrl_cfg + + +def main(): + """Train with RSL-RL agent.""" + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + agent_cfg = parse_rslrl_cfg(args_cli.task) + # override config from command line + if args_cli.seed is not None: + agent_cfg["seed"] = args_cli.seed + + # specify directory for logging experiments + log_root_path = os.path.join("logs", "rsl_rl", agent_cfg["runner"]["experiment_name"]) + 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 = datetime.now().strftime("%b%d_%H-%M-%S") + if agent_cfg["runner"]["run_name"]: + log_dir += f"_{agent_cfg['runner']['run_name']}" + log_dir = os.path.join(log_root_path, log_dir) + + # 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) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + # wrap around environment for rsl-rl + env = RslRlVecEnvWrapper(env) + + # create runner from rsl-rl + runner = OnPolicyRunner(env, agent_cfg, log_dir=log_dir, device=agent_cfg["device"]) + # save resume path before creating a new log_dir + if agent_cfg["runner"]["resume"]: + # specify directory for logging runs + run_dir = "*" + if agent_cfg["runner"]["load_run"] != -1: + run_dir = agent_cfg["runner"]["load_run"] + # specify name of checkpoint + checkpoint_file = None + if agent_cfg["runner"]["checkpoint"] != -1: + checkpoint_file = f"model_{agent_cfg['runner']['checkpoint']}.pt" + # get path to previous checkpoint + resume_path = get_checkpoint_path(log_root_path, run_dir, checkpoint_file) + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + # load previously trained model + runner.load(resume_path) + + # set seed of the environment + env.seed(agent_cfg["seed"]) + # run training + runner.learn(num_learning_iterations=agent_cfg["runner"]["max_iterations"], init_at_random_ep_len=True) + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/sb3/config.py b/source/standalone/workflows/sb3/config.py new file mode 100644 index 0000000000..37c5aaa25b --- /dev/null +++ b/source/standalone/workflows/sb3/config.py @@ -0,0 +1,50 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import yaml +from torch import nn as nn # noqa: F401 + +from omni.isaac.orbit_envs import ORBIT_ENVS_DATA_DIR + +__all__ = ["SB3_PPO_CONFIG_FILE", "parse_sb3_cfg"] + +SB3_PPO_CONFIG_FILE = { + # classic + "Isaac-Cartpole-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "sb3/cartpole_ppo.yaml"), + "Isaac-Ant-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "sb3/ant_ppo.yaml"), + "Isaac-Humanoid-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "sb3/humanoid_ppo.yaml"), + # manipulation + "Isaac-Reach-Franka-v0": os.path.join(ORBIT_ENVS_DATA_DIR, "sb3/reach_ppo.yaml"), +} +"""Mapping from environment names to PPO agent files.""" + + +def parse_sb3_cfg(task_name) -> dict: + """Parse configuration for Stable-baselines3 agent based on inputs. + + Args: + task_name (str): The name of the environment. + + Returns: + dict: A dictionary containing the parsed configuration. + """ + # retrieve the default environment config file + try: + config_file = SB3_PPO_CONFIG_FILE[task_name] + except KeyError: + raise ValueError(f"Task not found: {task_name}. Configurations exist for {SB3_PPO_CONFIG_FILE.keys()}") + + # parse agent configuration + with open(config_file) as f: + cfg = yaml.load(f, Loader=yaml.FullLoader) + + # post-process certain arguments + # reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/0e5eb145faefa33e7d79c7f8c179788574b20da5/utils/exp_manager.py#L358 + for kwargs_key in ["policy_kwargs", "replay_buffer_class", "replay_buffer_kwargs"]: + if kwargs_key in cfg and isinstance(cfg[kwargs_key], str): + cfg[kwargs_key] = eval(cfg[kwargs_key]) + + return cfg diff --git a/source/standalone/workflows/sb3/play.py b/source/standalone/workflows/sb3/play.py new file mode 100644 index 0000000000..4da1fa4342 --- /dev/null +++ b/source/standalone/workflows/sb3/play.py @@ -0,0 +1,93 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to play a checkpoint if an RL agent from Stable-Baselines3.""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +import numpy as np + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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("--checkpoint", type=str, default=None, help="Path to model checkpoint.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything follows.""" + + +import gym + +from stable_baselines3 import PPO +from stable_baselines3.common.vec_env import VecNormalize + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils.parse_cfg import parse_env_cfg +from omni.isaac.orbit_envs.utils.wrappers.sb3 import Sb3VecEnvWrapper + +from config import parse_sb3_cfg + + +def main(): + """Play with stable-baselines agent.""" + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + # wrap around environment for stable baselines + env = Sb3VecEnvWrapper(env) + # parse agent configuration + agent_cfg = parse_sb3_cfg(args_cli.task) + + # normalize environment (if needed) + if "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, + ) + + # check checkpoint is valid + if args_cli.checkpoint is None: + raise ValueError("Checkpoint path is not valid.") + # create agent from stable baselines + print(f"Loading checkpoint from: {args_cli.checkpoint}") + agent = PPO.load(args_cli.checkpoint, env, print_system_info=True) + + # reset environment + obs = env.reset() + # simulate environment + while simulation_app.is_running(): + # agent stepping + actions, _ = agent.predict(obs, deterministic=True) + # env stepping + obs, _, _, _ = env.step(actions) + # check if simulator is stopped + if env.unwrapped.sim.is_stopped(): + break + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/standalone/workflows/sb3/train.py b/source/standalone/workflows/sb3/train.py new file mode 100644 index 0000000000..69e2dd8f3d --- /dev/null +++ b/source/standalone/workflows/sb3/train.py @@ -0,0 +1,119 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to train RL agent with Stable Baselines3. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +import numpy as np +import os + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +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("--seed", type=int, default=None, help="Seed used for the environment") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +# load cheaper kit config in headless +if args_cli.headless: + app_experience = f"{os.environ['EXP_PATH']}/omni.isaac.sim.python.gym.headless.kit" +else: + app_experience = f"{os.environ['EXP_PATH']}/omni.isaac.sim.python.kit" +# launch the simulator +simulation_app = SimulationApp(config, experience=app_experience) + +"""Rest everything follows.""" + + +import gym +import os +from datetime import datetime + +from stable_baselines3 import PPO +from stable_baselines3.common.callbacks import CheckpointCallback +from stable_baselines3.common.logger import configure +from stable_baselines3.common.vec_env import VecNormalize + +from omni.isaac.orbit.utils.io import dump_pickle, dump_yaml + +import omni.isaac.contrib_envs # noqa: F401 +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils import parse_env_cfg +from omni.isaac.orbit_envs.utils.wrappers.sb3 import Sb3VecEnvWrapper + +from config import parse_sb3_cfg + + +def main(): + """Train with stable-baselines agent.""" + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + agent_cfg = parse_sb3_cfg(args_cli.task) + # override configuration with command line arguments + if args_cli.seed is not None: + agent_cfg["seed"] = args_cli.seed + + # directory for logging into + log_dir = os.path.join("logs", "sb3", args_cli.task, datetime.now().strftime("%b%d_%H-%M-%S")) + # 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) + + # read configurations about the agent-training + policy_arch = agent_cfg.pop("policy") + n_timesteps = agent_cfg.pop("n_timesteps") + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) + # wrap around environment for stable baselines + env = Sb3VecEnvWrapper(env) + # set the seed + env.seed(seed=agent_cfg["seed"]) + + if "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 + agent = PPO(policy_arch, env, verbose=1, **agent_cfg) + # configure the logger + new_logger = configure(log_dir, ["stdout", "tensorboard"]) + agent.set_logger(new_logger) + + # callbacks for agent + checkpoint_callback = CheckpointCallback(save_freq=100, save_path=log_dir, name_prefix="model", verbose=2) + # train the agent + agent.learn(total_timesteps=n_timesteps, callback=checkpoint_callback) + # save the final model + agent.save(os.path.join(log_dir, "model")) + + # close the simulator + env.close() + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/source/tools/blender_obj.py b/source/tools/blender_obj.py new file mode 100644 index 0000000000..3d8aac3c46 --- /dev/null +++ b/source/tools/blender_obj.py @@ -0,0 +1,105 @@ +#!/usr/bin/python +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Convert a mesh file to `.obj` using blender + +This file processes a given dae mesh file and saves the resulting mesh file in obj format. + +It needs to be called using the python packaged with blender, i.e.: + + blender --background --python blender_obj.py -- -in_file FILE -out_file FILE + +For more information: https://docs.blender.org/api/current/index.html + +The script was tested on Blender 3.2 on Ubuntu 20.04LTS. +""" + + +import bpy +import os +import sys + + +def parse_cli_args(): + """Parse the input command line arguments. + + Reference: https://developer.blender.org/diffusion/B/browse/master/release/scripts/templates_py/background_job.py + """ + import argparse + + # get the args passed to blender after "--", all of which are ignored by + # blender so scripts may receive their own arguments + argv = sys.argv + + if "--" not in argv: + argv = [] # as if no args are passed + else: + argv = argv[argv.index("--") + 1 :] # get all args after "--" + + # When --help or no args are given, print this help + usage_text = ( + "Run blender in background mode with this script:\n" + "\tblender --background --python " + __file__ + " -- [options]" + ) + parser = argparse.ArgumentParser(description=usage_text) + # Add arguments + parser.add_argument("-i", "--in_file", metavar="FILE", type=str, required=True, help="Path to input OBJ file.") + parser.add_argument("-o", "--out_file", metavar="FILE", type=str, required=True, help="Path to output OBJ file.") + args = parser.parse_args(argv) + # Check if any arguments provided + if not argv or not args.in_file or not args.out_file: + parser.print_help() + return None + # return arguments + return args + + +def convert_to_obj(in_file: str, out_file: str, save_usd: bool = False): + """Convert a mesh file to `.obj` using blender + + Args: + in_file (str): Input mesh file to process. + out_file (str): Path to store output obj file. + """ + # check valid input file + if not os.path.exists(in_file): + raise FileNotFoundError(in_file) + # add ending of file format + if not out_file.endswith(".obj"): + out_file += ".obj" + # create directory if it doesn't exist for destination file + if not os.path.exists(os.path.dirname(out_file)): + os.makedirs(os.path.dirname(out_file), exist_ok=True) + # reset scene to empty + bpy.ops.wm.read_factory_settings(use_empty=True) + # load object into scene + if in_file.endswith(".dae"): + bpy.ops.wm.collada_import(filepath=in_file) + elif in_file.endswith(".stl") or in_file.endswith(".STL"): + bpy.ops.import_mesh.stl(filepath=in_file) + else: + raise ValueError(f"Input file not in dae/stl format: {in_file}") + # convert to obj format and store with z up + # TODO: Read the convention from dae file instead of manually fixing it. + # Reference: https://docs.blender.org/api/2.79/bpy.ops.export_scene.html + bpy.ops.export_scene.obj( + filepath=out_file, check_existing=False, axis_forward="Y", axis_up="Z", global_scale=1, path_mode="RELATIVE" + ) + # save it as usd as well + if save_usd: + out_file = out_file.replace("obj", "usd") + bpy.ops.wm.usd_export(filepath=out_file, check_existing=False) + + +if __name__ == "__main__": + # read arguments + cli_args = parse_cli_args() + # check CLI args + if cli_args is None: + sys.exit() + # process via blender + convert_to_obj(cli_args.in_file, cli_args.out_file) diff --git a/source/tools/check_instanceable.py b/source/tools/check_instanceable.py new file mode 100644 index 0000000000..594eb08292 --- /dev/null +++ b/source/tools/check_instanceable.py @@ -0,0 +1,131 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script uses the cloner API to check if asset has been instanced properly. + +Usage with different inputs (replace `` and `` with the path to the +original asset and the instanced asset respectively): + +```bash +./orbit.sh -p source/tools/check_instanceable.py -n 4096 --headless --physics +./orbit.sh -p source/tools/check_instanceable.py -n 4096 --headless --physics +./orbit.sh -p source/tools/check_instanceable.py -n 4096 --headless +./orbit.sh -p source/tools/check_instanceable.py -n 4096 --headless +``` + +Output from the above commands: + +```bash +>>> Cloning time (cloner.clone): 0.648198 seconds +>>> Setup time (sim.reset): : 5.843589 seconds +[#clones: 4096, physics: True] Asset: : 6.491870 seconds + +>>> Cloning time (cloner.clone): 0.693133 seconds +>>> Setup time (sim.reset): 50.860526 seconds +[#clones: 4096, physics: True] Asset: : 51.553743 seconds + +>>> Cloning time (cloner.clone) : 0.687201 seconds +>>> Setup time (sim.reset) : 6.302215 seconds +[#clones: 4096, physics: False] Asset: : 6.989500 seconds + +>>> Cloning time (cloner.clone) : 0.678150 seconds +>>> Setup time (sim.reset) : 52.854054 seconds +[#clones: 4096, physics: False] Asset: : 53.532287 seconds +``` + +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +# omni-isaac-orbit +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Utility to empirically check if asset in instanced properly.") +parser.add_argument("input", type=str, help="The path to the USD file.") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("-n", "--num_clones", type=int, default=128, help="Number of clones to spawn.") +parser.add_argument("-s", "--spacing", type=float, default=1.5, help="Spacing between instances in a grid.") +parser.add_argument("-p", "--physics", action="store_true", default=False, help="Clone assets using physics cloner.") +args_cli = parser.parse_args() + +# launch omniverse app +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + + +"""Rest everything follows.""" + + +import omni.isaac.core.utils.prims as prim_utils +from omni.isaac.cloner import GridCloner +from omni.isaac.core.simulation_context import SimulationContext +from omni.isaac.core.utils.carb import set_carb_setting + +from omni.isaac.orbit.utils import Timer +from omni.isaac.orbit.utils.nucleus import check_file_path + + +def main(): + """Spawns the USD asset robot and clones it using Isaac Gym Cloner API.""" + + # check valid file path + 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" + ) + # enable flatcache 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: + sim.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(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + + # Create interface to clone the scene + cloner = GridCloner(spacing=args_cli.spacing) + cloner.define_base_env("/World/envs") + prim_utils.define_prim("/World/envs/env_0") + # Spawn things into stage + prim_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=args_cli.input) + # Clone the scene + num_clones = args_cli.num_clones + + # Create a timer to measure the cloning time + with Timer(f"[#clones: {num_clones}, physics: {args_cli.physics}] Asset: {args_cli.input}"): + # Clone the scene + with Timer(">>> Cloning time (cloner.clone)"): + cloner.define_base_env("/World/envs") + envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_clones) + _ = cloner.clone( + source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=args_cli.physics + ) + # Play the simulator + with Timer(">>> Setup time (sim.reset)"): + sim.reset() + + # Simulate scene (if not headless) + if not args_cli.headless: + try: + while True: + # perform step + sim.step() + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + # Run cloning example + main() + # Close the simulator + simulation_app.close() diff --git a/source/tools/process_meshes_to_obj.py b/source/tools/process_meshes_to_obj.py new file mode 100644 index 0000000000..f9fe0504a3 --- /dev/null +++ b/source/tools/process_meshes_to_obj.py @@ -0,0 +1,96 @@ +#!/usr/bin/python +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Convert all mesh files to `.obj` in given folders.""" + +import argparse +import os +import shutil +import subprocess +from typing import List + +# Constants +# Path to blender +BLENDER_EXE_PATH = shutil.which("blender") + + +def parse_cli_args(): + """Parse the input command line arguments. + + Reference: https://developer.blender.org/diffusion/B/browse/master/release/scripts/templates_py/background_job.py + """ + # add argparse arguments + parser = argparse.ArgumentParser("Utility to convert all mesh files to `.obj` in given folders.") + parser.add_argument("input_dir", type=str, help="The input directory from which to load meshes.") + parser.add_argument( + "-o", + "--output_dir", + type=str, + default=None, + help="The output directory to save converted meshes into. Default is same as input directory.", + ) + args_cli = parser.parse_args() + # resolve output directory + if args_cli.output_dir is None: + args_cli.output_dir = args_cli.input_dir + # return arguments + return args_cli + + +def run_blender_convert2obj(in_file: str, out_file: str): + """Calls the python script using `subprocess` to perform processing of mesh file. + + Args: + in_file (str): Input mesh file. + out_file (str): Output obj file. + format (str): The file type to import. + """ + # resolve for python file + tools_dirname = os.path.dirname(os.path.abspath(__file__)) + script_file = os.path.join(tools_dirname, "blender_obj.py") + # complete command + command_exe = f"{BLENDER_EXE_PATH} --background --python {script_file} -- -i {in_file} -o {out_file}" + # break command into list + command_exe_list = command_exe.split(" ") + # run command + subprocess.run(command_exe_list) + + +def convert_meshes(source_folders: List[str], destination_folders: List[str]): + """Processes all mesh files of supported format into OBJ file using blender. + + Args: + source_folders (List[str]): List of directories to search for meshes. + destination_folders (List[str]): List of directories to dump converted files. + """ + # create folder for corresponding destination + for folder in destination_folders: + os.makedirs(folder, exist_ok=True) + # iterate over each folder + for in_folder, out_folder in zip(source_folders, destination_folders): + # extract all dae files in the directory + mesh_filenames = [f for f in os.listdir(in_folder) if f.endswith("dae")] + mesh_filenames += [f for f in os.listdir(in_folder) if f.endswith("stl")] + mesh_filenames += [f for f in os.listdir(in_folder) if f.endswith("STL")] + # print status + print(f"Found {len(mesh_filenames)} files to process in directory: {in_folder}") + # iterate over each OBJ file + for mesh_file in mesh_filenames: + # extract meshname + mesh_name = os.path.splitext(mesh_file)[0] + # complete path of input and output files + in_file_path = os.path.join(in_folder, mesh_file) + out_file_path = os.path.join(out_folder, mesh_name + ".obj") + # perform blender processing + print("Processing: ", in_file_path) + run_blender_convert2obj(in_file_path, out_file_path) + + +if __name__ == "__main__": + # Parse command line arguments + args = parse_cli_args() + # Run conversion + convert_meshes(args.input_dir, args.output_dir)