From cf81655ef968d44115372c2cdf50de2a03761948 Mon Sep 17 00:00:00 2001 From: jehee Date: Thu, 5 Sep 2024 00:49:43 +0900 Subject: [PATCH 01/12] modified to run without stride sim --- scripts/rsl_rl/play.py | 8 ++++---- scripts/rsl_rl/train.py | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/scripts/rsl_rl/play.py b/scripts/rsl_rl/play.py index 1144555..6e5998f 100644 --- a/scripts/rsl_rl/play.py +++ b/scripts/rsl_rl/play.py @@ -42,7 +42,7 @@ from rsl_rl.runners import OnPolicyRunner # Import extensions to set up environment tasks -import StrideSim.tasks # noqa: F401 +# import StrideSim.tasks # noqa: F401 from omni.isaac.lab.utils.dict import print_dict from omni.isaac.lab_tasks.utils import get_checkpoint_path, parse_env_cfg @@ -53,7 +53,7 @@ def main(): """Play with RSL-RL agent.""" # parse configuration env_cfg = parse_env_cfg( - args_cli.task, use_gpu=True, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric + args_cli.task, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric ) agent_cfg: RslRlOnPolicyRunnerCfg = cli_args.parse_rsl_rl_cfg(args_cli.task, args_cli) @@ -65,8 +65,8 @@ def main(): log_dir = os.path.dirname(resume_path) # create isaac environment - env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) - # wrap for video recording +env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) +# wrap for video recording if args_cli.video: video_kwargs = { "video_folder": os.path.join(log_dir, "videos", "play"), diff --git a/scripts/rsl_rl/train.py b/scripts/rsl_rl/train.py index 3c9b3cb..b382693 100644 --- a/scripts/rsl_rl/train.py +++ b/scripts/rsl_rl/train.py @@ -44,7 +44,7 @@ from rsl_rl.runners import OnPolicyRunner # Import extensions to set up environment tasks -import StrideSim.tasks # noqa: F401 +# import StrideSim.tasks # noqa: F401 from omni.isaac.lab.envs import ManagerBasedRLEnvCfg from omni.isaac.lab.utils.dict import print_dict @@ -62,7 +62,7 @@ def main(): """Train with RSL-RL agent.""" # parse configuration env_cfg: ManagerBasedRLEnvCfg = parse_env_cfg( - args_cli.task, use_gpu=True, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric + args_cli.task, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric ) agent_cfg: RslRlOnPolicyRunnerCfg = cli_args.parse_rsl_rl_cfg(args_cli.task, args_cli) From 35461a62bdda33aaf9997de0f3d670ac2c29695e Mon Sep 17 00:00:00 2001 From: Kim Jinwon Date: Sat, 7 Sep 2024 17:11:36 +0900 Subject: [PATCH 02/12] Doc/readme (#50) * doc: write readme and formatting * chore: move exclude command to .pre-commit-config.yaml --- .vscode/settings.json | 2 +- README.md | 77 ++++++-------------- README_backup.md | 82 ++++++++++++++++++++++ exts/StrideSim/StrideSim/tasks/__init__.py | 2 +- exts/StrideSim/StrideSim/ui.py | 20 ++++-- pyproject.toml | 1 + 6 files changed, 120 insertions(+), 64 deletions(-) create mode 100644 README_backup.md diff --git a/.vscode/settings.json b/.vscode/settings.json index 46d9a84..2908300 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,3 +1,3 @@ { "ros.distro": "humble" -} \ No newline at end of file +} diff --git a/README.md b/README.md index 74dcd90..9981e2e 100644 --- a/README.md +++ b/README.md @@ -1,82 +1,47 @@ -# Template for Isaac Lab Projects +# StrideSim [![IsaacSim](https://img.shields.io/badge/IsaacSim-4.0.0-silver.svg)](https://docs.omniverse.nvidia.com/isaacsim/latest/overview.html) [![Isaac Lab](https://img.shields.io/badge/IsaacLab-1.0.0-silver)](https://isaac-sim.github.io/IsaacLab) [![Python](https://img.shields.io/badge/python-3.10-blue.svg)](https://docs.python.org/3/whatsnew/3.10.html) -[![Linux platform](https://img.shields.io/badge/platform-linux--64-orange.svg)](https://releases.ubuntu.com/20.04/) -[![Windows platform](https://img.shields.io/badge/platform-windows--64-orange.svg)](https://www.microsoft.com/en-us/) -[![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit&logoColor=white)](https://pre-commit.com/) [![License](https://img.shields.io/badge/license-MIT-yellow.svg)](https://opensource.org/license/mit) -## Overview +## 개요 -This repository serves as a template for building projects or extensions based on Isaac Lab. It allows you to develop in an isolated environment, outside of the core Isaac Lab repository. +StrideSim은 Isaac Lab을 기반으로 한 프로젝트입니다. 이 저장소는 Isaac Lab의 핵심 저장소 외부에서 독립적인 환경에서 개발할 수 있도록 설계되었습니다. -**Key Features:** +## 설치 -- `Isolation` Work outside the core Isaac Lab repository, ensuring that your development efforts remain self-contained. -- `Flexibility` This template is set up to allow your code to be run as an extension in Omniverse. +1. Isaac Sim 설치: [설치 가이드](https://docs.omniverse.nvidia.com/isaacsim/latest/overview.html) 참조 -**Keywords:** extension, template, isaaclab +2. Isaac Lab 설치: [설치 가이드](https://isaac-sim.github.io/IsaacLab/source/setup/installation/index.html) 참조 +3. 라이브러리 설치: -### Installation + ```bash + cd exts/StrideSim + python -m pip install -e . + ``` +## 사용법 -- Throughout the repository, the name `StrideSim` only serves as an example and we provide a script to rename all the references to it automatically: +TODO -``` -# Rename all occurrences of StrideSim (in files/directories) to your_fancy_extension_name -python scripts/rename_template.py your_fancy_extension_name -``` - -- Install Isaac Lab, see the [installation guide](https://isaac-sim.github.io/IsaacLab/source/setup/installation/index.html). - -- Using a python interpreter that has Isaac Lab installed, install the library - -``` -cd exts/StrideSim -python -m pip install -e . -``` - -#### Set up IDE (Optional) - -To setup the IDE, please follow these instructions: - -- Run VSCode Tasks, by pressing `Ctrl+Shift+P`, selecting `Tasks: Run Task` and running the `setup_python_env` in the drop down menu. When running this task, you will be prompted to add the absolute path to your Isaac Lab installation. +## 코드 포맷팅 -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. +pre-commit 훅을 사용하여 코드 포맷팅을 자동화합니다. - -#### Setup as Omniverse Extension (Optional) - -We provide an example UI extension that will load upon enabling your extension defined in `exts/StrideSim/StrideSim/ui_extension_example.py`. For more information on UI extensions, enable and check out the source code of the `omni.isaac.ui_template` extension and refer to the introduction on [Isaac Sim Workflows 1.2.3. GUI](https://docs.omniverse.nvidia.com/isaacsim/latest/introductory_tutorials/tutorial_intro_workflows.html#gui). - -To enable your extension, follow these steps: - -1. **Add the search path of your repository** to the extension manager: - - Navigate to the extension manager using `Window` -> `Extensions`. - - Click on the **Hamburger Icon** (☰), then go to `Settings`. - - In the `Extension Search Paths`, enter the absolute path to `IsaacLabExtensionTemplate/exts` - - If not already present, in the `Extension Search Paths`, enter the path that leads to Isaac Lab's extension directory directory (`IsaacLab/source/extensions`) - - Click on the **Hamburger Icon** (☰), then click `Refresh`. - -2. **Search and enable your extension**: - - Find your extension under the `Third Party` category. - - Toggle it to enable your extension. - - -## Code formatting - -We have a pre-commit template to automatically format your code. -To install pre-commit: +pre-commit 설치: ```bash pip install pre-commit ``` -Then you can run pre-commit with: +pre-commit 실행: ```bash pre-commit run --all-files ``` + +## 라이선스 + +이 프로젝트는 MIT 라이선스 하에 배포됩니다. diff --git a/README_backup.md b/README_backup.md new file mode 100644 index 0000000..74dcd90 --- /dev/null +++ b/README_backup.md @@ -0,0 +1,82 @@ +# Template for Isaac Lab Projects + +[![IsaacSim](https://img.shields.io/badge/IsaacSim-4.0.0-silver.svg)](https://docs.omniverse.nvidia.com/isaacsim/latest/overview.html) +[![Isaac Lab](https://img.shields.io/badge/IsaacLab-1.0.0-silver)](https://isaac-sim.github.io/IsaacLab) +[![Python](https://img.shields.io/badge/python-3.10-blue.svg)](https://docs.python.org/3/whatsnew/3.10.html) +[![Linux platform](https://img.shields.io/badge/platform-linux--64-orange.svg)](https://releases.ubuntu.com/20.04/) +[![Windows platform](https://img.shields.io/badge/platform-windows--64-orange.svg)](https://www.microsoft.com/en-us/) +[![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit&logoColor=white)](https://pre-commit.com/) +[![License](https://img.shields.io/badge/license-MIT-yellow.svg)](https://opensource.org/license/mit) + +## Overview + +This repository serves as a template for building projects or extensions based on Isaac Lab. It allows you to develop in an isolated environment, outside of the core Isaac Lab repository. + +**Key Features:** + +- `Isolation` Work outside the core Isaac Lab repository, ensuring that your development efforts remain self-contained. +- `Flexibility` This template is set up to allow your code to be run as an extension in Omniverse. + +**Keywords:** extension, template, isaaclab + + +### Installation + + +- Throughout the repository, the name `StrideSim` only serves as an example and we provide a script to rename all the references to it automatically: + +``` +# Rename all occurrences of StrideSim (in files/directories) to your_fancy_extension_name +python scripts/rename_template.py your_fancy_extension_name +``` + +- Install Isaac Lab, see the [installation guide](https://isaac-sim.github.io/IsaacLab/source/setup/installation/index.html). + +- Using a python interpreter that has Isaac Lab installed, install the library + +``` +cd exts/StrideSim +python -m pip install -e . +``` + +#### Set up IDE (Optional) + +To setup the IDE, please follow these instructions: + +- Run VSCode Tasks, by pressing `Ctrl+Shift+P`, selecting `Tasks: Run Task` and running the `setup_python_env` in the drop down menu. When running this task, you will be prompted to add the absolute path to your Isaac Lab installation. + +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. + + +#### Setup as Omniverse Extension (Optional) + +We provide an example UI extension that will load upon enabling your extension defined in `exts/StrideSim/StrideSim/ui_extension_example.py`. For more information on UI extensions, enable and check out the source code of the `omni.isaac.ui_template` extension and refer to the introduction on [Isaac Sim Workflows 1.2.3. GUI](https://docs.omniverse.nvidia.com/isaacsim/latest/introductory_tutorials/tutorial_intro_workflows.html#gui). + +To enable your extension, follow these steps: + +1. **Add the search path of your repository** to the extension manager: + - Navigate to the extension manager using `Window` -> `Extensions`. + - Click on the **Hamburger Icon** (☰), then go to `Settings`. + - In the `Extension Search Paths`, enter the absolute path to `IsaacLabExtensionTemplate/exts` + - If not already present, in the `Extension Search Paths`, enter the path that leads to Isaac Lab's extension directory directory (`IsaacLab/source/extensions`) + - Click on the **Hamburger Icon** (☰), then click `Refresh`. + +2. **Search and enable your extension**: + - Find your extension under the `Third Party` category. + - Toggle it to enable your extension. + + +## Code formatting + +We have a pre-commit template to automatically format your code. +To install pre-commit: + +```bash +pip install pre-commit +``` + +Then you can run pre-commit with: + +```bash +pre-commit run --all-files +``` diff --git a/exts/StrideSim/StrideSim/tasks/__init__.py b/exts/StrideSim/StrideSim/tasks/__init__.py index 9ea3fdf..71fd5f8 100644 --- a/exts/StrideSim/StrideSim/tasks/__init__.py +++ b/exts/StrideSim/StrideSim/tasks/__init__.py @@ -3,7 +3,7 @@ import os import toml -#FIXME: 무작정 주석처리해버림, 무슨 문제가 생길지 모름 +# FIXME: 무작정 주석처리해버림, 무슨 문제가 생길지 모름 # from omni.isaac.lab_tasks.utils import import_packages diff --git a/exts/StrideSim/StrideSim/ui.py b/exts/StrideSim/StrideSim/ui.py index 977292c..df415c8 100644 --- a/exts/StrideSim/StrideSim/ui.py +++ b/exts/StrideSim/StrideSim/ui.py @@ -1,6 +1,7 @@ -import omni.ext import subprocess +import omni.ext + # Functions and vars are available to other extension as usual in python: `example.python_ext.some_public_function(x)` def some_public_function(x: int): @@ -23,11 +24,19 @@ def on_startup(self, ext_id): label = omni.ui.Label("") def on_train(): - label.text = f"start training" - + label.text = "start training" + # Execute the specified Python command - subprocess.run(["python", "scripts/rsl_rl/train.py", "--task", "Isaac-Velocity-Rough-Anymal-D-v0", "--headless"]) - + subprocess.run( + [ + "python", + "scripts/rsl_rl/train.py", + "--task", + "Isaac-Velocity-Rough-Anymal-D-v0", + "--headless", + ] + ) + def on_play(): label.text = "empty" @@ -35,6 +44,5 @@ def on_play(): omni.ui.Button("Train", clicked_fn=on_train) omni.ui.Button("Reset", clicked_fn=on_play) - def on_shutdown(self): print("[StrideSim] shutdown") diff --git a/pyproject.toml b/pyproject.toml index 23f7603..04a4697 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -64,6 +64,7 @@ exclude = [ "**/logs", ".git", ".vscode", + "3rdparty/.*" ] typeCheckingMode = "basic" From 206c3b1725de2dbf14a4fd4ef01edb3597d169c2 Mon Sep 17 00:00:00 2001 From: Kim Jinwon Date: Sat, 7 Sep 2024 17:29:10 +0900 Subject: [PATCH 03/12] Doc/GitHub (#51) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * doc: issue and pr 포맷 정리와 코드 포매팅 workflow 적용 * chore: devel branch도 workflow 수행 --- .github/ISSUE_TEMPLATE/bug_report.md | 31 +++++++++++++++++++++++ .github/ISSUE_TEMPLATE/feature_request.md | 20 +++++++++++++++ .github/pull_request_template.md | 27 ++++++++++++++++++++ .github/workflows/code-quality.yml | 30 ++++++++++++++++++++++ 4 files changed, 108 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/bug_report.md create mode 100644 .github/ISSUE_TEMPLATE/feature_request.md create mode 100644 .github/pull_request_template.md create mode 100644 .github/workflows/code-quality.yml diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000..9d8f3ac --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,31 @@ +--- +name: 버그 리포트 +about: 버그를 보고하여 개선에 도움을 주세요 +title: '[버그] ' +labels: 버그 +assignees: '' + +--- + +**버그 설명** +버그에 대한 명확하고 간결한 설명을 해주세요. + +**재현 방법** +버그를 재현하는 단계: +1. '...'로 이동 +2. '....'를 클릭 +3. '....'까지 스크롤 +4. 오류 확인 + +**예상 동작** +예상했던 동작에 대한 명확하고 간결한 설명을 해주세요. + +**스크린샷** +해당되는 경우, 문제를 설명하는 데 도움이 되는 스크린샷을 추가해주세요. + +**환경 (다음 정보를 작성해주세요):** + - OS: [예: iOS] + - 버전 [예: 22] + +**추가 정보** +문제에 대한 다른 맥락이나 정보를 여기에 추가해주세요. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000..fe47b87 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: 기능 제안 +about: 이 프로젝트에 대한 아이디어를 제안해주세요 +title: '[기능] ' +labels: 개선 +assignees: '' + +--- + +**제안하는 기능이 어떤 문제와 관련이 있나요? 설명해주세요.** +문제에 대한 명확하고 간결한 설명을 해주세요. 예: 항상 [...] 때문에 불편합니다. + +**원하는 해결책을 설명해주세요** +원하는 결과에 대한 명확하고 간결한 설명을 해주세요. + +**고려한 대안을 설명해주세요** +고려했던 다른 대안이나 기능에 대해 설명해주세요. + +**추가 정보** +기능 요청에 대한 다른 맥락이나 스크린샷을 여기에 추가해주세요. diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..1681a72 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,27 @@ +## 설명 +이 PR에서 변경한 내용에 대해 간단히 설명해주세요. + +## 관련 이슈 +해결하는 이슈 번호를 적어주세요. 예: #123 + +## 변경 유형 +- [ ] 버그 수정 +- [ ] 새로운 기능 +- [ ] 성능 개선 +- [ ] 코드 스타일 업데이트 (포맷팅, 변수명 등) +- [ ] 리팩토링 (기능적 변경 없음, API 변경 없음) +- [ ] 빌드 관련 변경 +- [ ] 문서 내용 변경 +- [ ] 기타 (설명해주세요): + +## 체크리스트: +- [ ] 내 코드가 프로젝트의 코드 스타일 가이드라인을 따릅니다 +- [ ] 내가 내 코드를 자체 리뷰했습니다 +- [ ] 내 변경사항으로 인해 새로운 경고가 발생하지 않습니다 +- [ ] 단위 테스트를 추가했습니다 +- [ ] 기존 단위 테스트가 모두 통과합니다 +- [ ] 필요한 경우 해당 문서를 변경했습니다 +- [ ] 내 변경사항에 대한 종속성 업데이트가 포함되어 있습니다 + +## 추가 정보 +PR에 대한 추가 정보나 스크린샷을 여기에 추가하세요. diff --git a/.github/workflows/code-quality.yml b/.github/workflows/code-quality.yml new file mode 100644 index 0000000..da48c33 --- /dev/null +++ b/.github/workflows/code-quality.yml @@ -0,0 +1,30 @@ +name: 코드 품질 검사 + +on: + push: + branches: [ main, devel ] + pull_request: + branches: [ main, devel ] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Python 설정 + uses: actions/setup-python@v3 + with: + python-version: '3.10' + - name: pre-commit 캐시 설정 + uses: actions/cache@v3 + with: + path: ~/.cache/pre-commit + key: ${{ runner.os }}-pre-commit-${{ hashFiles('.pre-commit-config.yaml') }} + restore-keys: | + ${{ runner.os }}-pre-commit- + - name: pre-commit 설치 + run: | + python -m pip install --upgrade pip + pip install pre-commit + - name: pre-commit 실행 + run: pre-commit run --all-files From 797c0c8c81653d4d7332043fa0101830ba3d696f Mon Sep 17 00:00:00 2001 From: Kim Jinwon Date: Sat, 7 Sep 2024 18:40:01 +0900 Subject: [PATCH 04/12] Feat/ext example (#52) * feat: add simple example * chore: fix typo and formatting --- exts/StrideSim/StrideSim/__init__.py | 9 +- .../StrideSim/base_sample/__init__.py | 2 + .../StrideSim/base_sample/base_sample.py | 127 ++++++++++ .../base_sample/base_sample_extension.py | 237 ++++++++++++++++++ exts/StrideSim/StrideSim/quadruped.py | 140 +++++++++++ .../StrideSim/quadruped_extension.py | 85 +++++++ exts/StrideSim/StrideSim/ui.py | 48 ---- 7 files changed, 597 insertions(+), 51 deletions(-) create mode 100644 exts/StrideSim/StrideSim/base_sample/__init__.py create mode 100644 exts/StrideSim/StrideSim/base_sample/base_sample.py create mode 100644 exts/StrideSim/StrideSim/base_sample/base_sample_extension.py create mode 100644 exts/StrideSim/StrideSim/quadruped.py create mode 100644 exts/StrideSim/StrideSim/quadruped_extension.py delete mode 100644 exts/StrideSim/StrideSim/ui.py diff --git a/exts/StrideSim/StrideSim/__init__.py b/exts/StrideSim/StrideSim/__init__.py index 00c80da..330ff89 100644 --- a/exts/StrideSim/StrideSim/__init__.py +++ b/exts/StrideSim/StrideSim/__init__.py @@ -2,8 +2,11 @@ Python module serving as a project/extension template. """ -# Register Gym environments. -from .tasks import * +from .base_sample import * # Register UI extensions. -from .ui import * +from .quadruped import * +from .quadruped_extension import * + +# Register Gym environments. +from .tasks import * diff --git a/exts/StrideSim/StrideSim/base_sample/__init__.py b/exts/StrideSim/StrideSim/base_sample/__init__.py new file mode 100644 index 0000000..50af2f7 --- /dev/null +++ b/exts/StrideSim/StrideSim/base_sample/__init__.py @@ -0,0 +1,2 @@ +from .base_sample import BaseSample +from .base_sample_extension import BaseSampleExtension diff --git a/exts/StrideSim/StrideSim/base_sample/base_sample.py b/exts/StrideSim/StrideSim/base_sample/base_sample.py new file mode 100644 index 0000000..7059751 --- /dev/null +++ b/exts/StrideSim/StrideSim/base_sample/base_sample.py @@ -0,0 +1,127 @@ +# Copyright (c) 2018-2023, 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. +# +import gc +from abc import abstractmethod + +from omni.isaac.core import World +from omni.isaac.core.scenes.scene import Scene +from omni.isaac.core.utils.stage import create_new_stage_async, update_stage_async + + +class BaseSample: + def __init__(self) -> None: + self._world = None + self._current_tasks = None + self._world_settings = {"physics_dt": 1.0 / 60.0, "stage_units_in_meters": 1.0, "rendering_dt": 1.0 / 60.0} + # self._logging_info = "" + return + + def get_world(self): + return self._world + + def set_world_settings(self, physics_dt=None, stage_units_in_meters=None, rendering_dt=None): + if physics_dt is not None: + self._world_settings["physics_dt"] = physics_dt + if stage_units_in_meters is not None: + self._world_settings["stage_units_in_meters"] = stage_units_in_meters + if rendering_dt is not None: + self._world_settings["rendering_dt"] = rendering_dt + return + + async def load_world_async(self): + """Function called when clicking load button""" + if World.instance() is None: + await create_new_stage_async() + self._world = World(**self._world_settings) + await self._world.initialize_simulation_context_async() + self.setup_scene() + else: + self._world = World.instance() + self._current_tasks = self._world.get_current_tasks() + await self._world.reset_async() + await self._world.pause_async() + await self.setup_post_load() + if len(self._current_tasks) > 0: + self._world.add_physics_callback("tasks_step", self._world.step_async) + return + + async def reset_async(self): + """Function called when clicking reset button""" + if self._world.is_tasks_scene_built() and len(self._current_tasks) > 0: + self._world.remove_physics_callback("tasks_step") + await self._world.play_async() + await update_stage_async() + await self.setup_pre_reset() + await self._world.reset_async() + await self._world.pause_async() + await self.setup_post_reset() + if self._world.is_tasks_scene_built() and len(self._current_tasks) > 0: + self._world.add_physics_callback("tasks_step", self._world.step_async) + return + + @abstractmethod + def setup_scene(self, scene: Scene) -> None: + """used to setup anything in the world, adding tasks happen here for instance. + + Args: + scene (Scene): [description] + """ + return + + @abstractmethod + async def setup_post_load(self): + """called after first reset of the world when pressing load, + initializing private variables happen here. + """ + return + + @abstractmethod + async def setup_pre_reset(self): + """called in reset button before resetting the world + to remove a physics callback for instance or a controller reset + """ + return + + @abstractmethod + async def setup_post_reset(self): + """called in reset button after resetting the world which includes one step with rendering""" + return + + @abstractmethod + async def setup_post_clear(self): + """called after clicking clear button + or after creating a new stage and clearing the instance of the world with its callbacks + """ + return + + # def log_info(self, info): + # self._logging_info += str(info) + "\n" + # return + + def _world_cleanup(self): + self._world.stop() + self._world.clear_all_callbacks() + self._current_tasks = None + self.world_cleanup() + return + + def world_cleanup(self): + """Function called when extension shutdowns and starts again, (hot reloading feature)""" + return + + async def clear_async(self): + """Function called when clicking clear button""" + await create_new_stage_async() + if self._world is not None: + self._world_cleanup() + self._world.clear_instance() + self._world = None + gc.collect() + await self.setup_post_clear() + return diff --git a/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py b/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py new file mode 100644 index 0000000..246dddf --- /dev/null +++ b/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py @@ -0,0 +1,237 @@ +# Copyright (c) 2018-2023, 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. +# + +import asyncio +import weakref +from abc import abstractmethod + +import omni.ext +import omni.ui as ui +from omni.isaac.core import World +from omni.isaac.examples.base_sample import BaseSample +from omni.isaac.ui.menu import make_menu_item_description +from omni.isaac.ui.ui_utils import btn_builder, get_style, setup_ui_headers # scrolling_frame_builder +from omni.kit.menu.utils import MenuItemDescription, add_menu_items, remove_menu_items + + +class BaseSampleExtension(omni.ext.IExt): + def on_startup(self, ext_id: str): + self._menu_items = None + self._buttons = None + self._ext_id = ext_id + self._sample = None + self._extra_frames = [] + return + + def start_extension( + self, + menu_name: str, + submenu_name: str, + name: str, + title: str, + doc_link: str, + overview: str, + file_path: str, + sample=None, + number_of_extra_frames=1, + window_width=350, + keep_window_open=False, + ): + if sample is None: + self._sample = BaseSample() + else: + self._sample = sample + + menu_items = [make_menu_item_description(self._ext_id, name, lambda a=weakref.proxy(self): a._menu_callback())] + if menu_name == "" or menu_name is None: + self._menu_items = menu_items + elif submenu_name == "" or submenu_name is None: + self._menu_items = [MenuItemDescription(name=menu_name, sub_menu=menu_items)] + else: + self._menu_items = [ + MenuItemDescription( + name=menu_name, sub_menu=[MenuItemDescription(name=submenu_name, sub_menu=menu_items)] + ) + ] + add_menu_items(self._menu_items, "Isaac Examples") + + self._buttons = dict() + self._build_ui( + name=name, + title=title, + doc_link=doc_link, + overview=overview, + file_path=file_path, + number_of_extra_frames=number_of_extra_frames, + window_width=window_width, + keep_window_open=keep_window_open, + ) + return + + @property + def sample(self): + return self._sample + + def get_frame(self, index): + if index >= len(self._extra_frames): + raise Exception(f"there were {len(self._extra_frames)} extra frames created only") + return self._extra_frames[index] + + def get_world(self): + return World.instance() + + def get_buttons(self): + return self._buttons + + def _build_ui( + self, name, title, doc_link, overview, file_path, number_of_extra_frames, window_width, keep_window_open + ): + self._window = omni.ui.Window( + name, width=window_width, height=0, visible=keep_window_open, dockPreference=ui.DockPreference.LEFT_BOTTOM + ) + with self._window.frame: + self._main_stack = ui.VStack(spacing=5, height=0) + with self._main_stack: + setup_ui_headers(self._ext_id, file_path, title, doc_link, overview) + self._controls_frame = ui.CollapsableFrame( + title="World Controls", + width=ui.Fraction(1), + height=0, + collapsed=False, + style=get_style(), + horizontal_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED, + vertical_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON, + ) + with ui.VStack(style=get_style(), spacing=5, height=0): + for i in range(number_of_extra_frames): + self._extra_frames.append( + ui.CollapsableFrame( + title="", + width=ui.Fraction(0.33), + height=0, + visible=False, + collapsed=False, + style=get_style(), + horizontal_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED, + vertical_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON, + ) + ) + with self._controls_frame: + with ui.VStack(style=get_style(), spacing=5, height=0): + dict = { + "label": "Load World", + "type": "button", + "text": "Load", + "tooltip": "Load World and Task", + "on_clicked_fn": self._on_load_world, + } + self._buttons["Load World"] = btn_builder(**dict) + self._buttons["Load World"].enabled = True + dict = { + "label": "Reset", + "type": "button", + "text": "Reset", + "tooltip": "Reset robot and environment", + "on_clicked_fn": self._on_reset, + } + self._buttons["Reset"] = btn_builder(**dict) + self._buttons["Reset"].enabled = False + return + + def _set_button_tooltip(self, button_name, tool_tip): + self._buttons[button_name].set_tooltip(tool_tip) + return + + def _on_load_world(self): + async def _on_load_world_async(): + await self._sample.load_world_async() + await omni.kit.app.get_app().next_update_async() + self._sample._world.add_stage_callback("stage_event_1", self.on_stage_event) + self._enable_all_buttons(True) + self._buttons["Load World"].enabled = False + self.post_load_button_event() + self._sample._world.add_timeline_callback("stop_reset_event", self._reset_on_stop_event) + + asyncio.ensure_future(_on_load_world_async()) + return + + def _on_reset(self): + async def _on_reset_async(): + await self._sample.reset_async() + await omni.kit.app.get_app().next_update_async() + self.post_reset_button_event() + + asyncio.ensure_future(_on_reset_async()) + return + + @abstractmethod + def post_reset_button_event(self): + return + + @abstractmethod + def post_load_button_event(self): + return + + @abstractmethod + def post_clear_button_event(self): + return + + def _enable_all_buttons(self, flag): + for btn_name, btn in self._buttons.items(): + if isinstance(btn, omni.ui._ui.Button): + btn.enabled = flag + return + + def _menu_callback(self): + self._window.visible = not self._window.visible + return + + def _on_window(self, status): + # if status: + return + + def on_shutdown(self): + self._extra_frames = [] + if self._sample._world is not None: + self._sample._world_cleanup() + if self._menu_items is not None: + self._sample_window_cleanup() + if self._buttons is not None: + self._buttons["Load World"].enabled = True + self._enable_all_buttons(False) + self.shutdown_cleanup() + return + + def shutdown_cleanup(self): + return + + def _sample_window_cleanup(self): + remove_menu_items(self._menu_items, "Isaac Examples") + self._window = None + self._menu_items = None + self._buttons = None + return + + def on_stage_event(self, event): + if event.type == int(omni.usd.StageEventType.CLOSED): + if World.instance() is not None: + self.sample._world_cleanup() + self.sample._world.clear_instance() + if hasattr(self, "_buttons"): + if self._buttons is not None: + self._enable_all_buttons(False) + self._buttons["Load World"].enabled = True + return + + def _reset_on_stop_event(self, e): + if e.type == int(omni.timeline.TimelineEventType.STOP): + self._buttons["Load World"].enabled = False + self._buttons["Reset"].enabled = True + self.post_clear_button_event() + return diff --git a/exts/StrideSim/StrideSim/quadruped.py b/exts/StrideSim/StrideSim/quadruped.py new file mode 100644 index 0000000..c77af12 --- /dev/null +++ b/exts/StrideSim/StrideSim/quadruped.py @@ -0,0 +1,140 @@ +# Copyright (c) 2020-2023, 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. +# + +import numpy as np + +import carb +import omni +import omni.appwindow # Contains handle to keyboard +from omni.isaac.quadruped.robots import Unitree + +from .base_sample import BaseSample + + +class QuadrupedExample(BaseSample): + def __init__(self) -> None: + super().__init__() + self._world_settings["stage_units_in_meters"] = 1.0 + self._world_settings["physics_dt"] = 1.0 / 400.0 + self._world_settings["rendering_dt"] = 5.0 / 400.0 + self._enter_toggled = 0 + self._base_command = [0.0, 0.0, 0.0, 0] + self._event_flag = False + + # bindings for keyboard to command + self._input_keyboard_mapping = { + # forward command + "NUMPAD_8": [5, 0.0, 0.0], + "UP": [5, 0.0, 0.0], + # back command + "NUMPAD_2": [-5, 0.0, 0.0], + "DOWN": [-5, 0.0, 0.0], + # left command + "NUMPAD_6": [0.0, -4.0, 0.0], + "RIGHT": [0.0, -4.0, 0.0], + # right command + "NUMPAD_4": [0.0, 4.0, 0.0], + "LEFT": [0.0, 4.0, 0.0], + # yaw command (positive) + "NUMPAD_7": [0.0, 0.0, 1.0], + "N": [0.0, 0.0, 1.0], + # yaw command (negative) + "NUMPAD_9": [0.0, 0.0, -1.0], + "M": [0.0, 0.0, -1.0], + } + return + + def setup_scene(self) -> None: + world = self.get_world() + self._world.scene.add_default_ground_plane( + z_position=0, + name="default_ground_plane", + prim_path="/World/defaultGroundPlane", + static_friction=0.2, + dynamic_friction=0.2, + restitution=0.01, + ) + self._a1 = world.scene.add( + Unitree( + prim_path="/World/A1", + name="A1", + position=np.array([0, 0, 0.400]), + physics_dt=self._world_settings["physics_dt"], + ) + ) + timeline = omni.timeline.get_timeline_interface() + self._event_timer_callback = timeline.get_timeline_event_stream().create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), self._timeline_timer_callback_fn + ) + return + + async def setup_post_load(self) -> None: + self._world = self.get_world() + self._appwindow = omni.appwindow.get_default_app_window() + self._input = carb.input.acquire_input_interface() + self._keyboard = self._appwindow.get_keyboard() + self._sub_keyboard = self._input.subscribe_to_keyboard_events(self._keyboard, self._sub_keyboard_event) + self._world.add_physics_callback("sending_actions", callback_fn=self.on_physics_step) + await self._world.play_async() + return + + async def setup_post_reset(self) -> None: + self._event_flag = False + await self._world.play_async() + self._a1.set_state(self._a1._default_a1_state) + self._a1.post_reset() + return + + def on_physics_step(self, step_size) -> None: + if self._event_flag: + self._a1._qp_controller.switch_mode() + self._event_flag = False + self._a1.advance(step_size, self._base_command) + + def _sub_keyboard_event(self, event, *args, **kwargs) -> bool: + """Subscriber callback to when kit is updated.""" + # reset event + self._event_flag = False + # when a key is pressedor released the command is adjusted w.r.t the key-mapping + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + # on pressing, the command is incremented + if event.input.name in self._input_keyboard_mapping: + self._base_command[0:3] += np.array(self._input_keyboard_mapping[event.input.name]) + self._event_flag = True + + # enter, toggle the last command + if event.input.name == "ENTER" and self._enter_toggled is False: + self._enter_toggled = True + if self._base_command[3] == 0: + self._base_command[3] = 1 + else: + self._base_command[3] = 0 + self._event_flag = True + + elif event.type == carb.input.KeyboardEventType.KEY_RELEASE: + # on release, the command is decremented + if event.input.name in self._input_keyboard_mapping: + self._base_command[0:3] -= np.array(self._input_keyboard_mapping[event.input.name]) + self._event_flag = True + # enter, toggle the last command + if event.input.name == "ENTER": + self._enter_toggled = False + # since no error, we are fine :) + return True + + def _timeline_timer_callback_fn(self, event) -> None: + if self._a1: + self._a1.post_reset() + return + + def world_cleanup(self): + self._event_timer_callback = None + if self._world.physics_callback_exists("sending_actions"): + self._world.remove_physics_callback("sending_actions") + return diff --git a/exts/StrideSim/StrideSim/quadruped_extension.py b/exts/StrideSim/StrideSim/quadruped_extension.py new file mode 100644 index 0000000..e789fe7 --- /dev/null +++ b/exts/StrideSim/StrideSim/quadruped_extension.py @@ -0,0 +1,85 @@ +import os + +from .base_sample import BaseSampleExtension +from .quadruped import QuadrupedExample + + +class StrideSimExtension(BaseSampleExtension): + def on_startup(self, ext_id: str): + super().on_startup(ext_id) + + overview = "This Example shows quadruped simulation in Isaac Sim. Currently there is a performance issue with " + overview += ( + "the quadruped gait controller; it's being investigated and will be improved in an upcoming release." + ) + overview += "\n\tKeybord Input:" + overview += "\n\t\tup arrow / numpad 8: Move Forward" + overview += "\n\t\tdown arrow/ numpad 2: Move Reverse" + overview += "\n\t\tleft arrow/ numpad 4: Move Left" + overview += "\n\t\tright arrow / numpad 6: Move Right" + overview += "\n\t\tN / numpad 7: Spin Counterclockwise" + overview += "\n\t\tM / numpad 9: Spin Clockwise" + + overview += "\n\nPress the 'Open in IDE' button to view the source code." + + super().start_extension( + menu_name="", + submenu_name="", + name="StrideSim", + title="Auturbo quadruped robot simulation", + doc_link="https://github.com/AuTURBO/StrideSim", + overview=overview, + file_path=os.path.abspath(__file__), + sample=QuadrupedExample(), + ) + return + + +# import subprocess + +# import omni.ext + + +# # Functions and vars are available to other extension as usual in python: `example.python_ext.some_public_function(x)` +# def some_public_function(x: int): +# print("[StrideSim] some_public_function was called with x: ", x) +# return x**x + + +# # Any class derived from `omni.ext.IExt` in top level module (defined in `python.modules` of `extension.toml`) will be +# # instantiated when extension gets enabled and `on_startup(ext_id)` will be called. Later when extension gets disabled +# # on_shutdown() is called. +# class ExampleExtension(omni.ext.IExt): +# # ext_id is current extension id. It can be used with extension manager to query additional information, like where +# # this extension is located on filesystem. +# def on_startup(self, ext_id): +# print("[StrideSim] startup") + +# self._window = omni.ui.Window("My Window", width=300, height=300) +# with self._window.frame: +# with omni.ui.VStack(): +# label = omni.ui.Label("") + +# def on_train(): +# label.text = "start training" + +# # Execute the specified Python command +# subprocess.run( +# [ +# "python", +# "scripts/rsl_rl/train.py", +# "--task", +# "Isaac-Velocity-Rough-Anymal-D-v0", +# "--headless", +# ] +# ) + +# def on_play(): +# label.text = "empty" + +# with omni.ui.HStack(): +# omni.ui.Button("Train", clicked_fn=on_train) +# omni.ui.Button("Reset", clicked_fn=on_play) + +# def on_shutdown(self): +# print("[StrideSim] shutdown") diff --git a/exts/StrideSim/StrideSim/ui.py b/exts/StrideSim/StrideSim/ui.py deleted file mode 100644 index df415c8..0000000 --- a/exts/StrideSim/StrideSim/ui.py +++ /dev/null @@ -1,48 +0,0 @@ -import subprocess - -import omni.ext - - -# Functions and vars are available to other extension as usual in python: `example.python_ext.some_public_function(x)` -def some_public_function(x: int): - print("[StrideSim] some_public_function was called with x: ", x) - return x**x - - -# Any class derived from `omni.ext.IExt` in top level module (defined in `python.modules` of `extension.toml`) will be -# instantiated when extension gets enabled and `on_startup(ext_id)` will be called. Later when extension gets disabled -# on_shutdown() is called. -class ExampleExtension(omni.ext.IExt): - # ext_id is current extension id. It can be used with extension manager to query additional information, like where - # this extension is located on filesystem. - def on_startup(self, ext_id): - print("[StrideSim] startup") - - self._window = omni.ui.Window("My Window", width=300, height=300) - with self._window.frame: - with omni.ui.VStack(): - label = omni.ui.Label("") - - def on_train(): - label.text = "start training" - - # Execute the specified Python command - subprocess.run( - [ - "python", - "scripts/rsl_rl/train.py", - "--task", - "Isaac-Velocity-Rough-Anymal-D-v0", - "--headless", - ] - ) - - def on_play(): - label.text = "empty" - - with omni.ui.HStack(): - omni.ui.Button("Train", clicked_fn=on_train) - omni.ui.Button("Reset", clicked_fn=on_play) - - def on_shutdown(self): - print("[StrideSim] shutdown") From 9dd306a99bd82e891533cfe4e9ad6e431387b947 Mon Sep 17 00:00:00 2001 From: jinwon kim Date: Mon, 9 Sep 2024 02:11:38 +0900 Subject: [PATCH 05/12] feat: make folder structure and add anymal c spawning(but file name is anyaml d yet --- .vscode/tasks.json | 20 ++ README.md | 13 + exts/StrideSim/StrideSim/__init__.py | 8 +- .../StrideSim/anymal_articulation.py | 243 ++++++++++++++++++ .../StrideSim/{quadruped.py => anymal_d.py} | 29 +-- .../StrideSim/StrideSim/anymal_d_extension.py | 35 +++ .../base_sample/base_sample_extension.py | 2 +- .../agents/__init__.py => network/ppo.py} | 0 exts/StrideSim/StrideSim/omnigraph.py | 0 .../StrideSim/quadruped_extension.py | 85 ------ exts/StrideSim/StrideSim/tasks/__init__.py | 18 -- rl/.gitignore | 4 + rl/StrideSim_RL/tasks/__init__.py | 18 ++ .../tasks/locomotion/__init__.py | 0 .../tasks/locomotion/velocity/__init__.py | 0 .../locomotion/velocity/config/__init__.py | 0 .../velocity/config/anymal_d/__init__.py | 0 .../config/anymal_d/agents/__init__.py | 0 .../config/anymal_d/agents/rsl_rl_ppo_cfg.py | 0 .../velocity/config/anymal_d/flat_env_cfg.py | 0 .../velocity/config/anymal_d/rough_env_cfg.py | 2 +- .../tasks/locomotion/velocity/mdp/__init__.py | 0 .../locomotion/velocity/mdp/curriculums.py | 0 .../tasks/locomotion/velocity/mdp/rewards.py | 0 .../locomotion/velocity/velocity_env_cfg.py | 2 +- {scripts/rsl_rl => rl}/cli_args.py | 0 rl/config/extension.toml | 8 + {scripts/rsl_rl => rl}/play.py | 2 +- {exts/StrideSim => rl}/setup.py | 4 +- {scripts/rsl_rl => rl}/train.py | 2 +- 30 files changed, 364 insertions(+), 131 deletions(-) create mode 100644 .vscode/tasks.json create mode 100644 exts/StrideSim/StrideSim/anymal_articulation.py rename exts/StrideSim/StrideSim/{quadruped.py => anymal_d.py} (87%) create mode 100644 exts/StrideSim/StrideSim/anymal_d_extension.py rename exts/StrideSim/StrideSim/{tasks/locomotion/velocity/config/anymal_d/agents/__init__.py => network/ppo.py} (100%) create mode 100644 exts/StrideSim/StrideSim/omnigraph.py delete mode 100644 exts/StrideSim/StrideSim/quadruped_extension.py delete mode 100644 exts/StrideSim/StrideSim/tasks/__init__.py create mode 100644 rl/.gitignore create mode 100644 rl/StrideSim_RL/tasks/__init__.py rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/__init__.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/__init__.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/config/__init__.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/config/anymal_d/__init__.py (100%) create mode 100644 rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/agents/__init__.py rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/config/anymal_d/flat_env_cfg.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/config/anymal_d/rough_env_cfg.py (93%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/mdp/__init__.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/mdp/curriculums.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/mdp/rewards.py (100%) rename {exts/StrideSim/StrideSim => rl/StrideSim_RL}/tasks/locomotion/velocity/velocity_env_cfg.py (99%) rename {scripts/rsl_rl => rl}/cli_args.py (100%) create mode 100644 rl/config/extension.toml rename {scripts/rsl_rl => rl}/play.py (99%) rename {exts/StrideSim => rl}/setup.py (95%) rename {scripts/rsl_rl => rl}/train.py (99%) diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..574c1c4 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,20 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "setup_python_env", + "type": "shell", + "linux": { + "command": "export CARB_APP_PATH=${input:isaacsim_path}/kit && export ISAAC_PATH=${input:isaacsim_path} && export EXP_PATH=${input:isaacsim_path}/apps && source ${input:isaacsim_path}/setup_python_env.sh && printenv >${workspaceFolder}/.vscode/.python.env && ${input:isaacsim_path}/python.sh ${workspaceFolder}/.vscode/tools/setup_vscode.py --isaacsim_path ${input:isaacsim_path}" + } + } + ], + "inputs": [ + { + "id": "isaacsim_path", + "description": "Absolute path to Isaac Sim:", + "default": "", + "type": "promptString" + } + ] +} diff --git a/README.md b/README.md index 9981e2e..d3023f4 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,19 @@ StrideSim은 Isaac Lab을 기반으로 한 프로젝트입니다. 이 저장소 ## 사용법 +1. 강화학습 라이브러리 설치 (Optional) + +```bash +cd rl +python -m pip install -e . +``` + +2. 강화학습 라이브러리 실행 + +```bash +python rl/train.py --task Template-Isaac-Velocity-Rough-Anymal-D-v0 +``` + TODO ## 코드 포맷팅 diff --git a/exts/StrideSim/StrideSim/__init__.py b/exts/StrideSim/StrideSim/__init__.py index 330ff89..336eb4a 100644 --- a/exts/StrideSim/StrideSim/__init__.py +++ b/exts/StrideSim/StrideSim/__init__.py @@ -4,9 +4,7 @@ from .base_sample import * -# Register UI extensions. -from .quadruped import * -from .quadruped_extension import * +from .anymal_articulation import AnymalD_Atriculation -# Register Gym environments. -from .tasks import * +from .anymal_d import AnymalD +from .anymal_d_extension import AnyamlDExtension \ No newline at end of file diff --git a/exts/StrideSim/StrideSim/anymal_articulation.py b/exts/StrideSim/StrideSim/anymal_articulation.py new file mode 100644 index 0000000..de1e42f --- /dev/null +++ b/exts/StrideSim/StrideSim/anymal_articulation.py @@ -0,0 +1,243 @@ +# Copyright (c) 2022-2023, 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. +# + +import io +from typing import List, Optional + +import carb +import numpy as np +import omni +import omni.kit.commands +import torch +from omni.isaac.core.articulations import Articulation +from omni.isaac.core.utils.prims import define_prim, get_prim_at_path +from omni.isaac.core.utils.rotations import euler_to_rot_matrix, quat_to_euler_angles, quat_to_rot_matrix +from omni.isaac.core.utils.stage import get_current_stage +from omni.isaac.nucleus import get_assets_root_path +from omni.isaac.quadruped.utils import LstmSeaNetwork +from pxr import Gf + + +class AnymalD_Atriculation(Articulation): + """The ANYmal quadruped""" + + def __init__( + self, + prim_path: str, + name: str = "anymal", + usd_path: Optional[str] = None, + position: Optional[np.ndarray] = None, + orientation: Optional[np.ndarray] = None, + ) -> None: + """ + [Summary] + + initialize robot, set up sensors and controller + + Args: + prim_path {str} -- prim path of the robot on the stage + name {str} -- name of the quadruped + usd_path {str} -- robot usd filepath in the directory + position {np.ndarray} -- position of the robot + orientation {np.ndarray} -- orientation of the robot + + """ + self._stage = get_current_stage() + self._prim_path = prim_path + prim = get_prim_at_path(self._prim_path) + + assets_root_path = get_assets_root_path() + if not prim.IsValid(): + prim = define_prim(self._prim_path, "Xform") + if usd_path: + prim.GetReferences().AddReference(usd_path) + else: + if assets_root_path is None: + carb.log_error("Could not find Isaac Sim assets folder") + + asset_path = assets_root_path + "/Isaac/Robots/ANYbotics/anymal_c.usd" + + carb.log_warn("asset path is: " + asset_path) + prim.GetReferences().AddReference(asset_path) + + super().__init__(prim_path=self._prim_path, name=name, position=position, orientation=orientation) + + self._dof_control_modes: List[int] = list() + + # Policy + file_content = omni.client.read_file(assets_root_path + "/Isaac/Samples/Quadruped/Anymal_Policies/policy_1.pt")[ + 2 + ] + file = io.BytesIO(memoryview(file_content).tobytes()) + + self._policy = torch.jit.load(file) + self._base_vel_lin_scale = 2.0 + self._base_vel_ang_scale = 0.25 + self._joint_pos_scale = 1.0 + self._joint_vel_scale = 0.05 + self._action_scale = 0.5 + self._default_joint_pos = np.array([0.0, 0.4, -0.8, 0.0, -0.4, 0.8, -0.0, 0.4, -0.8, -0.0, -0.4, 0.8]) + self._previous_action = np.zeros(12) + self._policy_counter = 0 + + # Actuator network + file_content = omni.client.read_file( + assets_root_path + "/Isaac/Samples/Quadruped/Anymal_Policies/sea_net_jit2.pt" + )[2] + file = io.BytesIO(memoryview(file_content).tobytes()) + self._actuator_network = LstmSeaNetwork() + self._actuator_network.setup(file, self._default_joint_pos) + self._actuator_network.reset() + + # Height scaner + y = np.arange(-0.5, 0.6, 0.1) + x = np.arange(-0.8, 0.9, 0.1) + grid_x, grid_y = np.meshgrid(x, y) + self._scan_points = np.zeros((grid_x.size, 3)) + self._scan_points[:, 0] = grid_x.transpose().flatten() + self._scan_points[:, 1] = grid_y.transpose().flatten() + self.physx_query_interface = omni.physx.get_physx_scene_query_interface() + self._query_info = [] + + def _hit_report_callback(self, hit): + current_hit_body = hit.rigid_body + if "/World/GroundPlane" in current_hit_body: + self._query_info.append(hit.distance) + return True + + def _compute_observation(self, command): + """[summary] + + compute the observation vector for the policy + + Argument: + command {np.ndarray} -- the robot command (v_x, v_y, w_z) + + Returns: + np.ndarray -- The observation vector. + + """ + lin_vel_I = self.get_linear_velocity() + ang_vel_I = self.get_angular_velocity() + pos_IB, q_IB = self.get_world_pose() + + R_IB = quat_to_rot_matrix(q_IB) + R_BI = R_IB.transpose() + lin_vel_b = np.matmul(R_BI, lin_vel_I) + ang_vel_b = np.matmul(R_BI, ang_vel_I) + gravity_b = np.matmul(R_BI, np.array([0.0, 0.0, -1.0])) + + obs = np.zeros(235) + # Base lin vel + obs[:3] = self._base_vel_lin_scale * lin_vel_b + # Base ang vel + obs[3:6] = self._base_vel_ang_scale * ang_vel_b + # Gravity + obs[6:9] = gravity_b + # Command + obs[9] = self._base_vel_lin_scale * command[0] + obs[10] = self._base_vel_lin_scale * command[1] + obs[11] = self._base_vel_ang_scale * command[2] + # Joint states + # joint_state from the DC interface now has the order of + # 'FL_hip_joint', 'FR_hip_joint', 'RL_hip_joint', 'RR_hip_joint', + # 'FL_thigh_joint', 'FR_thigh_joint', 'RL_thigh_joint', 'RR_thigh_joint', + # 'FL_calf_joint', 'FR_calf_joint', 'RL_calf_joint', 'RR_calf_joint' + + # while the learning controller uses the order of + # FL_hip_joint FL_thigh_joint FL_calf_joint + # FR_hip_joint FR_thigh_joint FR_calf_joint + # RL_hip_joint RL_thigh_joint RL_calf_joint + # RR_hip_joint RR_thigh_joint RR_calf_joint + # Convert DC order to controller order for joint info + current_joint_pos = self.get_joint_positions() + current_joint_vel = self.get_joint_velocities() + current_joint_pos = np.array(current_joint_pos.reshape([3, 4]).T.flat) + current_joint_vel = np.array(current_joint_vel.reshape([3, 4]).T.flat) + obs[12:24] = self._joint_pos_scale * (current_joint_pos - self._default_joint_pos) + obs[24:36] = self._joint_vel_scale * current_joint_vel + + obs[36:48] = self._previous_action + + # height_scanner + rpy = -quat_to_euler_angles(q_IB) + rpy[:2] = 0.0 + yaw_rot = np.transpose(euler_to_rot_matrix(rpy)) + + world_scan_points = np.matmul(yaw_rot, self._scan_points.T).T + pos_IB + + for i in range(world_scan_points.shape[0]): + self._query_info.clear() + self.physx_query_interface.raycast_all( + tuple(world_scan_points[i]), (0.0, 0.0, -1.0), 100, self._hit_report_callback + ) + if self._query_info: + distance = min(self._query_info) + obs[48 + i] = np.clip(distance - 0.5, -1.0, 1.0) + else: + print("No hit") + return obs + + def advance(self, dt, command): + """[summary] + + compute the desired torques and apply them to the articulation + + Argument: + dt {float} -- Timestep update in the world. + command {np.ndarray} -- the robot command (v_x, v_y, w_z) + + """ + if self._policy_counter % 4 == 0: + obs = self._compute_observation(command) + with torch.no_grad(): + obs = torch.from_numpy(obs).view(1, -1).float() + self.action = self._policy(obs).detach().view(-1).numpy() + self._previous_action = self.action.copy() + + # joint_state from the DC interface now has the order of + # 'FL_hip_joint', 'FR_hip_joint', 'RL_hip_joint', 'RR_hip_joint', + # 'FL_thigh_joint', 'FR_thigh_joint', 'RL_thigh_joint', 'RR_thigh_joint', + # 'FL_calf_joint', 'FR_calf_joint', 'RL_calf_joint', 'RR_calf_joint' + + # while the learning controller uses the order of + # FL_hip_joint FL_thigh_joint FL_calf_joint + # FR_hip_joint FR_thigh_joint FR_calf_joint + # RL_hip_joint RL_thigh_joint RL_calf_joint + # RR_hip_joint RR_thigh_joint RR_calf_joint + # Convert DC order to controller order for joint info + current_joint_pos = self.get_joint_positions() + current_joint_vel = self.get_joint_velocities() + current_joint_pos = np.array(current_joint_pos.reshape([3, 4]).T.flat) + current_joint_vel = np.array(current_joint_vel.reshape([3, 4]).T.flat) + joint_torques, _ = self._actuator_network.compute_torques( + current_joint_pos, current_joint_vel, self._action_scale * self.action + ) + + # finally convert controller order to DC order for command torque + torque_reorder = np.array(joint_torques.reshape([4, 3]).T.flat) + self.set_joint_efforts(torque_reorder) + + self._policy_counter += 1 + + def initialize(self, physics_sim_view=None) -> None: + """[summary] + + initialize the articulation interface, set up drive mode + """ + super().initialize(physics_sim_view=physics_sim_view) + self.get_articulation_controller().set_effort_modes("force") + self.get_articulation_controller().switch_control_mode("effort") + + def post_reset(self) -> None: + """[summary] + + post reset articulation + """ + super().post_reset() diff --git a/exts/StrideSim/StrideSim/quadruped.py b/exts/StrideSim/StrideSim/anymal_d.py similarity index 87% rename from exts/StrideSim/StrideSim/quadruped.py rename to exts/StrideSim/StrideSim/anymal_d.py index c77af12..9433d01 100644 --- a/exts/StrideSim/StrideSim/quadruped.py +++ b/exts/StrideSim/StrideSim/anymal_d.py @@ -12,16 +12,16 @@ import carb import omni import omni.appwindow # Contains handle to keyboard -from omni.isaac.quadruped.robots import Unitree +from StrideSim.anymal_articulation import AnymalD_Atriculation -from .base_sample import BaseSample +from StrideSim.base_sample import BaseSample -class QuadrupedExample(BaseSample): +class AnymalD(BaseSample): def __init__(self) -> None: super().__init__() self._world_settings["stage_units_in_meters"] = 1.0 - self._world_settings["physics_dt"] = 1.0 / 400.0 + self._world_settings["physics_dt"] = 1.0 / 800.0 self._world_settings["rendering_dt"] = 5.0 / 400.0 self._enter_toggled = 0 self._base_command = [0.0, 0.0, 0.0, 0] @@ -60,12 +60,11 @@ def setup_scene(self) -> None: dynamic_friction=0.2, restitution=0.01, ) - self._a1 = world.scene.add( - Unitree( - prim_path="/World/A1", - name="A1", - position=np.array([0, 0, 0.400]), - physics_dt=self._world_settings["physics_dt"], + self._anymalD = world.scene.add( + AnymalD_Atriculation( + prim_path="/World/anymal_d", + name="AnymalD", + position=np.array([0, 0, 0.800]), ) ) timeline = omni.timeline.get_timeline_interface() @@ -87,15 +86,13 @@ async def setup_post_load(self) -> None: async def setup_post_reset(self) -> None: self._event_flag = False await self._world.play_async() - self._a1.set_state(self._a1._default_a1_state) - self._a1.post_reset() + self._anymalD.post_reset() return def on_physics_step(self, step_size) -> None: if self._event_flag: - self._a1._qp_controller.switch_mode() self._event_flag = False - self._a1.advance(step_size, self._base_command) + self._anymalD.advance(step_size, self._base_command) def _sub_keyboard_event(self, event, *args, **kwargs) -> bool: """Subscriber callback to when kit is updated.""" @@ -129,8 +126,8 @@ def _sub_keyboard_event(self, event, *args, **kwargs) -> bool: return True def _timeline_timer_callback_fn(self, event) -> None: - if self._a1: - self._a1.post_reset() + if self._anymalD: + self._anymalD.post_reset() return def world_cleanup(self): diff --git a/exts/StrideSim/StrideSim/anymal_d_extension.py b/exts/StrideSim/StrideSim/anymal_d_extension.py new file mode 100644 index 0000000..9e8a0bd --- /dev/null +++ b/exts/StrideSim/StrideSim/anymal_d_extension.py @@ -0,0 +1,35 @@ +import os + +from StrideSim.base_sample import BaseSampleExtension +from StrideSim.anymal_d import AnymalD + + +class AnyamlDExtension(BaseSampleExtension): + def on_startup(self, ext_id: str): + super().on_startup(ext_id) + + overview = "This Example shows quadruped simulation in Isaac Sim. Currently there is a performance issue with " + overview += ( + "the quadruped gait controller; it's being investigated and will be improved in an upcoming release." + ) + overview += "\n\tKeybord Input:" + overview += "\n\t\tup arrow / numpad 8: Move Forward" + overview += "\n\t\tdown arrow/ numpad 2: Move Reverse" + overview += "\n\t\tleft arrow/ numpad 4: Move Left" + overview += "\n\t\tright arrow / numpad 6: Move Right" + overview += "\n\t\tN / numpad 7: Spin Counterclockwise" + overview += "\n\t\tM / numpad 9: Spin Clockwise" + + overview += "\n\nPress the 'Open in IDE' button to view the source code." + + super().start_extension( + menu_name="", + submenu_name="", + name="StrideSim_AnymalD", + title="Auturbo quadruped robot simulation", + doc_link="https://github.com/AuTURBO/StrideSim", + overview=overview, + file_path=os.path.abspath(__file__), + sample=AnymalD(), + ) + return \ No newline at end of file diff --git a/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py b/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py index 246dddf..07590b1 100644 --- a/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py +++ b/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py @@ -14,7 +14,7 @@ import omni.ext import omni.ui as ui from omni.isaac.core import World -from omni.isaac.examples.base_sample import BaseSample +from StrideSim.base_sample import BaseSample from omni.isaac.ui.menu import make_menu_item_description from omni.isaac.ui.ui_utils import btn_builder, get_style, setup_ui_headers # scrolling_frame_builder from omni.kit.menu.utils import MenuItemDescription, add_menu_items, remove_menu_items diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/agents/__init__.py b/exts/StrideSim/StrideSim/network/ppo.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/agents/__init__.py rename to exts/StrideSim/StrideSim/network/ppo.py diff --git a/exts/StrideSim/StrideSim/omnigraph.py b/exts/StrideSim/StrideSim/omnigraph.py new file mode 100644 index 0000000..e69de29 diff --git a/exts/StrideSim/StrideSim/quadruped_extension.py b/exts/StrideSim/StrideSim/quadruped_extension.py deleted file mode 100644 index e789fe7..0000000 --- a/exts/StrideSim/StrideSim/quadruped_extension.py +++ /dev/null @@ -1,85 +0,0 @@ -import os - -from .base_sample import BaseSampleExtension -from .quadruped import QuadrupedExample - - -class StrideSimExtension(BaseSampleExtension): - def on_startup(self, ext_id: str): - super().on_startup(ext_id) - - overview = "This Example shows quadruped simulation in Isaac Sim. Currently there is a performance issue with " - overview += ( - "the quadruped gait controller; it's being investigated and will be improved in an upcoming release." - ) - overview += "\n\tKeybord Input:" - overview += "\n\t\tup arrow / numpad 8: Move Forward" - overview += "\n\t\tdown arrow/ numpad 2: Move Reverse" - overview += "\n\t\tleft arrow/ numpad 4: Move Left" - overview += "\n\t\tright arrow / numpad 6: Move Right" - overview += "\n\t\tN / numpad 7: Spin Counterclockwise" - overview += "\n\t\tM / numpad 9: Spin Clockwise" - - overview += "\n\nPress the 'Open in IDE' button to view the source code." - - super().start_extension( - menu_name="", - submenu_name="", - name="StrideSim", - title="Auturbo quadruped robot simulation", - doc_link="https://github.com/AuTURBO/StrideSim", - overview=overview, - file_path=os.path.abspath(__file__), - sample=QuadrupedExample(), - ) - return - - -# import subprocess - -# import omni.ext - - -# # Functions and vars are available to other extension as usual in python: `example.python_ext.some_public_function(x)` -# def some_public_function(x: int): -# print("[StrideSim] some_public_function was called with x: ", x) -# return x**x - - -# # Any class derived from `omni.ext.IExt` in top level module (defined in `python.modules` of `extension.toml`) will be -# # instantiated when extension gets enabled and `on_startup(ext_id)` will be called. Later when extension gets disabled -# # on_shutdown() is called. -# class ExampleExtension(omni.ext.IExt): -# # ext_id is current extension id. It can be used with extension manager to query additional information, like where -# # this extension is located on filesystem. -# def on_startup(self, ext_id): -# print("[StrideSim] startup") - -# self._window = omni.ui.Window("My Window", width=300, height=300) -# with self._window.frame: -# with omni.ui.VStack(): -# label = omni.ui.Label("") - -# def on_train(): -# label.text = "start training" - -# # Execute the specified Python command -# subprocess.run( -# [ -# "python", -# "scripts/rsl_rl/train.py", -# "--task", -# "Isaac-Velocity-Rough-Anymal-D-v0", -# "--headless", -# ] -# ) - -# def on_play(): -# label.text = "empty" - -# with omni.ui.HStack(): -# omni.ui.Button("Train", clicked_fn=on_train) -# omni.ui.Button("Reset", clicked_fn=on_play) - -# def on_shutdown(self): -# print("[StrideSim] shutdown") diff --git a/exts/StrideSim/StrideSim/tasks/__init__.py b/exts/StrideSim/StrideSim/tasks/__init__.py deleted file mode 100644 index 71fd5f8..0000000 --- a/exts/StrideSim/StrideSim/tasks/__init__.py +++ /dev/null @@ -1,18 +0,0 @@ -"""Package containing task implementations for various robotic environments.""" - -import os -import toml - -# FIXME: 무작정 주석처리해버림, 무슨 문제가 생길지 모름 - -# from omni.isaac.lab_tasks.utils import import_packages - -# ## -# # Register Gym environments. -# ## - - -# # The blacklist is used to prevent importing configs from sub-packages -# _BLACKLIST_PKGS = ["utils"] -# # Import all configs in this package -# import_packages(__name__, _BLACKLIST_PKGS) diff --git a/rl/.gitignore b/rl/.gitignore new file mode 100644 index 0000000..db66f1f --- /dev/null +++ b/rl/.gitignore @@ -0,0 +1,4 @@ +build/ +dist/ + +files.txt \ No newline at end of file diff --git a/rl/StrideSim_RL/tasks/__init__.py b/rl/StrideSim_RL/tasks/__init__.py new file mode 100644 index 0000000..a140a75 --- /dev/null +++ b/rl/StrideSim_RL/tasks/__init__.py @@ -0,0 +1,18 @@ +"""Package containing task implementations for various robotic environments.""" + +import os +import toml + +# FIXME: 무작정 주석처리해버림, 무슨 문제가 생길지 모름 + +from omni.isaac.lab_tasks.utils import import_packages + +## +# Register Gym environments. +## + + +# The blacklist is used to prevent importing configs from sub-packages +_BLACKLIST_PKGS = ["utils"] +# Import all configs in this package +import_packages(__name__, _BLACKLIST_PKGS) diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/__init__.py b/rl/StrideSim_RL/tasks/locomotion/__init__.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/__init__.py rename to rl/StrideSim_RL/tasks/locomotion/__init__.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/__init__.py b/rl/StrideSim_RL/tasks/locomotion/velocity/__init__.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/__init__.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/__init__.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/__init__.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/__init__.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/__init__.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/config/__init__.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/__init__.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/__init__.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/__init__.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/__init__.py diff --git a/rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/agents/__init__.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/agents/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/flat_env_cfg.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/flat_env_cfg.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/flat_env_cfg.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/flat_env_cfg.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/rough_env_cfg.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/rough_env_cfg.py similarity index 93% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/rough_env_cfg.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/rough_env_cfg.py index 3dd4778..59795e2 100644 --- a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/anymal_d/rough_env_cfg.py +++ b/rl/StrideSim_RL/tasks/locomotion/velocity/config/anymal_d/rough_env_cfg.py @@ -1,4 +1,4 @@ -from StrideSim.tasks.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg +from StrideSim_RL.tasks.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg from omni.isaac.lab.utils import configclass diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/mdp/__init__.py b/rl/StrideSim_RL/tasks/locomotion/velocity/mdp/__init__.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/mdp/__init__.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/mdp/__init__.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/mdp/curriculums.py b/rl/StrideSim_RL/tasks/locomotion/velocity/mdp/curriculums.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/mdp/curriculums.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/mdp/curriculums.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/mdp/rewards.py b/rl/StrideSim_RL/tasks/locomotion/velocity/mdp/rewards.py similarity index 100% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/mdp/rewards.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/mdp/rewards.py diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/velocity_env_cfg.py b/rl/StrideSim_RL/tasks/locomotion/velocity/velocity_env_cfg.py similarity index 99% rename from exts/StrideSim/StrideSim/tasks/locomotion/velocity/velocity_env_cfg.py rename to rl/StrideSim_RL/tasks/locomotion/velocity/velocity_env_cfg.py index a590dc2..3d91593 100644 --- a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/velocity_env_cfg.py +++ b/rl/StrideSim_RL/tasks/locomotion/velocity/velocity_env_cfg.py @@ -3,7 +3,7 @@ import math from dataclasses import MISSING -import StrideSim.tasks.locomotion.velocity.mdp as mdp +import StrideSim_RL.tasks.locomotion.velocity.mdp as mdp import omni.isaac.lab.sim as sim_utils from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg diff --git a/scripts/rsl_rl/cli_args.py b/rl/cli_args.py similarity index 100% rename from scripts/rsl_rl/cli_args.py rename to rl/cli_args.py diff --git a/rl/config/extension.toml b/rl/config/extension.toml new file mode 100644 index 0000000..bb9cad1 --- /dev/null +++ b/rl/config/extension.toml @@ -0,0 +1,8 @@ +[package] +version = "0.0.1" + +author = "Jinwon Kim" +maintainer = "Jinwon Kim" +description="Quadruped robot simulation from A to Z" +repository = "https://github.com/AuTURBO/StrideSim" +keywords = ["extension", "quadruped robot", "isaaclab"] \ No newline at end of file diff --git a/scripts/rsl_rl/play.py b/rl/play.py similarity index 99% rename from scripts/rsl_rl/play.py rename to rl/play.py index 1144555..11f5ea2 100644 --- a/scripts/rsl_rl/play.py +++ b/rl/play.py @@ -42,7 +42,7 @@ from rsl_rl.runners import OnPolicyRunner # Import extensions to set up environment tasks -import StrideSim.tasks # noqa: F401 +import StrideSim_RL.tasks # noqa: F401 from omni.isaac.lab.utils.dict import print_dict from omni.isaac.lab_tasks.utils import get_checkpoint_path, parse_env_cfg diff --git a/exts/StrideSim/setup.py b/rl/setup.py similarity index 95% rename from exts/StrideSim/setup.py rename to rl/setup.py index c13a7c7..d4f1093 100644 --- a/exts/StrideSim/setup.py +++ b/rl/setup.py @@ -18,8 +18,8 @@ # Installation operation setup( - name="StrideSim", - packages=["StrideSim"], + name="StrideSim_RL", + packages=["StrideSim_RL"], author=EXTENSION_TOML_DATA["package"]["author"], maintainer=EXTENSION_TOML_DATA["package"]["maintainer"], url=EXTENSION_TOML_DATA["package"]["repository"], diff --git a/scripts/rsl_rl/train.py b/rl/train.py similarity index 99% rename from scripts/rsl_rl/train.py rename to rl/train.py index 3c9b3cb..9cc3551 100644 --- a/scripts/rsl_rl/train.py +++ b/rl/train.py @@ -44,7 +44,7 @@ from rsl_rl.runners import OnPolicyRunner # Import extensions to set up environment tasks -import StrideSim.tasks # noqa: F401 +import StrideSim_RL.tasks # noqa: F401 from omni.isaac.lab.envs import ManagerBasedRLEnvCfg from omni.isaac.lab.utils.dict import print_dict From 796da4561c46dcb3efa774f761e2b4ee743a7f16 Mon Sep 17 00:00:00 2001 From: jinwon kim Date: Mon, 9 Sep 2024 02:12:40 +0900 Subject: [PATCH 06/12] fix: formatting --- README.md | 2 +- exts/StrideSim/StrideSim/__init__.py | 6 ++---- exts/StrideSim/StrideSim/anymal_articulation.py | 7 ++++--- exts/StrideSim/StrideSim/anymal_d.py | 2 +- exts/StrideSim/StrideSim/anymal_d_extension.py | 4 ++-- .../StrideSim/base_sample/base_sample_extension.py | 3 ++- rl/.gitignore | 2 +- rl/StrideSim_RL/tasks/__init__.py | 3 ++- rl/config/extension.toml | 2 +- rl/play.py | 3 +-- rl/train.py | 3 +-- 11 files changed, 18 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index d3023f4..65a726f 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ python -m pip install -e . 2. 강화학습 라이브러리 실행 ```bash -python rl/train.py --task Template-Isaac-Velocity-Rough-Anymal-D-v0 +python rl/train.py --task Template-Isaac-Velocity-Rough-Anymal-D-v0 ``` TODO diff --git a/exts/StrideSim/StrideSim/__init__.py b/exts/StrideSim/StrideSim/__init__.py index 336eb4a..3fb2583 100644 --- a/exts/StrideSim/StrideSim/__init__.py +++ b/exts/StrideSim/StrideSim/__init__.py @@ -2,9 +2,7 @@ Python module serving as a project/extension template. """ -from .base_sample import * - from .anymal_articulation import AnymalD_Atriculation - from .anymal_d import AnymalD -from .anymal_d_extension import AnyamlDExtension \ No newline at end of file +from .anymal_d_extension import AnyamlDExtension +from .base_sample import * diff --git a/exts/StrideSim/StrideSim/anymal_articulation.py b/exts/StrideSim/StrideSim/anymal_articulation.py index de1e42f..9ed1698 100644 --- a/exts/StrideSim/StrideSim/anymal_articulation.py +++ b/exts/StrideSim/StrideSim/anymal_articulation.py @@ -8,20 +8,21 @@ # import io +import numpy as np +import torch from typing import List, Optional import carb -import numpy as np import omni import omni.kit.commands -import torch from omni.isaac.core.articulations import Articulation from omni.isaac.core.utils.prims import define_prim, get_prim_at_path from omni.isaac.core.utils.rotations import euler_to_rot_matrix, quat_to_euler_angles, quat_to_rot_matrix from omni.isaac.core.utils.stage import get_current_stage from omni.isaac.nucleus import get_assets_root_path from omni.isaac.quadruped.utils import LstmSeaNetwork -from pxr import Gf + +# from pxr import Gf class AnymalD_Atriculation(Articulation): diff --git a/exts/StrideSim/StrideSim/anymal_d.py b/exts/StrideSim/StrideSim/anymal_d.py index 9433d01..92852b5 100644 --- a/exts/StrideSim/StrideSim/anymal_d.py +++ b/exts/StrideSim/StrideSim/anymal_d.py @@ -12,8 +12,8 @@ import carb import omni import omni.appwindow # Contains handle to keyboard -from StrideSim.anymal_articulation import AnymalD_Atriculation +from StrideSim.anymal_articulation import AnymalD_Atriculation from StrideSim.base_sample import BaseSample diff --git a/exts/StrideSim/StrideSim/anymal_d_extension.py b/exts/StrideSim/StrideSim/anymal_d_extension.py index 9e8a0bd..51cf886 100644 --- a/exts/StrideSim/StrideSim/anymal_d_extension.py +++ b/exts/StrideSim/StrideSim/anymal_d_extension.py @@ -1,7 +1,7 @@ import os -from StrideSim.base_sample import BaseSampleExtension from StrideSim.anymal_d import AnymalD +from StrideSim.base_sample import BaseSampleExtension class AnyamlDExtension(BaseSampleExtension): @@ -32,4 +32,4 @@ def on_startup(self, ext_id: str): file_path=os.path.abspath(__file__), sample=AnymalD(), ) - return \ No newline at end of file + return diff --git a/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py b/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py index 07590b1..7b90afe 100644 --- a/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py +++ b/exts/StrideSim/StrideSim/base_sample/base_sample_extension.py @@ -14,11 +14,12 @@ import omni.ext import omni.ui as ui from omni.isaac.core import World -from StrideSim.base_sample import BaseSample from omni.isaac.ui.menu import make_menu_item_description from omni.isaac.ui.ui_utils import btn_builder, get_style, setup_ui_headers # scrolling_frame_builder from omni.kit.menu.utils import MenuItemDescription, add_menu_items, remove_menu_items +from StrideSim.base_sample import BaseSample + class BaseSampleExtension(omni.ext.IExt): def on_startup(self, ext_id: str): diff --git a/rl/.gitignore b/rl/.gitignore index db66f1f..28e4d87 100644 --- a/rl/.gitignore +++ b/rl/.gitignore @@ -1,4 +1,4 @@ build/ dist/ -files.txt \ No newline at end of file +files.txt diff --git a/rl/StrideSim_RL/tasks/__init__.py b/rl/StrideSim_RL/tasks/__init__.py index a140a75..94d74aa 100644 --- a/rl/StrideSim_RL/tasks/__init__.py +++ b/rl/StrideSim_RL/tasks/__init__.py @@ -3,9 +3,10 @@ import os import toml +from omni.isaac.lab_tasks.utils import import_packages + # FIXME: 무작정 주석처리해버림, 무슨 문제가 생길지 모름 -from omni.isaac.lab_tasks.utils import import_packages ## # Register Gym environments. diff --git a/rl/config/extension.toml b/rl/config/extension.toml index bb9cad1..5777dc6 100644 --- a/rl/config/extension.toml +++ b/rl/config/extension.toml @@ -5,4 +5,4 @@ author = "Jinwon Kim" maintainer = "Jinwon Kim" description="Quadruped robot simulation from A to Z" repository = "https://github.com/AuTURBO/StrideSim" -keywords = ["extension", "quadruped robot", "isaaclab"] \ No newline at end of file +keywords = ["extension", "quadruped robot", "isaaclab"] diff --git a/rl/play.py b/rl/play.py index 11f5ea2..9bb2440 100644 --- a/rl/play.py +++ b/rl/play.py @@ -39,10 +39,9 @@ import os import torch -from rsl_rl.runners import OnPolicyRunner - # Import extensions to set up environment tasks import StrideSim_RL.tasks # noqa: F401 +from rsl_rl.runners import OnPolicyRunner from omni.isaac.lab.utils.dict import print_dict from omni.isaac.lab_tasks.utils import get_checkpoint_path, parse_env_cfg diff --git a/rl/train.py b/rl/train.py index 9cc3551..f308f13 100644 --- a/rl/train.py +++ b/rl/train.py @@ -41,10 +41,9 @@ import torch from datetime import datetime -from rsl_rl.runners import OnPolicyRunner - # Import extensions to set up environment tasks import StrideSim_RL.tasks # noqa: F401 +from rsl_rl.runners import OnPolicyRunner from omni.isaac.lab.envs import ManagerBasedRLEnvCfg from omni.isaac.lab.utils.dict import print_dict From bef75e72048512093b37043125596979246aff62 Mon Sep 17 00:00:00 2001 From: jehee Date: Thu, 3 Oct 2024 06:17:14 +0900 Subject: [PATCH 07/12] fix typo --- scripts/rsl_rl/play.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/rsl_rl/play.py b/scripts/rsl_rl/play.py index 6e5998f..110ab7f 100644 --- a/scripts/rsl_rl/play.py +++ b/scripts/rsl_rl/play.py @@ -65,8 +65,8 @@ def main(): log_dir = os.path.dirname(resume_path) # create isaac environment -env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) -# wrap for video recording + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + # wrap for video recording if args_cli.video: video_kwargs = { "video_folder": os.path.join(log_dir, "videos", "play"), From 0c0445e6687a59cff73851f67a5977f10a978223 Mon Sep 17 00:00:00 2001 From: jehee Date: Thu, 3 Oct 2024 06:17:38 +0900 Subject: [PATCH 08/12] Add Train guide --- README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/README.md b/README.md index 74dcd90..129bc2f 100644 --- a/README.md +++ b/README.md @@ -80,3 +80,17 @@ Then you can run pre-commit with: ```bash pre-commit run --all-files ``` + +## How to Train and Play trained policy +1. Train: + ```python StrideSim/scripts/rsl_rl/train.py --task= --headless``` + - To run rendering remove `--headless`. + - The trained policy is saved in `script/rsl_rl/logs/rsl_rl//_/model_.pt`. Where `` and `` are defined in the train config. + - The tasks defined in + `StrideSim/StrideSim/tasks/locomotion/velocity/config//__init__.py` + - Anymal(train):`Isaac-Velocity-Rough-Anymal-D-v0` + - Anymal(play):`Isaac-Velocity-Rough-Anymal-D-Play-v0` +2. Play a trained policy: +```python StrideSim/scripts/rsl_rl/play.py --task=Isaac-Velocity-Rough-Anymal-D-Play-v0``` + - By default, the loaded policy is the last model of the last run of the experiment folder. + - Other runs/model iteration can be selected by setting `load_run` and `checkpoint` in the train config. \ No newline at end of file From a3fbf55b8ca49fda86a1425a7e288405e73da70a Mon Sep 17 00:00:00 2001 From: jehee Date: Thu, 3 Oct 2024 07:19:51 +0900 Subject: [PATCH 09/12] Add GO2 --- .gitignore | 1 + .../velocity/config/go2/__init__.py | 52 ++++++++++++ .../velocity/config/go2/agents/__init__.py | 4 + .../config/go2/agents/rsl_rl_ppo_cfg.py | 52 ++++++++++++ .../velocity/config/go2/flat_env_cfg.py | 43 ++++++++++ .../velocity/config/go2/rough_env_cfg.py | 84 +++++++++++++++++++ 6 files changed, 236 insertions(+) create mode 100644 exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/__init__.py create mode 100644 exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/__init__.py create mode 100644 exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py create mode 100644 exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/flat_env_cfg.py create mode 100644 exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/rough_env_cfg.py diff --git a/.gitignore b/.gitignore index 8afac9a..43b0ae2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ **/__pycache__/ *.egg-info logs/ +wandb/ \ No newline at end of file diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/__init__.py b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/__init__.py new file mode 100644 index 0000000..1aaa63e --- /dev/null +++ b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/__init__.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents, flat_env_cfg, rough_env_cfg + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go2-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": flat_env_cfg.UnitreeGo2FlatEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2FlatPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go2-Play-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": flat_env_cfg.UnitreeGo2FlatEnvCfg_PLAY, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2FlatPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go2-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": rough_env_cfg.UnitreeGo2RoughEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2RoughPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go2-Play-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": rough_env_cfg.UnitreeGo2RoughEnvCfg_PLAY, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2RoughPPORunnerCfg", + }, +) diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/__init__.py b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/__init__.py new file mode 100644 index 0000000..c3ee657 --- /dev/null +++ b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 0000000..e211e0d --- /dev/null +++ b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.utils import configclass + +from omni.isaac.lab_tasks.utils.wrappers.rsl_rl import ( + RslRlOnPolicyRunnerCfg, + RslRlPpoActorCriticCfg, + RslRlPpoAlgorithmCfg, +) + + +@configclass +class UnitreeGo2RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "unitree_go2_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class UnitreeGo2FlatPPORunnerCfg(UnitreeGo2RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "unitree_go2_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/flat_env_cfg.py b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/flat_env_cfg.py new file mode 100644 index 0000000..1c3f41d --- /dev/null +++ b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/flat_env_cfg.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.utils import configclass + +from .rough_env_cfg import UnitreeGo2RoughEnvCfg + + +@configclass +class UnitreeGo2FlatEnvCfg(UnitreeGo2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -2.5 + self.rewards.feet_air_time.weight = 0.25 + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class UnitreeGo2FlatEnvCfg_PLAY(UnitreeGo2FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/rough_env_cfg.py b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/rough_env_cfg.py new file mode 100644 index 0000000..21f2683 --- /dev/null +++ b/exts/StrideSim/StrideSim/tasks/locomotion/velocity/config/go2/rough_env_cfg.py @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.utils import configclass + +from omni.isaac.lab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from omni.isaac.lab_assets.unitree import UNITREE_GO2_CFG # isort: skip + + +@configclass +class UnitreeGo2RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.robot = UNITREE_GO2_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base" + # scale down the terrains because the robot is small + self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_step = 0.01 + + # reduce action scale + self.actions.joint_pos.scale = 0.25 + + # event + self.events.push_robot = None + self.events.add_base_mass.params["mass_distribution_params"] = (-1.0, 3.0) + self.events.add_base_mass.params["asset_cfg"].body_names = "base" + self.events.base_external_force_torque.params["asset_cfg"].body_names = "base" + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # rewards + self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" + self.rewards.feet_air_time.weight = 0.01 + self.rewards.undesired_contacts = None + self.rewards.dof_torques_l2.weight = -0.0002 + self.rewards.track_lin_vel_xy_exp.weight = 1.5 + self.rewards.track_ang_vel_z_exp.weight = 0.75 + self.rewards.dof_acc_l2.weight = -2.5e-7 + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = "base" + + +@configclass +class UnitreeGo2RoughEnvCfg_PLAY(UnitreeGo2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None From 53dd86d9b5a7236be957b55df7de0e96dc4e781d Mon Sep 17 00:00:00 2001 From: jehee Date: Thu, 3 Oct 2024 23:08:11 +0900 Subject: [PATCH 10/12] Add go2 --- exts/StrideSim/setup.py | 40 +++++++++ .../velocity/config/go2/__init__.py | 52 ++++++++++++ .../velocity/config/go2/agents/__init__.py | 4 + .../config/go2/agents/rsl_rl_ppo_cfg.py | 52 ++++++++++++ .../velocity/config/go2/flat_env_cfg.py | 43 ++++++++++ .../velocity/config/go2/rough_env_cfg.py | 84 +++++++++++++++++++ 6 files changed, 275 insertions(+) create mode 100644 exts/StrideSim/setup.py create mode 100644 rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/__init__.py create mode 100644 rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/__init__.py create mode 100644 rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py create mode 100644 rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/flat_env_cfg.py create mode 100644 rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/rough_env_cfg.py diff --git a/exts/StrideSim/setup.py b/exts/StrideSim/setup.py new file mode 100644 index 0000000..c13a7c7 --- /dev/null +++ b/exts/StrideSim/setup.py @@ -0,0 +1,40 @@ +"""Installation script for the 'StrideSim' python package.""" + +import os +import toml + +from setuptools import setup + +# Obtain the extension data from the extension.toml file +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) +# Read the extension.toml file +EXTENSION_TOML_DATA = toml.load(os.path.join(EXTENSION_PATH, "config", "extension.toml")) + +# Minimum dependencies required prior to installation +INSTALL_REQUIRES = [ + # NOTE: Add dependencies + "psutil", +] + +# Installation operation +setup( + name="StrideSim", + packages=["StrideSim"], + author=EXTENSION_TOML_DATA["package"]["author"], + maintainer=EXTENSION_TOML_DATA["package"]["maintainer"], + url=EXTENSION_TOML_DATA["package"]["repository"], + version=EXTENSION_TOML_DATA["package"]["version"], + description=EXTENSION_TOML_DATA["package"]["description"], + keywords=EXTENSION_TOML_DATA["package"]["keywords"], + install_requires=INSTALL_REQUIRES, + license="MIT", + include_package_data=True, + python_requires=">=3.10", + classifiers=[ + "Natural Language :: English", + "Programming Language :: Python :: 3.10", + "Isaac Sim :: 2023.1.1", + "Isaac Sim :: 4.0.0", + ], + zip_safe=False, +) diff --git a/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/__init__.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/__init__.py new file mode 100644 index 0000000..1aaa63e --- /dev/null +++ b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/__init__.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents, flat_env_cfg, rough_env_cfg + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go2-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": flat_env_cfg.UnitreeGo2FlatEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2FlatPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go2-Play-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": flat_env_cfg.UnitreeGo2FlatEnvCfg_PLAY, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2FlatPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go2-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": rough_env_cfg.UnitreeGo2RoughEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2RoughPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go2-Play-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": rough_env_cfg.UnitreeGo2RoughEnvCfg_PLAY, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2RoughPPORunnerCfg", + }, +) diff --git a/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/__init__.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/__init__.py new file mode 100644 index 0000000..c3ee657 --- /dev/null +++ b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 0000000..e211e0d --- /dev/null +++ b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.utils import configclass + +from omni.isaac.lab_tasks.utils.wrappers.rsl_rl import ( + RslRlOnPolicyRunnerCfg, + RslRlPpoActorCriticCfg, + RslRlPpoAlgorithmCfg, +) + + +@configclass +class UnitreeGo2RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "unitree_go2_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class UnitreeGo2FlatPPORunnerCfg(UnitreeGo2RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "unitree_go2_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/flat_env_cfg.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/flat_env_cfg.py new file mode 100644 index 0000000..1c3f41d --- /dev/null +++ b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/flat_env_cfg.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.utils import configclass + +from .rough_env_cfg import UnitreeGo2RoughEnvCfg + + +@configclass +class UnitreeGo2FlatEnvCfg(UnitreeGo2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -2.5 + self.rewards.feet_air_time.weight = 0.25 + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class UnitreeGo2FlatEnvCfg_PLAY(UnitreeGo2FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/rough_env_cfg.py b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/rough_env_cfg.py new file mode 100644 index 0000000..21f2683 --- /dev/null +++ b/rl/StrideSim_RL/tasks/locomotion/velocity/config/go2/rough_env_cfg.py @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.utils import configclass + +from omni.isaac.lab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from omni.isaac.lab_assets.unitree import UNITREE_GO2_CFG # isort: skip + + +@configclass +class UnitreeGo2RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.robot = UNITREE_GO2_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base" + # scale down the terrains because the robot is small + self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_step = 0.01 + + # reduce action scale + self.actions.joint_pos.scale = 0.25 + + # event + self.events.push_robot = None + self.events.add_base_mass.params["mass_distribution_params"] = (-1.0, 3.0) + self.events.add_base_mass.params["asset_cfg"].body_names = "base" + self.events.base_external_force_torque.params["asset_cfg"].body_names = "base" + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # rewards + self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" + self.rewards.feet_air_time.weight = 0.01 + self.rewards.undesired_contacts = None + self.rewards.dof_torques_l2.weight = -0.0002 + self.rewards.track_lin_vel_xy_exp.weight = 1.5 + self.rewards.track_ang_vel_z_exp.weight = 0.75 + self.rewards.dof_acc_l2.weight = -2.5e-7 + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = "base" + + +@configclass +class UnitreeGo2RoughEnvCfg_PLAY(UnitreeGo2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None From 06a646ed2f17afb29740528a4f359a94e5002bb9 Mon Sep 17 00:00:00 2001 From: jehee Date: Thu, 3 Oct 2024 23:09:35 +0900 Subject: [PATCH 11/12] update train/play guide --- README.md | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index de71b7b..72d77d0 100644 --- a/README.md +++ b/README.md @@ -32,10 +32,22 @@ python -m pip install -e . ``` 2. 강화학습 라이브러리 실행 - +Train ```bash -python rl/train.py --task Template-Isaac-Velocity-Rough-Anymal-D-v0 +python train.py --task= --headless ``` +tasks list +- Anymal-D(Rough): `Isaac-Velocity-Rough-Anymal-D-v0` +- Anymal-D(Flat): `Isaac-Velocity-Rough-Anymal-D-v0` +- Go2(Rough): `Isaac-Velocity-Rough-Unitree-Go2-v0` +- Go2(Flat): `Isaac-Velocity-Flat-Unitree-Go2-v0` + +Play +`python play.py --task=` +- Anymal-D(Rough): `Isaac-Velocity-Rough-Anymal-D-Play-v0` +- Anymal-D(Flat): `Isaac-Velocity-Rough-Anymal-D-Play-v0` +- Go2(Rough): `Isaac-Velocity-Rough-Unitree-Go2-Play-v0` +- Go2(Flat): `Isaac-Velocity-Flat-Unitree-Go2-Play-v0` TODO From ad9c531ea09fa39ec6584dacc2fd245b632f104a Mon Sep 17 00:00:00 2001 From: jehee Date: Thu, 3 Oct 2024 23:16:21 +0900 Subject: [PATCH 12/12] update RL tuto --- README.md | 32 ++++++++++---------------------- 1 file changed, 10 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index 72d77d0..5c30739 100644 --- a/README.md +++ b/README.md @@ -31,25 +31,29 @@ cd rl python -m pip install -e . ``` -2. 강화학습 라이브러리 실행 -Train +2. 강화학습 라이브러리 실행 - Train ```bash +cd StrideSim/rl python train.py --task= --headless ``` -tasks list + +Train Tasks list - Anymal-D(Rough): `Isaac-Velocity-Rough-Anymal-D-v0` - Anymal-D(Flat): `Isaac-Velocity-Rough-Anymal-D-v0` - Go2(Rough): `Isaac-Velocity-Rough-Unitree-Go2-v0` - Go2(Flat): `Isaac-Velocity-Flat-Unitree-Go2-v0` -Play -`python play.py --task=` +3. 강화학습 라이브러리 실행 - Play +```bash +cd StrideSim/rl +python play.py --task= +``` +Play Tasks list - Anymal-D(Rough): `Isaac-Velocity-Rough-Anymal-D-Play-v0` - Anymal-D(Flat): `Isaac-Velocity-Rough-Anymal-D-Play-v0` - Go2(Rough): `Isaac-Velocity-Rough-Unitree-Go2-Play-v0` - Go2(Flat): `Isaac-Velocity-Flat-Unitree-Go2-Play-v0` -TODO ## 코드 포맷팅 @@ -67,22 +71,6 @@ pre-commit 실행: pre-commit run --all-files ``` -<<<<<<< HEAD -## How to Train and Play trained policy -1. Train: - ```python StrideSim/scripts/rsl_rl/train.py --task= --headless``` - - To run rendering remove `--headless`. - - The trained policy is saved in `script/rsl_rl/logs/rsl_rl//_/model_.pt`. Where `` and `` are defined in the train config. - - The tasks defined in - `StrideSim/StrideSim/tasks/locomotion/velocity/config//__init__.py` - - Anymal(train):`Isaac-Velocity-Rough-Anymal-D-v0` - - Anymal(play):`Isaac-Velocity-Rough-Anymal-D-Play-v0` -2. Play a trained policy: -```python StrideSim/scripts/rsl_rl/play.py --task=Isaac-Velocity-Rough-Anymal-D-Play-v0``` - - By default, the loaded policy is the last model of the last run of the experiment folder. - - Other runs/model iteration can be selected by setting `load_run` and `checkpoint` in the train config. -======= ## 라이선스 이 프로젝트는 MIT 라이선스 하에 배포됩니다. ->>>>>>> upstream/devel